Incremental smoothing and mapping

Abstract

Incremental smoothing and mapping (iSAM) is presented, a novel approach to the simultaneous localization and mapping (SLAM) problem. SLAM is the problem of estimating an observer’s position from local measurements only, while creating a consistent map of the environment. The problem is difficult because even very small errors in the local measurements accumulate over time and lead to large global errors. While SLAM is a key capability in mobile robotics today, the problem has a long history in land surveying. And the underlying estimation problem was already solved by Gauss in 1809 [56, 57] for computing the orbits of asteroids.

iSAM provides an exact and efficient solution to the SLAM estimation problem while also addressing data association. For the estimation problem, iSAM provides an exact solution by performing smoothing, which keeps all previous poses as part of the estimation problem, and therefore avoids linearization errors. iSAM uses methods from sparse linear algebra to provide an efficient incremental solution. In particular, iSAM deploys a direct equation solver based on QR matrix factorization of the naturally sparse smoothing information matrix. Instead of refactoring the matrix whenever new measurements arrive, only the entries of the factor matrix that actually change are calculated. iSAM is efficient even for robot trajectories with many loops as it performs periodic variable reordering to avoid unnecessary fill-in in the factor matrix. For the data association problem, I present state of the art data association techniques in the context of iSAM and present an efficient algorithm to obtain the necessary estimation uncertainties in real-time based on the factored information matrix. I systematically evaluate the components of iSAM as well as the overall algorithm using various simulated and real-world datasets.