A fast and accurate approximation for planar pose graph optimization

This work investigates the pose graph optimization problem, which arises in maximum likelihood approaches to simultaneous localization and mapping (SLAM). State-of-the-art approaches have been demonstrated to be very efficient in medium- and large-sized scenarios; however, their convergence to the maximum likelihood estimate heavily relies on the quality of the initial guess. We show that, in planar scenarios, pose graph optimization has a very peculiar structure. The problem of estimating robot orientations from relative orientation measurements is a quadratic optimization problem (after computing suitable regularization terms); moreover, given robot orientations, the overall optimization problem becomes quadratic. We exploit these observations to design an approximation of the maximum likelihood estimate, which does not require the availability of an initial guess. The approximation, named LAGO (Linear Approximation for pose Graph Optimization), can be used as a stand-alone tool or can bootstrap state-of-the-art techniques, reducing the risk of being trapped in local minima. We provide analytical results on existence and sub-optimality of LAGO, and we discuss the factors influencing its quality. Experimental results demonstrate that LAGO is accurate in common SLAM problems. Moreover, it is remarkably faster than state-of-the-art techniques, and is able to solve very large-scale problems in a few seconds.