
Research
Abstracts  2006 
Fast and Robust Optimization of Pose GraphsEdwin Olson, John Leonard, & Seth TellerOverviewSimultaneous 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:
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 nonlinear. Below, we show the cost function that results from aligning just two laser scans. Ongoing WorkWith 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 SupportThis research was funded by an NSF SGER grant, number IIS0514639. References:[1] Edwin Olson and John Leonard and Seth Teller. Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates. In ICRA 2006 

