This work reports a novel multi-frame Bundle Adjustment (BA) framework called RKHS-BA. It uses continuous landmark representations that encode RGB-D/LiDAR and semantic observations in a reproducing kernel hilbert space (RKHS). With a correspondence-free pose graph formulation, the proposed system constructs a loss function that achieves more generalized convergence than classical point-wise convergence. We demonstrate its applications in multi-view point cloud registration, sliding-window odometry, and global LiDAR mapping on simulated and real data. It shows highly robust pose estimations in extremely noisy scenes and exhibits strong generalization with various types of semantic inputs.

In this article, we present a novel and flexible multitask multilayer Bayesian mapping framework with readily extendable attribute layers. The proposed framework goes beyond modern metric-semantic maps to provide even richer environmental information for robots in a single mapping formalism while exploiting intralayer and interlayer correlations. It removes the need for a robot to access and process information from many separate maps when performing a complex task, advancing the way robots interact with their environments. To this end, we design a multitask deep neural network with attention mechanisms as our front-end to provide heterogeneous observations for multiple map layers simultaneously. Our back-end runs a scalable closed-form Bayesian inference with only logarithmic time complexity. We apply the framework to build a dense robotic map, including metric-semantic occupancy and traversability layers. Traversability ground truth labels are automatically generated from exteroceptive sensory data in a self-supervised manner. We present extensive experimental results on publicly available datasets and data collected by a three-dimensional bipedal robot platform and show reliable mapping performance in different environments. Finally, we also discuss how the current framework can be extended to incorporate more information, such as friction, signal strength, temperature, and physical quantity concentration using Gaussian map layers. The software for reproducing the presented results or running on customized data is made publicly available.

This paper reports on a novel nonparametric rigid point cloud registration framework, Semantic Continuous Visual Odometry (CVO), that jointly integrates geometric and semantic measurements such as color or semantic labels into the alignment process and does not require explicit data association. The point clouds are represented as nonparametric functions in a reproducible kernel Hilbert space. The alignment problem is formulated as maximizing the inner product between two functions, essentially a sum of weighted kernels, each of which exploits the local geometric and semantic features. As a result of the continuous models, analytical gradients can be computed, and a local solution can be obtained by optimization over the rigid body transformation group. Besides, we present a new point cloud alignment metric that is intrinsic to the proposed framework and takes into account geometric and semantic information. The evaluations using publicly available stereo and RGB-D datasets show that the proposed method outperforms state-of-the-art outdoor and indoor frame-to-frame registration methods. An open-source GPU implementation is also provided.

Determining the rigid-body transformation be-tween 2D image data and 3D point cloud data has applications for mobile robotics including sensor calibration and localizing into a prior map. Common approaches to 2D-3D registration use least-squares solvers assuming known associations often provided by heuristic front-ends, or iterative nearest-neighbor. We present a linear line-based 2D-3D registration algorithm formulated as a mixed-integer program to simultaneously solve for the correct transformation and data association. Our formulation is explicitly formulated to handle outliers, by modeling associations as integer variables. Additionally, we can constrain the registration to SE(2) to improve runtime and accuracy. We evaluate this search over multiple real-world data sets demonstrating adaptability to scene variation.

This paper reports a new continuous 3D loss function for learning depth from monocular images. The dense depth prediction from a monocular image is supervised using sparse LIDAR points, which enables us to leverage available open source datasets with camera-LIDAR sensor suites during training. Currently, accurate and affordable range sensor is not readily available. Stereo cameras and LIDARs measure depth either inaccurately or sparsely/costly. In contrast to the current point-to-point loss evaluation approach, the proposed 3D loss treats point clouds as continuous objects; therefore, it compensates for the lack of dense ground truth depth due to LIDAR's sparsity measurements. We applied the proposed loss in three state-of-the-art monocular depth prediction approaches DORN, BTS, and Monodepth2. Experimental evaluation shows that the proposed loss improves the depth prediction accuracy and produces point-clouds with more consistent 3D geometric structures compared with all tested baselines, implying the benefit of the proposed loss on general depth prediction networks. A video demo of this work is available at https://youtu.be/5HL8BjSAY4Y.

An accurate characterization of pose uncertainty is essential for safe autonomous navigation. Early pose uncertainty characterization methods proposed by Smith, Self, and Cheeseman (SCC) used coordinate-based first-order methods to propagate uncertainty through nonlinear functions such as pose composition (head-to-tail), pose inversion, and relative pose extraction (tail-to-tail). Characterizing uncertainty in the Lie algebra of the special Euclidean group results in better uncertainty estimates. However, existing Lie-group-based uncertainty propagation techniques assume that individual poses are independent. After solving a pose graph, however, the entire trajectory is jointly distributed as factors induce correlation. Hence, the independence assumption does not capture reality. In addition, prior work has focused primarily on the pose composition operation. This article develops a framework for modeling the uncertainty of jointly distributed poses and describes how to perform the equivalent of the SSC pose operations while characterizing uncertainty in the Lie algebra. Evaluation on simulated and open-source datasets shows that the proposed methods result in more accurate uncertainty estimates and thus more accurate filtering of potential loop closures. An accompanying C++ library implementation is also released.

Autonomous navigation requires an accurate model or map of the environment. While dramatic progress in the prior two decades has enabled large-scale simultaneous localization and mapping (SLAM), the majority of existing methods rely on non-linear optimization techniques to find the maximum likelihood estimate (MLE) of the robot trajectory and surrounding environment. These methods are prone to local minima and are thus sensitive to initialization. Several recent papers have developed optimization algorithms for the Pose-Graph SLAM problem that can certify the optimality of a computed solution. Though this does not guarantee a priori that this approach generates an optimal solution, a recent extension has shown that when the noise lies within a critical threshold that the solution to the optimization algorithm is guaranteed to be optimal. To address the limitations of existing approaches, this paper illustrates that the Pose-Graph SLAM and Landmark SLAM can be formulated as polynomial optimization programs that are sum-of-squares (SOS) convex. This paper then describes how the Pose-Graph and Landmark SLAM problems can be solved to a global minimum without initialization regardless of noise level using the sparse bounded degree sum-of-squares (Sparse-BSOS) optimization method. Finally, the superior performance of the proposed approach when compared to existing SLAM methods is illustrated on graphs with several hundred nodes.

WiFi technology has been used pervasively in fine-grained indoor localization, gesture recognition, and adaptive communication. Achieving better performance in these tasks generally boils down to differentiating Line-Of-Sight (LOS) from Non-Line-Of-Sight (NLOS) signal propagation reliably which generally requires expensive/specialized hardware due to the complex nature of indoor environments. Hence, the development of low-cost accurate positioning systems that exploit available infrastructure is not entirely solved. In this paper, we develop a framework for indoor localization and tracking of ubiquitous mobile devices such as smartphones using on-board sensors. We present a novel deep LOS/NLOS classifier which uses the Received Signal Strength Indicator (RSSI), and can classify the input signal with an accuracy of 85%. The proposed algorithm can globally localize and track a smartphone (or robot) with a priori unknown location, and with a semi-accurate prior map (error within 0.8m) of the WiFi Access Points (AP). Through simultaneously solving for the trajectory and the map of access points, we recover a trajectory of the device and corrected locations for the access points. Experimental evaluations of the framework show that localization accuracy is increased by using the trained deep network; furthermore, the system becomes robust to any error in the map of APs.

State-of-the-art robotic perception systems have achieved sufficiently good performance using Inertial Measurement Units (IMUs), cameras, and nonlinear optimization techniques, that they are now being deployed as technologies. However, many of these methods rely significantly on vision and often fail when visual tracking is lost due to lighting or scarcity of features. This paper presents a state-estimation technique for legged robots that takes into account the robot's kinematic model as well as its contact with the environment. We introduce forward kinematic factors and preintegrated contact factors into a factor graph framework that can be incrementally solved in real-time. The forward kinematic factor relates the robot's base pose to a contact frame through noisy encoder measurements. The preintegrated contact factor provides odometry measurements of this contact frame while accounting for possible foot slippage. Together, the two developed factors constrain the graph optimization problem allowing the robot's trajectory to be estimated. The paper evaluates the method using simulated and real sensory IMU and kinematic data from experiments with a Cassie-series robot designed by Agility Robotics. These preliminary experiments show that using the proposed method in addition to IMU decreases drift and improves localization accuracy, suggesting that its use can enable successful recovery from a loss of visual tracking.

This paper reports on a method for robust selection of inter-map loop closures in multi-robot simultaneous localization and mapping (SLAM). Existing robust SLAM methods assume a good initialization or an “odometry backbone” to classify inlier and outlier loop closures. In the multi-robot case, these assumptions do not always hold. This paper presents an algorithm called Pairwise Consistency Maximization (PCM) that estimates the largest pairwise internally consistent set of measurements. Finding the largest pairwise internally consistent set can be transformed into an instance of the maximum clique problem from graph theory, and by leveraging the associated literature it can be solved in realtime. This paper evaluates how well PCM approximates the combinatorial gold standard using simulated data. It also evaluates the performance of PCM on synthetic and real-world data sets in comparison with DCS, SCGP, and RANSAC, and shows that PCM significantly outperforms these methods.

The factor graph framework is a convenient modeling technique for robotic state estimation where states are represented as nodes, and measurements are modeled as factors. When designing a sensor fusion framework for legged robots, one often has access to visual, inertial, joint encoder, and contact sensors. While visual-inertial odometry has been studied extensively in this framework, the addition of a preintegrated contact factor for legged robots has been only recently proposed. This allowed for integration of encoder and contact measurements into existing factor graphs, however, new nodes had to be added to the graph every time contact was made or broken. In this work, to cope with the problem of switching contact frames, we propose a hybrid contact preintegration theory that allows contact information to be integrated through an arbitrary number of contact switches. The proposed hybrid modeling approach reduces the number of required variables in the nonlinear optimization problem by only requiring new states to be added alongside camera or selected keyframes. This method is evaluated using real experimental data collected from a Cassie-series robot where the trajectory of the robot produced by a motion capture system is used as a proxy for ground truth. The evaluation shows that inclusion of the proposed preintegrated hybrid contact factor alongside visual-inertial navigation systems improves estimation accuracy as well as robustness to vision failure, while its generalization makes it more accessible for legged platforms.

Many autonomous systems require the ability to perceive and understand motion in a dynamic environment. We present a novel algorithm that estimates this motion from raw LIDAR data in real-time without the need for segmentation or model-based tracking. The sensor data is first used to construct an occupancy grid. The foreground is then extracted via a learned background filter. Using the filtered occupancy grid, raw scene flow between successive scans is computed. Finally, we incorporate these measurements in a filtering framework to estimate temporal scene flow. We evaluate our method on the KITTI dataset.

This paper reports on a model-assisted bundle adjustment framework in which visually-derived features are fused with an underlying three-dimensional (3D) mesh provided a priori. By using an approach inspired by the expectation-maximization (EM) class of algorithms, we introduce a hidden binary label for each visual feature that indicates if that feature is considered part of the nominal model, or if the feature corresponds to 3D structure that is absent from this model. Therefore, in addition to improved estimates of the feature locations, we can also label the features based on their deviation from the model. We show that this method is a special case of the Gaussian max-mixtures framework, which can be efficiently incorporated into state-of-the-art graph-based simultaneous localization and mapping (SLAM) solvers. We provide field tests taken from the Bluefin Robotics Hovering Autonomous Underwater Vehicle (HAUV) surveying the SS Curtiss.

This paper reports on an active simultaneous localization and mapping (SLAM) framework that leverages the Bayes tree data structure for efficient planning. Evaluating information-theoretic objective functions in the context of active SLAM is a very expensive process that requires significant computational overhead. The contributions of this work involve exploiting the structure of the planning problem integrated with SLAM via the Bayes tree graphical model. Specifically, we propose a constrained variable ordering and subtree caching scheme that reduce computational complexity by eliminating redundant computations between candidate actions that are similar. We also propose an active SLAM framework that utilizes these concepts, and demonstrate the benefits of the approach with an underwater robot performing visual SLAM in a hybrid simulation environment.

Feature learning has become popular in robotics due to recent advances in machine learning. In this paper, we propose a novel method to utilize the high-dimensional features from these techniques as observations in Bayesian estimation problems in a real-time manner. We develop an approach that: 1) pre-processes the observations and maps them into a new space with both reduced dimensions and a linear relationship to the estimation states; and 2) estimates the uncertainty of resulting outputs using data perturbation. The result is that deep learning approaches can be combined with more traditional filtering approaches like the Kalman filter (KF) to achieve state-of-the-art real-time performance. We validate the method by presenting the first real-time application of underwater robot localization using an imaging sonar. The proposed technique shows similar localization accuracy to benchmark approaches while simultaneously achieving real-time performance.

Real-world autonomous driving in city traffic must cope with dynamic environments including other agents with uncertain intentions. This poses a challenging decision-making problem, e.g., deciding when to perform a passing maneuver or how to safely merge into traffic. Previous work in the literature has typically approached the problem using ad-hoc solutions that do not consider the possible future states of other agents, and thus have difficulty scaling to complex traffic scenarios where the actions of participating agents are tightly conditioned on one another. In this paper we present multipolicy decision-making (MPDM), a decision-making algorithm that exploits knowledge from the autonomous driving domain to make decisions online for an autonomous vehicle navigating in traffic. By assuming the controlled vehicle and other traffic participants execute a policy from a set of plausible closed-loop policies at every timestep, the algorithm selects the best available policy for the controlled vehicle to execute. We perform policy election using forward simulation of both the controlled vehicle and other agents, efficiently sampling from the high-likelihood outcomes of their interactions. We then score the resulting outcomes using a user-defined cost function to accommodate different driving preferences, and select the policy with the highest score. We demonstrate the algorithm on a real-world autonomous vehicle performing passing maneuvers and in a simulated merging scenario.

This paper reports on a method to perform robust visual relocalization between temporally separated sets of underwater images gathered by a robot. The place recognition and relocalization problem is more challenging in the underwater environment mainly due to three factors: 1) changes in illumination; 2) long-term changes in the visual appearance of features because of phenomena like biofouling on man-made structures and growth or movement in natural features; and 3) low density of visually salient features for image matching. To address these challenges, a patch-based feature matching approach is proposed, which uses image segmentation and local intensity contrast to locate salient patches and HOG description to make correspondences between patches. Compared to traditional point-based features that are sensitive to dramatic appearance changes underwater, patch-based features are able to encode higher level information such as shape or structure which tends to persist across years in underwater environments. The algorithm is evaluated on real data, from multiple years, collected by a Hovering Autonomous Underwater Vehicle for ship hull inspection. Results in relocalization performance across missions from different years are compared to other traditional methods.

This paper reports on a 3D photomosaicing pipeline using data collected from an autonomous underwater vehicle performing simultaneous localization and mapping (SLAM). The pipeline projects and blends 2D imaging sonar data onto a large-scale 3D mesh that is either given a priori or derived from SLAM. Compared to other methods that generate a 2D-only mosaic, our approach produces 3D models that are more structurally representative of the environment being surveyed. Additionally, our system leverages recent work in underwater SLAM using sparse point clouds derived from Doppler velocity log range returns to relax the need for a prior model. We show that the method produces reasonably accurate surface reconstruction and blending consistency, with and without the use of a prior mesh. We experimentally evaluate our approach with a Hovering Autonomous Underwater Vehicle (HAUV) performing inspection of a large underwater ship hull.

This paper reports on an underwater cooperative localization algorithm for faulty low-bandwidth communication channels based on a factor graph estimation framework. Vehicles measure the one-way-travel-time (OWTT) of acoustic broadcasts to obtain a relative range observation to the transmitting vehicle. We present a method to robustly share locally observed sensor data across the network by exploiting odometry factor composition. Our algorithm calls on approximate marginalization techniques to compute a compact set of informative factors that enable local navigation data to be shared efficiently. We provide results from a real-time implementation of our algorithm using two autonomous underwater vehicles and a surface vehicle.

This paper reports on a fast multiresolution scan matcher for vehicle localization in urban environments for self-driving cars. State-of-the-art approaches to vehicle localization rely on observing road surface reflectivity with a three-dimensional (3D) light detection and ranging (LIDAR) scanner to achieve centimeter-level accuracy. However, these approaches can often fail when faced with adverse weather conditions that obscure the view of the road paint (e.g., puddles and snowdrifts) or poor road surface texture. We propose a new scan matching algorithm that leverages Gaussian mixture maps to exploit the structure in the environment; these maps are a collection of Gaussian mixtures over the z-height distribution. We achieve real-time performance by developing a novel branch-and-bound, multiresolution approach that makes use of rasterized lookup tables of these Gaussian mixtures. Results are shown on two datasets that are 3.0 km: a standard trajectory and another under adverse weather conditions.

This paper reports on a Gaussian belief-space planning formulation for mobile robots that includes random measurement acquisition variables that model whether or not each measurement is actually acquired. We show that maintaining the stochasticity of these variables in the planning formulation leads to a random belief covariance matrix, allowing us to consider the risk associated with the acquisition in the objective function. Inspired by modern portfolio theory and utility optimization, we design objective functions that are risk-averse, and show that risk-averse planning leads to decisions made by the robot that are desirable when operating under uncertainty. We show the benefit of this approach using simulations of a planar robot traversing an uncertain environment and of an underwater robot searching for loop-closure actions while performing visual SLAM.

This paper reports on an algorithm to support autonomous vehicles in reasoning about occluded regions of their environment to make safe, reliable decisions. In autonomous driving scenarios, other traffic participants are often occluded from sensor measurements by buildings or large vehicles like buses or trucks, which makes tracking dynamic objects challenging.We present a method to augment standard dynamic object trackers with means to 1) estimate the occluded state of other traffic agents and 2) robustly associate the occluded estimates with new observations after the tracked object reenters the visible region of the sensor horizon. We perform occluded state estimation using a dynamics model that accounts for the driving behavior of traffic agents and a hybrid Gaussian mixture model (hGMM) to capture multiple hypotheses over discrete behavior, such as driving along different lanes or turning left or right at an intersection. Upon new observations, we associate them to existing estimates in terms of the Kullback-Leibler divergence (KLD). We evaluate the proposed method in simulation and using a real-world traffic-tracking dataset from an autonomous vehicle platform. Results show that our method can handle significantly prolonged occlusions when compared to a standard dynamic object tracking system.

This paper reports on a system for dynamic obstacle tracking for autonomous vehicles. In this work, we seek to simultaneously estimate both the trajectory of the obstacle and the obstacle's shape. These two tasks are inherently coupled-given only noisy partial views, one cannot accurately estimate the trajectory of an obstacle if its shape is unknown, nor can one estimate its shape without knowing its trajectory. To address this challenge, we note that simultaneous localization and mapping (SLAM), where a robot must build a map of the environment while localizing itself within the map, presents similar challenges. By treating the obstacle's shape as a “map” in the obstacle's moving reference frame, we can formulate the obstacle tracking and shape estimation similarly to SLAM. Additionally, we use a continuous time estimation framework to incorporate sensor data that is collected at a fast rate (e.g., light detection and ranging (LIDAR)). Using these methods, we are able to obtain smooth trajectories and crisp point clouds for tracked obstacles. We test our proposed tracker on real-world data collected by our autonomous vehicle platform and demonstrate that it produces improved results when compared to a standard centroid-based extended Kalman filter (EKF) tracker.

This paper reports on the inclusion of a probabilistic channel model within a cooperative localization planning framework. Underwater cooperative localization reduces positioning errors by sharing sensor data across a team of underwater vehicles. Relative range constraints between vehicles are measured by the one-way-travel-time of successfully received acoustic communication broadcasts. The quality of the navigation solution is intimately linked to the geometry of the network and, therefore, can benefit from planning informative relative trajectories. We cast this planning problem as an instance of belief space planning. In order to weight packet loss over the acoustic channel, we introduce a probabilistic channel model into the planning framework. We propose an optimization algorithm that allows us to plan open-loop control actions and, by extension, closed-loop parameterized trajectories.

This paper proposes an integrated motion planning and control approach for autonomous car navigation. Existing approaches to autonomous vehicle navigation typically plan a trajectory and pass it on to a steering controller that commands steering wheel angle (SWA) or curvature at every timestep to minimize tracking error. However, this approach exhibits large amounts of control effort, and ignores other criteria such as smoothness or the importance of staying on plan at different times. Conversely, our proposed approach leverages the concept of potential fields to represent a driving corridor with a desired tracking error tolerance and direct torque-based steering control to smoothly steer the vehicle with a much smaller control effort. Further, using potential fields allows us to naturally incorporate obstacles in the driving corridor to circumvent them, with typically no need for explicit trajectory planning. We compare our approach to a standard steering controller in experiments with a real-world autonomous vehicle platform. Results show that our proposed approach achieves similar path tracking performance as a high-gain SWA controller, but with much less actuator effort.

This paper reports on optimization-based methods for producing a sparse, conservative approximation of the dense potentials induced by node marginalization in simultaneous localization and mapping (SLAM) factor graphs. The proposed methods start with a sparse, but overconfident, Chow-Liu tree approximation of the marginalization potential and then use optimization-based methods to adjust the approximation so that it is conservative subject to minimizing the Kullback-Leibler divergence (KLD) from the true marginalization potential. Results are presented over multiple real-world SLAM graphs and show that the proposed methods enforce a conservative approximation, while achieving low KLD from the true marginalization potential.

This paper reports on a method for an autonomous underwater vehicle to perform real-time visual simultaneous localization and mapping (SLAM) on large ship hulls over multiple sessions. Along with a monocular camera, our method uses a piecewise-planar model to explicitly optimize the ship hull surface in our factor-graph framework, and anchor nodes to co-register multiple surveys. To enable realtime performance for long-term SLAM, we use the recent Generic Linear Constraints (GLC) framework to sparsify our factor-graph. This paper analyzes how our single-session SLAM techniques can be used in the GLC framework, and describes a particle filter reacquisition algorithm so that an underwater session can be automatically re-localized to a previously built SLAM graph. We provide real-world experimental results involving automated ship hull inspection, and show that our localization filter out-performs Fast Appearance-Based Mapping (FAB-MAP), a popular place-recognition system. Using our approach, we can automatically align surveys that were taken days, months, and even years apart.

This paper reports on a novel mutual information (MI) based algorithm for robust place recognition. The proposed method provides a principled framework for fusing the complementary information obtained from 3D lidar and camera imagery for recognizing places within an a priori map of a dynamic environment. The visual appearance of the locations in the map can be significantly different due to changing weather, lighting conditions and dynamical objects present in the environment. Various 3D/2D features are extracted from the textured point clouds (scans) and each scan is represented as a collection of these features. For two scans acquired from the same location, the high value of MI between the features present in the scans indicates that the scans are captured from the same location. We use a non-parametric entropy estimator to estimate the true MI from the sparse marginal and joint histograms of the features extracted from the scans. Experimental results using seasonal datasets collected over several years are used to validate the robustness of the proposed algorithm.

In many robotic applications, especially long-term outdoor deployments, the success or failure of feature-based image registration is largely determined by changes in lighting. This paper reports on a method to learn visual feature point descriptors that are more robust to changes in scene lighting than standard hand-designed features. We demonstrate that, by tracking feature points in time-lapse videos, one can easily generate training data that captures how the visual appearance of interest points changes with lighting over time. This training data is used to learn feature descriptors that map the image patches associated with feature points to a lower-dimensional feature space where Euclidean distance provides good discrimination between matching and non-matching image patches. Results showing that the learned descriptors increase the ability to register images under varying lighting conditions are presented for a challenging indoor-outdoor dataset spanning 27 mapping sessions over a period of 15 months, containing a wide variety of lighting changes.

This paper reports on an active visual SLAM path planning algorithm that plans loop-closure paths in order to decrease visual navigation uncertainty. Loop-closing revisit actions bound the robot's uncertainty but also contribute to redundant area coverage and increased path length.We propose an opportunistic path planner that leverages sampling-based techniques and information filtering for planning revisit paths that are coverage efficient. Our algorithm employs Gaussian Process regression for modeling the prediction of camera registrations and uses a two-step optimization for selecting revisit actions. We show that the proposed method outperforms existing solutions for bounding navigation uncertainty with a hybrid simulation experiment using a real-world dataset collected by a ship hull inspection robot.

This paper reports on a novel method for estimating the sensor bias of three-axis magnetometers (or any other field sensor). Our approach employs relative angular position measurements to estimate the three-axis magnetometer measurement bias, significantly improving magnetometer-based attitude estimation. Relative angular position measurements can be calculated from a variety of sources, including multiview image registration or laser-based scan matching. We report two methods implementing this approach based on batch linear least squares and a real-time discrete Kalman filter. Compared with previously reported methods our approach is time independent and less restrictive with data sampling. In addition, our two methods (i) are empirically shown to impose less restrictive conditions for the movements of the instrument required for calibration, (ii) do not require knowledge of the direction of the field (e.g., the local magnetic field) or the attitude of the instrument, and (iii) also ensure convergence for the estimated parameters. The proposed methods are evaluated and compared with previously reported methods in both numerical simulation and in comparative experimental evaluation using cameras and magnetometer sensors under different conditions.

This paper reports on the problem of map-based visual localization in urban environments for autonomous vehicles. Self-driving cars have become a reality on roadways and are going to be a consumer product in the near future. One of the most significant road-blocks to autonomous vehicles is the prohibitive cost of the sensor suites necessary for localization. The most common sensor on these platforms, a three-dimensional (3D) light detection and ranging (LIDAR) scanner, generates dense point clouds with measures of surface reflectivity-which other state-of-the-art localization methods have shown are capable of centimeter-level accuracy. Alternatively, we seek to obtain comparable localization accuracy with significantly cheaper, commodity cameras. We propose to localize a single monocular camera within a 3D prior ground-map, generated by a survey vehicle equipped with 3D LIDAR scanners. To do so, we exploit a graphics processing unit to generate several synthetic views of our belief environment. We then seek to maximize the normalized mutual information between our real camera measurements and these synthetic views. Results are shown for two different datasets, a 3.0 km and a 1.5 km trajectory, where we also compare against the state-of-the-art in LIDAR map-based localization.

This paper reports on a generic factor-based method for node removal in factor-graph simultaneous localization and mapping (SLAM), which we call generic linear constraints (GLCs). The need for a generic node removal tool is motivated by long-term SLAM applications, whereby nodes are removed in order to control the computational cost of graph optimization. GLC is able to produce a new set of linearized factors over the elimination clique that can represent either the true marginalization (i.e., dense GLC) or a sparse approximation of the true marginalization using a ChowLiu tree (i.e., sparse GLC). The proposed algorithm improves upon commonly used methods in two key ways: First, it is not limited to graphs with strictly full-state relative-pose factors and works equally well with other low-rank factors, such as those produced by monocular vision. Second, the new factors are produced in such a way that accounts for measurement correlation, which is a problem encountered in other methods that rely strictly upon pairwise measurement composition. We evaluate the proposed method over multiple real-world SLAM graphs and show that it outperforms other recently proposed methods in terms of Kullback-Leibler divergence. Additionally, we experimentally demonstrate that the proposed GLC method provides a principled and flexible tool to control the computational complexity of long-term graph SLAM, with results shown for 34.9 h of real-world indoor-outdoor data covering 147.4 km collected over 27 mapping sessions spanning a period of 15 months.

This paper reports on a factor-based method for node marginalization in simultaneous localization and mapping (SLAM) pose-graphs. Node marginalization in a pose-graph induces fill-in and leads to computational challenges in performing inference. The proposed method is able to produce a new set of constraints over the elimination clique that can represent either the true marginalization, or a sparse approximation of the true marginalization using a Chow-Liu tree. The proposed algorithm improves upon existing methods in two key ways: First, it is not limited to strictly full-state relative-pose constraints and works equally well with other low-rank constraints such as those produced by monocular vision. Second, the new factors are produced in a way that accounts for measurement correlation, a problem ignored in other methods that rely upon measurement composition. We evaluate the proposed method over several real-world SLAM graphs and show that it outperforms other state-of-the-art methods in terms of Kullback-Leibler divergence.

This paper reports on an integrated navigation algorithm for the visual simultaneous localization and mapping (SLAM) robotic area coverage problem. In the robotic area coverage problem, the goal is to explore and map a given target area in a reasonable amount of time. This goal necessitates the use of minimally redundant overlap trajectories for coverage efficiency; however, visual SLAM's navigation estimate will inevitably drift over time in the absence of loop-closures. Therefore, efficient area coverage and good SLAM navigation performance represent competing objectives. To solve this decision-making problem, we introduce perception-driven navigation (PDN), an integrated navigation algorithm that automatically balances between exploration and revisitation using a reward framework. This framework accounts for vehicle localization uncertainty, area coverage performance, and the identification of good candidate regions in the environment for loop-closure. Results are shown for a hybrid simulation using synthetic and real imagery from an autonomous underwater ship hull inspection application.

This paper reports on methods for incorporating camera calibration uncertainty into a two-view sparse bundle adjustment (SBA) framework. The co-registration of two images is useful in mobile robotics for determining motion over time. These camera measurements can constrain a robot's relative poses so that the trajectory and map can be estimated in a technique known as simultaneous localization and mapping (SLAM). Here, we comment on the importance of propagating uncertainty in both feature extraction and camera calibration in visual pose-graph SLAM. We derive an improved pose covariance estimate that leverages the Unscented Transform, and compare its performance to previous methods in both simulated and experimental trials. The two experiments reported here involve data from a camera mounted on a KUKA robotic arm (where a precise ground-truth trajectory is available) and a Hovering Autonomous Underwater Vehicle (HAUV) for large-scale autonomous ship hull inspection.

This paper reports on the use of generic linear constraint (GLC) node removal as a method to control the computational complexity of long-term simultaneous localization and mapping. We experimentally demonstrate that GLC provides a principled and flexible tool enabling a wide variety of complexity management schemes. Specifically, we consider two main classes: batch multi-session node removal, in which nodes are removed in a batch operation between mapping sessions, and online node removal, in which nodes are removed as the robot operates. Results are shown for 34.9 h of real-world indoor-outdoor data covering 147.4 km collected over 27 mapping sessions spanning a period of 15 months.

This paper reports on the use of planar patches as features in a real-time simultaneous localization and mapping (SLAM) system to model smooth surfaces as piecewise-planar. This approach works well for using observed point clouds to correct odometry error, even when the point cloud is sparse. Such sparse point clouds are easily derived by Doppler velocity log sensors for underwater navigation. Each planar patch contained in this point cloud can be constrained in a factor-graph-based approach to SLAM so that neighboring patches are sufficiently coplanar so as to constrain the robot trajectory, but not so much so that the curvature of the surface is lost in the representation. To validate our approach, we simulated a virtual 6-degree of freedom robot performing a spiral-like survey of a sphere, and provide real-world experimental results for an autonomous underwater vehicle used for automated ship hull inspection. We demonstrate that using the sparse 3D point cloud greatly improves the self-consistency of the map. Furthermore, the use of our piecewise-planar framework provides an additional constraint to multi-session underwater SLAM, improving performance over monocular camera measurements alone.

This paper reports a real-time monocular visual simultaneous localization and mapping (SLAM) algorithm and results for its application in the area of autonomous underwater ship hull inspection. The proposed algorithm overcomes some of the specific challenges associated with underwater visual SLAM, namely, limited field of view imagery and feature-poor regions. It does so by exploiting our SLAM navigation prior within the image registration pipeline and by being selective about which imagery is considered informative in terms of our visual SLAM map. A novel online bag-of-words measure for intra and interimage saliency are introduced and are shown to be useful for image key-frame selection, information-gain-based link hypothesis, and novelty detection. Results from three real-world hull inspection experiments evaluate the overall approach, including one survey comprising a 3.4-h/2.7-km-long trajectory.

We report a decentralized extended information filter (DEIF) algorithm designed for single-beacon cooperative acoustic navigation of one or more client underwater vehicles. In single-beacon cooperative acoustic navigation, ranges and state information from a single reference source (the server) are used to improve localization and navigation of an underwater vehicle (the client). The ranges and state information are obtained using underwater acoustic modems and a synchronous-clock time-of-flight paradigm. Apart from the server's acoustic data broadcasts, the client has no access to the server's position or sensor measurements. We show that at the instance of each range measurement update, the DEIF algorithm yields identical results for the current vehicle state estimate as the corresponding centralized extended information filter (CEIF), which fully tracks the joint probability distribution between the server and client. We compare the state estimation results of the DEIF algorithm with that of a CEIF and three other filters reported in the literature. The evaluation is performed using both simulated data and an experimental dataset comprised of one surface craft and two autonomous underwater vehicles.

This paper reports on a mutual information (MI) based algorithm for automatic extrinsic calibration of a 3D laser scanner and optical camera system. By using MI as the registration criterion, our method is able to work in situ without the need for any specific calibration targets, which makes it practical for in-field calibration. The calibration parameters are estimated by maximizing the mutual information obtained between the sensor-measured surface intensities. We calculate the Cramer-Rao-Lower-Bound (CRLB) and show that the sample variance of the estimated parameters empirically approaches the CRLB for a sufficient number of views. Furthermore, we compare the calibration results to independent ground-truth and observe that the mean error also empirically approaches to zero as the number of views are increased. This indicates that the proposed algorithm, in the limiting case, calculates a minimum variance unbiased (MVUB) estimate of the calibration parameters. Experimental results are presented for data collected by a vehicle mounted with a 3D laser scanner and an omnidirectional camera system.

This paper reports a novel mutual information (MI) based algorithm for automatic registration of unstructured 3D point clouds comprised of co-registered 3D lidar and camera imagery. The proposed method provides a robust and principled framework for fusing the complementary information obtained from these two different sensing modalities. High-dimensional features are extracted from a training set of textured point clouds (scans) and hierarchical k-means clustering is used to quantize these features into a set of codewords. Using this codebook, any new scan can be represented as a collection of codewords. Under the correct rigid-body transformation aligning two overlapping scans, the MI between the codewords present in the scans is maximized. We apply a James-Stein-type shrinkage estimator to estimate the true MI from the marginal and joint histograms of the codewords extracted from the scans. Experimental results using scans obtained by a vehicle equipped with a 3D laser scanner and an omnidirectional camera are used to validate the robustness of the proposed algorithm over a wide range of initial conditions. We also show that the proposed method works well with 3D data alone.

This paper reports an algorithm for the registration of images with low overlap and low visual feature density- a typical characteristic of down-looking underwater imagery. Our algorithm exploits locally accurate temporal motion-priors and pairwise image correspondences to aggregate semi-rigid sets of sequential images. These sets are then used to search for visual correspondences across sets instead of between individual pairs of images. By simultaneously searching over multiple views, we increase the physical area seen by more than one image, effectively increasing the "field of view" of the image correspondence search. This increases the probability that the area viewed by both sets will contain enough visual features to register the sets. Our algorithm systematically reduces the uncertainty in the motion prior between the two sets resulting in a refined motion prior that is used to geometrically constrain the correspondence search between sets. This geometric constraint allows us to confidently identify local correspondences that would not be possible globally, further increasing our ability to register images in feature poor environments. We present results using a real-world ship hull inspection data set collected by an autonomous underwater vehicle.

This paper reports a novel algorithm for boot strapping the automatic registration of unstructured 3D point clouds collected using co-registered 3D lidar and omnidirectional camera imagery. Here, we exploit the co-registration of the 3D point cloud with the available camera imagery to associate high dimensional feature descriptors such as scale invariant feature transform (SIFT) or speeded up robust features (SURF) to the 3D points. We first establish putative point correspondence in the high dimensional feature space and then use these correspondences in a random sample consensus (RANSAC) framework to obtain an initial rigid body transformation that aligns the two scans. This initial transformation is then refined in a generalized iterative closest point (ICP) framework. The proposed method is completely data driven and does not require any initial guess on the transformation. We present results from a real world dataset collected by a vehicle equipped with a 3D laser scanner and an omnidirectional camera.

This paper reports on a method for tracking a camera system within an a priori known map constructed from co-registered 3D light detection and ranging (LIDAR) and omnidirectional image data. Our method pre-processes the raw 3D LIDAR and camera data to produce a sparse map that can scale to city-size environments. From the original LIDAR and camera data we extract visual features and identify those that are most robust to varying viewpoint. This allows us to include only the visual features that are most useful for localization in the map. Additionally, we quantize the visual features using a vocabulary tree to further reduce the map's file size. We then use vision-based localization to track the vehicle's motion through the map. We present results on urban data collected with Ford Motor Company's autonomous vehicle testbed. In our experiments the map is built using urban data from winter 2009, and localization is performed using data collected in fall 2010 and winter 2011. This demonstrates our algorithm's robustness to temporal changes in the environment.

This paper reports on a method to combine expected information gain with visual saliency scores in order to choose geometrically and visually informative loop-closure candidates for pose-graph visual simultaneous localization and mapping (SLAM). Two different bag-of-words saliency metrics are introduced—global saliency and local saliency. Global saliency measures the rarity of an image throughout the entire data set, while local saliency describes the amount of texture richness in an image. The former is important in measuring an overall global saliency map for a given area, and is motivated from inverse document frequency (a measure of rarity) in information retrieval. Local saliency is defined by computing the entropy of the bag-of-words histogram, and is useful to avoid adding visually benign key frames to the map. The two different metrics are presented and experimentally evaluated with indoor and underwater imagery to verify their utility.

This paper reports the application of vision based simultaneous localization and mapping (SLAM) to the problem of autonomous ship hull inspection by an underwater vehicle. The goal of this work is to automatically map and navigate the underwater surface area of a ship hull for foreign object detection and maintenance inspection tasks. For this purpose we employ a pose-graph SLAM algorithm using an extended information filter for inference. For perception, we use a calibrated monocular camera system mounted on a tilt actuator so that the camera approximately maintains a nadir view to the hull. A combination of SIFT and Harris features detectors are used within a pairwise image registration framework to provide camera-derived relative-pose constraints (modulo scale). Because the ship hull surface can vary from being locally planar to highly three-dimensional (e.g., screws, rudder), we employ a geometric model selection framework to appropriately choose either an essential matrix or homography registration model during image registration. This allows the image registration engine to exploit geometry information at the early stages of estimation, which results in better navigation and structure reconstruction via more accurate and robust camera-constraints. Preliminary results are reported for mapping a 1,300 image data set covering a 30 m by 5 m section of the hull of a USS aircraft carrier. The post-processed result validates the algorithm's potential to provide in-situ navigation in the underwater environment for trajectory control, while generating a texture-mapped 3D model of the ship hull as a byproduct for inspection.

This paper reports the development and experimental evaluation of a novel navigation system for underwater vehicles that employs Doppler sonar, synchronous clocks, and acoustic modems to achieve simultaneous acoustic communication and navigation. The system reported herein, which is employed to renavigate the vehicle in post-processing, forms the basis for a vehicle-based real-time navigation system. Existing high-precision absolute navigation techniques for underwater vehicles are impractical over long length scales and lack scalability for simultaneously navigating multiple vehicles. The navigation method reported in this paper relies on a single moving reference beacon, eliminating the requirement for the underwater vehicle to remain in a bounded navigable area. The use of underwater modems and synchronous clocks enables range measurements based on one-way time-of-flight information from acoustic data packet broadcasts. The acoustic data packets are broadcast from the single, moving reference beacon and can be received simultaneously by multiple vehicles within acoustic range. We report experimental results from the first deep-water evaluation of this method using data collected from an autonomous underwater vehicle (AUV) survey carried out in 4000 m of water on the southern Mid-Atlantic Ridge. We report a comparative experimental evaluation of the navigation fixes provided by the proposed synchronous acoustic navigation system in comparison to navigation fixes obtained by an independent conventional long baseline acoustic navigation system.

Abstract — The Arctic seafloor remains one of the last unexplored areas on Earth. Exploration of this unique environment using standard remotely operated oceanographic tools has been obstructed by the dense Arctic ice cover. In the summer of 2007 the Arctic Gakkel Vents Expedition (AGAVE) was conducted with the express intention of understanding aspects of the marine biology, chemistry and geology associated with hydrothermal venting on the section of the mid-ocean ridge known as the Gakkel Ridge. Unlike previous research expeditions to the Arctic the focus was on high resolution imaging and sampling of the deep seafloor. To accomplish our goals we designed two new Autonomous Underwater Vehicles (AUVs) named Jaguar and Puma, which performed a total of nine dives at depths of up to 4062m. These AUVs were used in combination with a towed vehicle and a conventional CTD (conductivity, temperature and depth) program to characterize the seafloor. This paper describes the design decisions and operational changes required to ensure useful service, and facilitate deployment, operation, and recovery in the unique Arctic environment. I.

This paper reports recent experimental results in the development and deployment of a synchronous-clock acoustic navigation system suitable for the simultaneous navigation of multiple underwater vehicles. The goal of this work is to enable the task of navigating multiple autonomous underwater vehicles (AUVs) over length scales of O(100 km), while maintaining error tolerances commensurate with conventional long-baseline transponder-based navigation systems (i.e., O(1 m)), but without the requisite need for deploying, calibrating, and recovering seafloor anchored acoustic transponders. Our navigation system is comprised of an acoustic modem-based communication/navigation system that allows for onboard navigational data to be broadcast as a data packet by a source node, and for all passively receiving nodes to be able to decode the data packet to obtain a one-way travel time pseudo-range measurement and ephemeris data. We present results for two different field experiments using a two-node configuration consisting of a global positioning system (GPS) equipped surface ship acting as a global navigation aid to a Doppler-aided AUV. In each experiment, vehicle position was independently corroborated by other standard navigation means. Initial results for a maximum-likelihood sensor fusion framework are reported.

This paper reports the novel insight that the simultaneous localization and mapping (SLAM) information matrix is exactly sparse in a delayed-state framework. Such a framework is used in view-based representations of the environment that rely upon scan-matching raw sensor data to obtain virtual observations of robot motion with respect to a place it has previously been. The exact sparseness of the delayed-state information matrix is in contrast to other recent feature-based SLAM information algorithms, such as sparse extended information filter or thin junction-tree filter, since these methods have to make approximations in order to force the feature-based SLAM information matrix to be sparse. The benefit of the exact sparsity of the delayed-state framework is that it allows one to take advantage of the information space parameterization without incurring any sparse approximation error. Therefore, it can produce equivalent results to the full-covariance solution. The approach is validated experimentally using monocular imagery for two datasets: a test-tank experiment with ground truth, and a remotely operated vehicle survey of the RMS Titanic.

This paper presents the novel insight that the SLAM information matrix is exactly sparse in a delayed-state framework. Such a framework is used in view-based representations of the environment which rely upon scan-matching raw sensor data. Scan-matching raw data results in virtual observations of robot motion with respect to a place its previously been. The exact sparseness of the delayed-state information matrix is in contrast to other recent feature based SLAM information algorithms like Sparse Extended Information Filters or Thin Junction Tree Filters. These methods have to make approximations in order to force the feature-based SLAM information matrix to be sparse. The benefit of the exact sparseness of the delayed-state framework is that it allows one to take advantage of the information space parameterization without having to make any approximations. Therefore, it can produce equivalent results to the “full-covariance” solution.

Recently, there have been a number of variant simultaneous localization and mapping (SLAM) algorithms that have made substantial progress towards large-area scalability by parameterizing the SLAM posterior within the information (canonical/inverse covariance) form. Of these, probably the most well known and popular approach is the sparse extended information filter (SEIF) by Thrun et al. While SEIFs have been successfully implemented with a variety of challenging real world datasets and have led to new insights into scalable SLAM, open research questions remain regarding the approximate sparsification procedure and its effect on map error consistency. In this paper, we examine the constant time SEIF sparsification procedure in depth and offer new insight into issues of consistency. In particular, we show that exaggerated map inconsistency occurs within the global reference frame where estimation is performed, but that empirical testing shows that relative local map relationships are preserved. We then present a slightly modified version of their sparsification procedure, which is shown to preserve sparsity while also generating both local and global map estimates comparable to those obtained by the nonsparsified SLAM filter. While this modified approximation is no longer constant time, it does serve as a theoretical benchmark against which to compare SEIFs constant time results. We demonstrate our findings by benchmark comparison of the modified and original SEIF sparsification rule using simulation in the linear Gaussian SLAM case and real world experiments for a nonlinear dataset.

This work describes a framework for sensor fusion of navigation data with camera-based 5 DOF relative pose measurements for 6 DOF vehicle motion in an unstructured 3D underwater environment. The fundamental goal of this work is to concurrently estimate online current vehicle position and its past trajectory. This goal is framed within the context of improving mobile robot navigation to support sub-sea science and exploration. Vehicle trajectory is represented by a history of poses in an augmented state Kalman filter. Camera spatial constraints from overlapping imagery provide partial observation of these poses and are used to enforce consistency and provide a mechanism for loop-closure. The multi-sensor camera + navigation framework is shown to have compelling advantages over a camera-only based approach by: 1) improving the robustness of pairwise image registration, 2) setting the free gauge scale, and 3) allowing for a unconnected camera graph topology. Results are shown for a real world data set collected by an autonomous underwater vehicle in an unstructured undersea environment.

Describes an approach for the fusion of 3D data underwater obtained from multiple sensing modalities. In particular, we examine the combination of image-based structure-from-motion (SFM) data with bathymetric data obtained using pencil-beam underwater sonar, in order to recover the shape of the seabed terrain. We also combine image-based egomotion estimation with acoustic-based and inertial navigation data on board the underwater vehicle. When fusion is performed at the data level, each modality is used to extract 3D information independently. The 3D representations are then aligned and compared. In this case, we use the bathymetric data as ground truth to measure the accuracy and drift of the SFM approach. Similarly we use the navigation data as ground truth against which we measure the accuracy of the image-based ego-motion estimation. We examine how low-resolution bathymetric data can be used to seed the higher-resolution SFM algorithm, improving convergence rates, and reducing drift error. Similarly, acoustic-based and inertial navigation data improves the convergence and drift properties of egomotion estimation.