Computational Enhancements for Certifiably Correct SLAM

Abstract

We investigate numerical and computational aspects of the use of convex relaxation for simultaneous localization and mapping (SLAM). Recent work has shown that convex relaxation provides an effective tool for computing, and certifying the correctness of, globally optimal SLAM solutions. This paper expands upon this prior work by demonstrating how to exploit the structure of the relaxed optimization problem to design very fast solvers, capable of computing globally optimal trajectories with thousands of poses in a fraction of a second. In particular, we describe several computational enhancements for accelerating the underlying Riemannian trust-region optimization method, including the use of structure-exploiting matrix decomposition, iterative linear-algebraic techniques, truncated-Newton methods, and preconditioning strategies. We also describe methods for accelerating the minimum-eigenvalue computation used to certify the optimality of a recovered estimate. We have incorporated these computational enhancements in an updated version of the SE-Sync library, and released the corresponding code online. Experimental results indicate that this enhanced implementation of SE-Sync is approximately twice as fast as GTSAM, a highly optimized, state-of-the-art library for SLAM.

Publication
International Conference on Intelligent Robots and Systems workshop “Introspective Methods for Reliable Autonomy