The nonlinearity structure of point feature SLAM problems with spherical covariance matrices

Abstract

This paper proves that the optimization problem of one-step point feature Simultaneous Localization and Mapping (SLAM) is equivalent to a nonlinear optimization problem of a single variable when the associated uncertainties can be described using spherical covariance matrices. Furthermore, it is proven that this optimization problem has at most two minima. The necessary and sufficient conditions for the existence of one or two minima are derived in a form that can be easily evaluated using observation and odometry data. It is demonstrated that more than one minimum exists only when the observation and odometry data are extremely inconsistent with each other. A numerical algorithm based on bisection is proposed for solving the one-dimensional nonlinear optimization problem. It is shown that the approach extends to joining of two maps, thus can be used to obtain an approximate solution to the complete SLAM problem through map joining.