iSAM2: Incremental smoothing and mapping using the Bayes tree

By Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John J. Leonard, Frank Dellaert. 2012

1,453 citations recorded by the source corpus.

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.

Subfields: Aerospace Engineering, Artificial Intelligence, Computer Vision and Pattern Recognition

How researchers see this paper

The researchers whose trusted networks rank it highest, strongest advocate first. Every researcher gets a different order.

In Aerospace Engineering

In Artificial Intelligence

In Computer Vision and Pattern Recognition

Citations in the corpus

The limits and potentials of deep learning for robotics (2018, methodology): “This would allow robots to treat a deep neural network like any other sensor, and use the established Bayesian techniques [53], [58], [117] to fuse the network’s predictions with prior knowledge or other sensor measurements, or to accumulate information over time.”
Agile Autonomous Driving using End-to-End Deep Imitation Learning (2017, methodology): “We use an MPC expert [22] based on an incremental Sparse Spectrum Gaussian Process (SSGP) [14] dynamics model (which was learned from 30 minute-long driving data) and an iSAM2 state estimator [8].”
Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping (2019, methodology): “The factor graph is solved using iSAM2 [56] in GTSAM [57].”
Agile Off-Road Autonomous Driving Using End-to-End Deep Imitation Learning (2017, methodology): “We use an MPC expert [21] based on an incremental Sparse Spectrum Gaussian Process (SSGP) dynamics model (which was learned from 30 minute-long driving data) and an iSAM2 state estimator [22].”
Computer Vision for Autonomous Vehicles: Problems, Datasets and State-of-the-Art (2017, methodology): “Kaess et al. (2012) have introduced the Bayes tree, a novel data structure, to allow a better understanding of the connection between graphical model inference and sparse matrix factorization in SLAM.”
A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots (2018, methodology): “• SVO+GTSAM [8] - a lightweight visual odometry frontend with a full-smoothing backend provided by iSAM2 [9]”
Scan Context++: Structural Place Recognition Robust to Rotation and Lateral Variations in Urban Environments (2021, methodology): “As in Table IV, the Scan Context-based loop detection and posegraph optimization with iSAM2 [51] successfully alleviated the odometry trajectory’s drifts.”
FutureMapping: The Computational Structure of Spatial AI Systems (2018, methodology): “This graph structure has been recognized and made use of by many important SLAM methods (e.g. [22, 26, 49, 12, 35]).”
Visual-Inertial Navigation: A Concise Review (2019, methodology): “[27] employed the factor graph to represent the VINS problem and then solved it incrementally in analogy to iSAM [81, 82].”
A constant-time SLAM back-end in the continuum between global mapping and submapping: application to visual stereo SLAM (2016, methodology): “That is, any efﬁcient global SLAM method (such as iSAM2 in (Kaess et al. 2012)) can be used to retrieve the global map from the relative graph built by RBA, although it would normally not be required for any robot operation.”
PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features (2018, methodology): “Bundle adjustment by nonlinear optimization can be treated as a factor graph [35] as shown in Figure 3a: round nodes are the camera poses or 3D landmarks needed to be optimized; edges with square boxes represent the visual measurements as constraints between nodes.”