On the Structure of Nonlinearities in Pose Graph

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.

Novel insights into the impact of graph structure on SLAM

“…Also, all of the existing methods are subject to local minima [5]. In [6] authors show that for a special class of pose-graphs with spherical noise covariance matrices, the nonlinear least squares problem is equivalent to a one-dimensional problem for which there are at most 3 minima. Local optimization algorithms such as GN exhibit different convergence behaviours for different SLAM problems.…”

“…[Cramér-Rao Lower Bound (CRLB)] Under some regularity conditions [15], the covariance matrix of any unbiased estimator of x, such asx, satisfies 6 where I(x) is the Fisher information matrix (FIM),…”

“…In a Bayesian approach, Gaussian prior over poses/features can be incorporated into the least squares formulation in order to obtain the maximum a posteriori (MAP) estimate. 6 A B means A − B is positive semidefinite. As a consequence we have A i,i ≥ B i,i .…”

A critique of current developments in simultaneous localization and mapping

“…Some special structures present in point feature based SLAM and pose-graph SLAM have been recently discovered by Wang et al and Carlone et al, accounting for this suprisingly good behaviour. [20][21][22] Research along this direction has resulted in the development of new algorithms for optimization based SLAM. 22,23 The authors are of the view that further research on the structure of the SLAM problem has the potential to result in more robust SLAM algorithms.…”

Exploiting the Separable Structure of SLAM

“…The equivalence between the minima of the original optimization problem and those of the reduced problem (15) have allowed researchers to study various properties of SLAM by looking at the reduced problem. For instance in [27,28] we analyze the number of local minima is in some small special cases using this idea. Similarly in [3] the reduced problem is used to analyze the convergence of GN in 2D pose-graphs with spherical noise covariance.…”