Analysis of minima for geodesic and chordal cost for a minimal 2D pose-graph SLAM problem
This addresses a fundamental issue in SLAM optimization for robotics, but is incremental as it focuses on a minimal problem under ideal assumptions.
The paper tackled the problem of multiple suboptimal local minima in minimal 2D pose-graph SLAM when using geodesic cost with a wrap function, showing numerically that these minima have nonzero measure, and demonstrated that chordal distance representation avoids this issue with a unique minimum and fewer convergence failures.
In this paper, we show that for a minimal pose-graph problem, even in the ideal case of perfect measurements and spherical covariance, using the so-called "wrap function" when comparing angles results in multiple suboptimal local minima. We numerically estimate regions of attraction to these local minima for some numerical examples, and give evidence to show that they are of nonzero measure. In contrast, under the same assumptions, we show that the \textit{chordal distance} representation of angle error has a unique minimum up to periodicity. For chordal cost, we also search for initial conditions that fail to converge to the global minimum, and find that this occurs with far fewer points than with geodesic cost.