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
back-substitution. 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.