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.
展开▼
