Pose Graph Optimization


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
Multi-level Relaxation (MLR)
Our Method


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.


Edwin Olson eolson@mit.edAlumni 
John Leonard jleonard@mit.edu  Faculty 
Seth Teller teller@csail.mit.edu  Faculty 


Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates (PDF) (1.1 MB)

       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},

The tree-based data structure which allows O(log N) constraint updates, as mentioned in the paper, is implemented in Java:


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-33x
Cambridge, MA 02139
Tel: 617-253-6583
Fax: 617-258-7413
Logo of the MIT Computer Science and Artificial Intelligence Laboratory