Research Abstracts - 2006

Fast and Robust Optimization of Pose Graphs

Edwin Olson, John Leonard, & Seth Teller

Overview

Simultaneous Localization and Mapping (SLAM) problems can be cast as a pose graph optimization problem. In this formulation, robot poses are nodes in a graph, and edges are constraints on the relative location of two poses. We have developed a nonlinear optimization algorithm that solves this problem quickly, even when the initial estimate (typically derived from robot odometry) is very poor.

Like a growing family of SLAM approaches, we explicitly optimize only the robot trajectory since features are easily computed once the trajectory is known (features are conditionally independent). Our approach uses an iterative nonlinear optimization algorithm similar to stochastic gradient descent. In short, constraints in the pose graph are considered one at a time: each constraint yields a loop with respect to the posterior robot trajectory:

Any error in the constraint is distributed around the loop. The error is distributed according to the relative "stiffness" of each pose, as determined by how strongly each pose is constrained. The "stiffness" matrix is the information matrix; our iterative algorithm uses a diagonal approximation.

In addition, we present a state space representation which allows each constraint to be updated in O(log N) time, for a trajectory with N poses. Our algorithm, applied to a large dataset (3500 poses, 6500 constraints), outperforms several others:

Input	Ground Truth	Gauss-Seidel Relaxation (60s)	Multi-level Relaxation (MLR) (8.6s)	Our Method (2.9s)

Compared to other fast approaches, our approach does not require relatively small correlations to be set to zero, nor does it require finding a variable ordering that allows an efficient (sparse) LU decomposition.

Other algorithms, such as those based upon EKFs, EIFs, or SEIFs, suffer from linearization error. The structure of these solutions permits new data to be incorporated only once; if the information is nonlinear, then the observation information is linearized around the current state estimate. Since the state estimate is invariably contaminated by noise, this linearization introduces noise into the state estimate that cannot be removed, even if a better state estimate is obtained later. Our approach is a member of a family of solutions that can incorporate information as many times as necessary, thus removing the effects of earlier linearizations. This yields better quality solutions, since the SLAM problem is inherently non-linear. Below, we show the cost function that results from aligning just two laser scans.

Ongoing Work

With a solid SLAM capability in hand, we are looking at exploration: what can a robot do (actively) so that a SLAM algorithm will produce even better results? How does this extend to multiple robots? With regard to multiple robots, we are interested in mapping environments that would otherwise be unmappable, rather than merely increasing mapping speed.

Research Support

This research was funded by an NSF SGER grant, number IIS-0514639.

References:

[1] Edwin Olson and John Leonard and Seth Teller. Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates. In ICRA 2006