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.