A fast and accurate approximation for planar pose graph optimization

Abstract

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 mediumand 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.

A critique of current developments in simultaneous localization and mapping

“…71,72 Moreover, some algorithms such as LAGO and pose-chain SLAM may only work well for datasets with particular characteristics. 22,73 Thus it is important to recognize that both the computational efficiency and accuracy need to be considered when selecting a SLAM algorithm for a practical application.…”

Tree-connectivity: Evaluating the graphical structure of SLAM

“…1 Each measurement is a noisy 2D rigid body transformation between two robot poses. The measurement function, after computing the correct regularization terms for the rotational component of measurements (see [3], [7], [12]) can be expressed as…”

Exploiting the Separable Structure of SLAM

“…CHOLMOD [6] is used as the linear solver with the default choice of fill-reducing permutation. We implemented Algorithm 3 (VP) 5 in C++. An Intel Core i5-2400 CPU operating at 3.1GHz is used for all of the experiments in this paper.…”