Pose Graph Optimization
Summary
Simultaneous Localization and Mapping (SLAM) problems can be posed as a pose graph optimization problem. We have developed a nonlinear optimization algorithm that solves this problem quicky, even when the initial estimate (e.g., robot odometry) is very poor.
Input | Ground Truth | Gauss-Seidel Relaxation (60s) | Multi-level Relaxation (MLR) (8.6s) | Our Method (2.9s) |
Description
Like a growing family of SLAM approaches, we explicitly optimize only the robot trajectory-- features are easily computed once the trajectory is known. 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. 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.
The major advantages of this approach are that it is fast (without requiring approximate factoring of the probability density function), and that it is robust to poor initial estimates.
People
Name | Position | |
---|---|---|
Edwin Olson | eolson@mit.edu | Alumni |
John Leonard | jleonard@mit.edu | Faculty |
Seth Teller | teller@csail.mit.edu | Faculty |
Resources
Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates (PDF) (1.1 MB)
@inproceedings{OlsonGraph2006, author="Edwin Olson and John Leonard and Seth Teller", title="Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates", journal = {ICRA}, institution = {MIT CSAIL}, year = {2006}, pages="2262-2269" }
The tree-based data structure which allows O(log N) constraint updates, as mentioned in the paper, is implemented in Java:
- One dimensional implementation, heavily documented with an explanation of the data structure. WTree.java
- Three dimensional implementation, a straight-forward extension of WTree.java, but without extensive documentation. WTree3.java
Movies
The following videos demonstrate an early version of the system building a map of a large area in MIT's new computer science building.
Mac OS X users may need to download the MS-MPEG4v2 codec.
Robotics, Vision, and Sensor Networks Group
32 Vassar Street, 32-33xCambridge, MA 02139 Tel: 617-253-6583 Fax: 617-258-7413 |