Closing a Million-Landmarks Loop

Abstract

We present an improved version of the treemap SLAM algorithm which uses Cholesky factors for representing Gaussians and a hierarchical tree partitioning algorithm derived from the established Kernighan-Lin heuristic for graph bisection. We demonstrate the algorithm's efficiency by mapping a simulated building with 1032271 landmarks. In the end, we close a million-landmarks loop in 21 ms, providing an estimate for ap10000 selected landmarks close to the robot, or in 442 ms for computing a full estimate