Inference on networks of mixtures for robust robot mapping

The central challenge in robotic mapping is obtaining reliable data associations (or “loop closures”): state-of-the-art inference algorithms can fail catastrophically if even one erroneous loop closure is incorporated into the map. Consequently, much work has been done to push error rates closer to zero. However, a long-lived or multi-robot system will still encounter errors, leading to system failure. We propose a fundamentally different approach: allow richer error models that allow the probability of a failure to be explicitly modeled. In other words, rather than characterizing loop closures as being “right” or “wrong”, we propose characterizing the error of those loop closures in a more expressive manner that can account for their non-Gaussian behavior. Our approach leads to an fully integrated Bayesian framework for dealing with error-prone data. Unlike earlier multiple-hypothesis approaches, our approach avoids exponential memory complexity and is fast enough for real-time performance. We show that the proposed method not only allows loop closing errors to be automatically identified, but also that in extreme cases, the “front-end” loop-validation systems can be unnecessary. We demonstrate our system both on standard benchmarks and on the real-world data sets that motivated this work.

Pushing the envelope of modern methods for bundle adjustment

In this paper, we present results and experiments with several methods for bundle adjustment, producing the fastest bundle adjuster ever published in terms of computation and convergence. From a computational perspective, the fastest methods naturally handle the block-sparse pattern that arises in a reduced camera system. Adapting to the naturally arising block-sparsity allows the use of BLAS3, efficient memory handling, fast variable ordering, and customized sparse solving, all simultaneously. We present two methods; one uses exact minimum degree ordering and block-based LDL solving and the other uses block-based preconditioned conjugate gradients. Both methods are performed on the reduced camera system. We show experimentally that the adaptation to the natural block sparsity allows both of these methods to perform better than previous methods. Further improvements in convergence speed are achieved by the novel use of embedded point iterations. Embedded point iterations take place inside each camera update step, yielding a greater cost decrease from each camera update step and, consequently, a lower minimum. This is especially true for points projecting far out on the flatter region of the robustifier. Intensive analyses from various angles demonstrate the improved performance of the presented bundler.

A two-tier map representation for compact-stereo-vision-based SLAM

Vision sensors are increasingly being used in the implementation of Simultaneous Localization and Mapping (SLAM). Even though the mathematical framework of SLAM is well understood, considerable issues remain to be resolved when a particular sensing modality is considered. For instance, the observation model of a small baseline stereo camera is known to be highly nonlinear. As a consequence, state estimations obtained from standard recursive estimators, such as the Extended Kalman Filter, tend to be inconsistent. Further, vision-based approaches are plagued with high feature densities, and the consequent requisite of maintaining large feature databases for loop closure and data association. This paper proposes a two-tier solution for resolving these issues, inspired by the mechanics of human navigation. The proposed two-tier solution addresses the consistency issue by formulating the SLAM problem as a nonlinear batch optimization and presents a novel method for feature management through a two-tier map representation. Simulations and experiments are carried out in an office-like environment to validate the performance of the algorithm.

iSAM2: Incremental smoothing and mapping using the Bayes tree

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.

Power-SLAM: a linear-complexity, anytime algorithm for SLAM

In this paper, we present an extended Kalman filter (EKF)-based estimator for simultaneous localization and mapping (SLAM) with processing requirements that are linear in the number of features in the map. The proposed algorithm, called the Power-SLAM, is based on three key ideas. Firstly, by introducing the Global Map Postponement method, approximations necessary for ensuring linear computational complexity of EKF-based SLAM are delayed over multiple time steps. Then by employing the Power Method, only the most informative of the Kalman vectors, generated during the postponement phase, are retained for updating the covariance matrix. This ensures that the information loss during each approximation epoch is minimized. Next, linear-complexity, rank-2 updates, that minimize the trace of the covariance matrix, are employed to increase the speed of convergence of the estimator. The resulting estimator, in addition to being conservative as compared to the standard EKF, has processing requirements that can be adjusted depending on the availability of computational resources. Lastly, simulation and experimental results are presented that demonstrate the accuracy of the proposed algorithm (Power-SLAM) when compared to the standard EKF-based SLAM with quadratic computational cost and two linear-complexity competing alternatives.

Navigating, Recognizing and Describing Urban Spaces With Vision and Lasers

In this paper we describe a body of work aimed at extending the reach of mobile navigation and mapping. We describe how running topological and metric mapping and pose estimation processes concurrently, using vision and laser ranging, has produced a full six-degree-of-freedom outdoor navigation system. It is capable of producing intricate three-dimensional maps over many kilometers and in real time. We consider issues concerning the intrinsic quality of the built maps and describe our progress towards adding semantic labels to maps via scene de-construction and labeling. We show how our choices of representation, inference methods and use of both topological and metric techniques naturally allow us to fuse maps built from multiple sessions with no need for manual frame alignment or data association.

Trajectory Optimization using Reinforcement Learning for Map Exploration

Automatically building maps from sensor data is a necessary and fundamental skill for mobile robots; as a result, considerable research attention has focused on the technical challenges inherent in the mapping problem. While statistical inference techniques have led to computationally efficient mapping algorithms, the next major challenge in robotic mapping is to automate the data collection process. In this paper, we address the problem of how a robot should plan to explore an unknown environment and collect data in order to maximize the accuracy of the resulting map. We formulate exploration as a constrained optimization problem and use reinforcement learning to find trajectories that lead to accurate maps. We demonstrate this process in simulation and show that the learned policy not only results in improved map building, but that the learned policy also transfers successfully to a real robot exploring on MIT campus.