Fast Incremental Square Root Information Smoothing

Abstract

We propose a novel approach to the problem of simultaneous localization and mapping (SLAM) based on incremental smoothing, that is suitable for real-time applications in large-scale environments. The main advantages over filter-based algorithms are that we solve the full SLAM problem without the need for any approximations, and that we do not suffer from linearization errors. We achieve efficiency by updating the square-root information matrix, a factored version of the naturally sparse smoothing information matrix. We can efficiently recover the exact trajectory and map at any given time by back-substitution. Furthermore, our ap- proach allows access to the exact covariances, as it does not suffer from under-estimation of uncertain- ties, which is another problem inherent to filters. We present simulation-based results for the linear case, showing constant time updates for exploration tasks. We further evaluate the behavior in the pres- ence of loops, and discuss how our approach ex- tends to the non-linear case. Finally, we evalu- ate the overall non-linear algorithm on the standard Victoria Park data set.