Pose graph optimization

Poses are nodes and measurements are edges#

Each graph node represents an unknown pose along one or more robot trajectories. An edge records a measured relative transformation between two poses, with an uncertainty or information model. Consecutive odometry creates local edges, while a loop closure can add a long-range edge between visits to the same place.

The optimizer adjusts all pose estimates to reduce the weighted disagreement between predicted and measured relative transforms. The distributed PGO formulation by Tian and colleagues states the multi-robot problem as jointly estimating trajectories in a global frame from relative-pose measurements. The g2o project provides a general graph-optimization implementation used for related estimation problems.

A SLAM back end, not the whole system#

Pose graph optimization is commonly the back end of simultaneous localization and mapping. A front end still has to extract measurements, associate observations, propose loop closures, and estimate their uncertainty. PGO then reconciles those constraints.

The pose graph is also a type of factor graph, but not every factor graph is a pose graph. Factor graphs can contain landmarks, biases, velocities, calibration parameters, and other variables. Bundle adjustment often optimizes camera poses and scene points from image reprojection errors rather than only relative-pose edges.

Bad constraints can distort the map#

One pose or frame must be anchored because relative measurements do not determine an absolute global origin. Rotations make the optimization nonlinear, so initialization and solver design matter. Large graphs also impose memory, computation, and communication costs, especially across several robots.

Most importantly, a false loop closure can pull distant trajectory sections together. A least-squares residual assumes a noise model and can give an outlier excessive influence. Outlier-resistant losses, switchable constraints, consistency checks, or rejection can reduce that risk, but none makes incorrect data harmless. A small final objective does not prove that the map matches the physical environment.