Fast iterative alignment of pose graphs with poor initial estimates

TLDR

SLAM fuses motion and feature estimates into a map and trajectory, but the resulting non‑linear optimization is difficult because poor odometry initializations and large state spaces make convergence challenging. We propose a fast non‑linear optimization algorithm that quickly recovers the robot trajectory even from poor initial estimates. The method applies a variant of stochastic gradient descent on an alternative state‑space representation that offers improved stability and computational efficiency, and we benchmark it against several existing algorithms on real and synthetic data.

Abstract

A robot exploring an environment can estimate its own motion and the relative positions of features in the environment. Simultaneous localization and mapping (SLAM) algorithms attempt to fuse these estimates to produce a map and a robot trajectory. The constraints are generally non-linear, thus SLAM can be viewed as a non-linear optimization problem. The optimization can be difficult, due to poor initial estimates arising from odometry data, and due to the size of the state space. We present a fast non-linear optimization algorithm that rapidly recovers the robot trajectory, even when given a poor initial estimate. Our approach uses a variant of stochastic gradient descent on an alternative state-space representation that has good stability and computational properties. We compare our algorithm to several others, using both real and synthetic data sets