A convergence analysis for pose graph optimization via Gauss-Newton methods

Abstract

In this work we present a convergence analysis of the pose graph optimization problem, that arises in the context of mobile robots localization and mapping. The analysis is performed under some simplifying assumptions on the structure of the measurement covariance matrix and provides non trivial results on the aspects affecting convergence in nonlinear optimization based on Gauss-Newton methods. We also provide estimates for the basin of attraction of the maximum likelihood solution and results on the uniqueness of such solution. The results confirm observations of related work and explain why common Simultaneous Localization and Mapping (SLAM) instances are so well-behaved in terms of convergence. Moreover, as a by-product of the derivation, we present different techniques that can enlarge the convergence radius a-priori (i.e., during robot operation) or a-posteriori (i.e., given the data). We validate the theoretical derivation with experiments on standard benchmarking datasets.

A critique of current developments in simultaneous localization and mapping

“…Finding a good initial value is critical, but how good is adequate is an interesting question to pose. Although some important progress along this direction has been made, 20,48 more investigation is necessary to gain further understanding of this important problem. Furthermore, given that most of the existing SLAM formulations assume Gaussian noise, understanding the impact of nonGaussian noise and strategies for handling this efficiently may lead to more effective SLAM algorithms, particularly when the data rate and sensor quality are poor, for example in turbid underwater environments.…”

“…20,51 In a paper by Carlone, a conservative estimate of the region of attraction of the global minimum for a Gauss-Newton algorithm is provided for 2D pose-graph SLAM. 48 Furthermore, Carlone et al provide a method to verify whether the global minimum solution is obtained by using Lagrange duality. 50 Despite these interesting developments, an algorithm that can guarantee the convergence to the globally optimal solution of a SLAM problem involving a large number of poses is not yet available.…”

Tree-connectivity: Evaluating the graphical structure of SLAM

“…• As noted by [7], ∆ ∆ is a diagonal matrix with an interesting structure. (∆ ∆) i,i is equal to the sum of squared distances between the ith robot pose, and every node observed by it,…”

“…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…”

“…Carlone [7] computes a conservative estimate of the basin of attraction of the maximum likelihood estimate under Gauss-Newton. This estimate is related to the smallest eigenvalue of the reduced Laplacian matrix of the corresponding graph.…”