We present a novel data structure, the Bayes tree, that provides an algorithmic foundation enabling a better understanding of existing graphical model inference algorithms and their connection to sparse matrix factorization methods. Similar to a clique tree, a Bayes tree encodes a factored probability density, but unlike the clique tree it is directed and maps more naturally to the square root information matrix of the simultaneous localization and mapping (SLAM) problem. In this paper, we highlight three insights provided by our new data structure. First, the Bayes tree provides a better understanding of the matrix factorization in terms of probability densities. Second, we show how the fairly abstract updates to a matrix factorization translate to a simple editing of the Bayes tree and its conditional densities. Third, we apply the Bayes tree to obtain a completely novel algorithm for sparse nonlinear incremental optimization, named iSAM2, which achieves improvements in efficiency through incremental variable re-ordering and fluid relinearization, eliminating the need for periodic batch steps. We analyze various properties of iSAM2 in detail, and show on a range of real and simulated datasets that our algorithm compares favorably with other recent mapping algorithms in both quality and efficiency.

/pdf/isam2-incremental-smoothing-and-mapping-using-the-bayes-tree-3ixkuyhnhk.pdf

iSAM2: Incremental smoothing and mapping using the Bayes tree

Incremental smoothing and mapping (iSAM) is presented, a novel approach to the simultaneous localization and mapping (SLAM) problem. SLAM is the problem of estimating an observer's position from local measurements only, while creating a consistent map of the environment. The problem is difficult because even very small errors in the local measurements accumulate over time and lead to large global errors. iSAM provides an exact and efficient solution to the SLAM estimation problem while also addressing data association. For the estimation problem, iSAM provides an exact solution by performing smoothing, which keeps all previous poses as part of the estimation problem, and therefore avoids linearization errors. iSAM uses methods from sparse linear algebra to provide an efficient incremental solution. In particular, iSAM deploys a direct equation solver based on QR matrix factorization of the naturally sparse smoothing information matrix. Instead of refactoring the matrix whenever new measurements arrive, only the entries of the factor matrix that actually change are calculated. iSAM is efficient even for robot trajectories with many loops as it performs periodic variable reordering to avoid unnecessary fill-in in the factor matrix. For the data association problem, I present state of the art data association techniques in the context of iSAM and present an efficient algorithm to obtain the necessary estimation uncertainties in real-time based on the factored information matrix. I systematically evaluate the components of iSAM as well as the overall algorithm using various simulated and real-world datasets.

/pdf/incremental-smoothing-and-mapping-44qx05icwy.pdf

Incremental smoothing and mapping

In this paper we address the problem of drift-free navigation for underwater vehicles performing harbor surveillance and ship hull inspection. Maintaining accurate localization for the duration of a mission is important for a variety of tasks, such as planning the vehicle trajectory and ensuring coverage of the area to be inspected. Our approach only uses onboard sensors in a simultaneous localization and mapping setting and removes the need for any external infrastructure like acoustic beacons. We extract dense features from a forward-looking imaging sonar and apply pair-wise registration between sonar frames. The registrations are combined with onboard velocity, attitude and acceleration sensors to obtain an improved estimate of the vehicle trajectory. We show results from several experiments that demonstrate drift-free navigation in various underwater environments.

/pdf/imaging-sonar-aided-navigation-for-autonomous-underwater-10et2ph13q.pdf

Imaging sonar-aided navigation for autonomous underwater harbor surveillance

This paper describes a system for reacquiring features of interest in a shallow-water ocean environment, using autonomous underwater vehicles (AUVs) equipped with low-cost sonar and navigation sensors. In performing mine countermeasures, it is critical to enable AUVs to navigate accurately to previously mapped objects of interest in the water column or on the seabed, for further assessment or remediation. An important aspect of the overall system design is to keep the size and cost of the reacquisition vehicle as low as possible, as it may potentially be destroyed in the reacquisition mission. This low-cost requirement prevents the use of sophisticated AUV navigation sensors, such as a Doppler velocity log (DVL) or an inertial navigation system (INS). Our system instead uses the Proviewer 900-kHz imaging sonar from Blueview Technologies, which produces forward-looking sonar (FLS) images at ranges up to 40 mat approximately 4 Hz. In large volumes, it is hoped that this sensor can be manufactured at low cost. Our approach uses a novel simultaneous localization and mapping (SLAM) algorithm that detects and tracks features in the FLS images to renavigate to a previously mapped target. This feature-based navigation (FBN) system incorporates a number of recent advances in pose graph optimization algorithms for SLAM. The system has undergone extensive field testing over a period of more than four years, demonstrating the potential for the use of this new approach for feature reacquisition. In this report, we review the methodologies and components of the FBN system, describe the system's technological features, review the performance of the system in a series of extensive in-water field tests, and highlight issues for future research.

/pdf/relocating-underwater-features-autonomously-using-sonar-1zxa3lzhoq.pdf

Relocating Underwater Features Autonomously Using Sonar-Based SLAM

This paper presents a novel tightly-coupled keyframe-based Simultaneous Localization and Mapping (SLAM) system with loop-closing and relocalization capabilities targeted for the underwater domain.Our previous work, SVIn, augmented the state-of-the-art visual-inertial state estimation package OKVIS to accommodate acoustic data from sonar in a non-linear optimization-based framework. This paper addresses drift and loss of localization – one of the main problems affecting other packages in underwater domain – by providing the following main contributions: a robust initialization method to refine scale using depth measurements, a fast preprocessing step to enhance the image quality, and a real-time loop-closing and relocalization method using bag of words (BoW). An additional contribution is the addition of depth measurements from a pressure sensor to the tightly-coupled optimization formulation. Experimental results on datasets collected with a custom-made underwater sensor suite and an autonomous underwater vehicle from challenging underwater environments with poor visibility demonstrate performance never achieved before in terms of accuracy and robustness.

SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor

Tracking sonar features in real time on an underwater robot is a challenging task. One reason is the low observability of the sonar in some directions. For example, using a blazed array sonar one observes range and the angle to the array axis with fair precision. The angle around the axis is poorly constrained. This situation is problematic for tracking features in world frame Cartesian coordinates as the error surfaces will not be ellipsoids. Thus Gaussian tracking of the features will not work properly. The situation is similar to the problem of tracking features in camera images. There the unconstrained direction is depth and its errors are highly non-Gaussian. We propose a solution to the sonar problem that is analogous to the successful inverse depth feature parameterization for vision tracking, introduced by [1]. We parameterize the features by the robot pose where it was first seen and the range/bearing from that pose. Thus the 3D features have 9 parameters that specify their world coordinates. We use a nonlinear transformation on the poorly observed bearing angle to give a more accurate Gaussian approximation to the uncertainty. These features are tracked in a SLAM framework until there is enough information to initialize world frame Cartesian coordinates for them. The more compact representation can then be used for a global SLAM or localization purposes. We present results for a system running real time underwater SLAM/localization. These results show that the parameterization leads to greater consistency in the feature location estimates.

/pdf/feature-tracking-for-underwater-navigation-using-sonar-ao0npi6ucf.pdf

Feature tracking for underwater navigation using sonar

We present a system for autonomous underwater navigation as implemented on a Nekton Ranger autonomous underwater vehicle, AUV. This is one of the first implementations of a practical application for simultaneous localization and mapping on an AUV. Besides being an application of real-time SLAM, the implemtation demonstrates a novel data fusion solution where data from 7 sources are fused at different time scales in 5 separate estimators. By modularizing the data fusion problem in this way each estimator can be tuned separately to provide output useful to the end goal of localizing the AUV, on an a priori map. The Ranger AUV is equipped with a BlueView blazed array sonar which is used to detect features in the underwater environment. Underwater testing results are presented. The features in these tests are deployed radar reflectors.

/pdf/a-feature-based-navigation-system-for-an-autonomous-44bfi0cjys.pdf

A Feature Based Navigation System for an Autonomous Underwater Robot

The paper describes an ongoing effort to enable autonomous mobile robots to play soccer in unstructured, everyday environments Unlike conventional robot soccer competitions that are usually held on purpose-built robot soccer "fields", in our work we seek to develop the capability for robots to demonstrate aspects of soccer-playing in more diverse environments, such as schools, hospitals, or shopping malls, with static obstacles (furniture) and dynamic natural obstacles (people) This problem of "Soccer Anywhere" presents numerous research challenges including: (1) Simultaneous Localization and Mapping (SLAM) in dynamic, unstructured environments, (2) software control architectures for decentralized, distributed control of mobile agents, (3) integration of vision-based object tracking with dynamic control, and (4) social interaction with human participants In addition to the intrinsic research merit of these topics, we believe that this capability would prove useful for outreach activities, in demonstrating robotics technology to primary and secondary school students, to motivate them to pursue careers in science and engineering

/pdf/robot-soccer-anywhere-achieving-persistent-autonomous-4l4dnintja.pdf

Jacques Leederkerken

Papers

Feature tracking for underwater navigation using sonar

A Feature Based Navigation System for an Autonomous Underwater Robot

Robot soccer anywhere: achieving persistent autonomous navigation, mapping, and object vision tracking in dynamic environments