On the Structure of Nonlinearities in Pose Graph SLAM

Abstract

Pose graphs have become an attractive representation for solving Simultaneous Localization and Mapping (SLAM) problems. In this paper, we analyze the structure of the nonlinearities in the 2D SLAM problem formulated as the optimizing of a pose graph. First, we prove that finding the optimal configuration of a very basic pose graph with 3 nodes (poses) and 3 edges (relative pose constraints) with spherical covariance matrices, which can be formulated as a six dimensional least squares optimization problem, is equivalent to solving a one dimensional optimization problem. Then we show that the same result can be extended to the optimizing of a pose graph with “two anchor nodes” where every edge is connecting to one of the two anchor nodes. Furthermore, we prove that the global minimum of the resulting one dimensional optimization problem must belong to a certain interval and there are at most 3 minima in that interval. Thus the globally optimal pose configuration of the pose graph can be obtained very easily through the bisection method and closed-form formulas.