iSAM: Fast Incremental Smoothing and Mapping with Efficient Data Association

Abstract

We introduce incremental smoothing and mapping (iSAM), a novel approach to the problem of simultaneous localization and mapping (SLAM) that addresses the data association problem and allows real-time application in large-scale environments. We employ smoothing to obtain the complete trajectory and map without the need for any approximations, exploiting the natural sparsity of the smoothing information matrix. A QR-factorization of this information matrix is at the heart of our approach. It provides efficient access to the exact covariances as well as to conservative estimates that are used for online data association. It also allows recovery of the exact trajectory and map at any given time by backsubstitution. Instead of refactoring in each step, we update the QR-factorization whenever a new measurement arrives. We analyze the effect of loops, and show how our approach extends to the non-linear case. Finally, we provide experimental validation of the overall non-linear algorithm based on the standard Victoria Park data set with unknown correspondences.

Bayesian surprise and landmark detection

“…9. Metric map of Killian Court dataset [3] produced using the iSAM algorithm [9] shown as reference for comparison with Figure 10 We next apply the landmark detection scheme to the MIT Killian Court dataset [3] which is another widely used dataset in the SLAM community. The dataset consists of 1941 poses and corresponding laser scans.…”

“…Given the above discussion, we can now compute surprise as the KL-divergence between two exponential family Polya models using the expression for the model (9). The calculation is straight-forward using basic properties of exponential family distributions and is omitted here for brevity.…”

Efficient Sparse Pose Adjustment for 2D mapping

“…Kaess et al [14] introduced a variant of √ SAM, called iSAM, that performs incremental update of the linear matrix associated with the nonlinear least-squares problem. Relinearization and variable ordering are performed only occasionally, thereby increasing computational efficiency.…”

3D I-SLSJF: A consistent sparse local submap joining algorithm for building large-scale 3D Map

“…This avoids the scenarios where the Jacobian with respect to the same feature/pose be evaluated at different estimate data, which is one of the major causes of inconsistency for EIF/EKF SLAM algorithms [11]. In fact, the state estimate obtained in I-SLSJF is the optimal solution of the least squares problem (14).…”