iSAM2: Incremental smoothing and mapping using the Bayes tree

Abstract

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.

Reduced Dimensionality Extended Kalman Filter for SLAM

“…But such a backend will impose some other limitations in a multi camera setup. When multiple cameras are updating the same sub-map, it is impossible to do a bundle adjustment within that sub-map in an incremental fashion, making methods like incremental smoothing and mapping (ISAM) [14] less appropriate as they cannot have multiple roots. The possible solution; merging maps together requires marginalizing all camera poses from each map to build a secondary map only with landmarks before fusing them together [6].…”

“…It exploits the sparseness inherent in the structure-frommotion problem to reduce the complexity. Without maintaining all landmark descriptors in this manner one could even use a more efficient sparse matrix system [19] [20] [8], [13], [14] as the back end to build a globally consistent map. As efficient as sparse matrix methods are, they still have limitations and aren't used to process all frames of video as this would generate much denser graphs with high connectivity which would overwhelm the approaches.…”

Towards Autonomous Planetary Exploration

“…The highlighted rectangles represent overlapping submaps that match, resulting in a loop closure constraint that is added to the graph the filter's uncertainty estimates into our overlying SLAM system. For online global optimization of pose and map estimates, we add the submap origins as nodes to a SLAM graph, connect them via the filter estimates and then run the iSAM2 [40] incremental least-squares error minimization after each change of the graph. The combination of a local reference filter and incremental graph SLAM allows us to benefit from their particular advantages: The filter provides real-time, long-term stable state estimation for control and fast obstacle avoidance while the online graph optimization provides global pose and map estimates.…”

Simultaneous Localization and Mapping in Marine Environments

“…While not obvious in the matrix formulation, the Bayes tree allows a fully incremental algorithm, with incremental variable reordering and fluid relinearization. The resulting sparse nonlinear least squares solver is called iSAM2 [38].…”

“…iSAM [38,39] provides an incremental solution to Gauss-Newton style methods, in particular Powell's dog leg [65]. When new measurements are received, this approach updates the existing matrix factorization rather than recalculating the nonlinear least squares system anew each iteration.…”