Factor Graph Optimization

Draws every measurement constraint as a “variable-factor” graph, then finds the most likely state by least squares.

A factor graph is a bipartite graph: one type of node holds the variables being estimated, such as a robot’s pose at each moment or a landmark’s 3D coordinates; the other type of node holds factors, each representing a probabilistic constraint from a measurement or a prior — for example, the IMU gives the relative motion between two frames, or a camera gives an observation of a landmark. Finding the maximum a posteriori estimate — the most likely state given all observations — is equivalent to minimizing the sum of squared errors over all factors, which can be solved with nonlinear least-squares methods like Gauss-Newton or Levenberg-Marquardt, sped up by exploiting the graph’s sparsity. A 2017 survey by Frank Dellaert and Michael Kaess systematically lays out this framework, and Georgia Tech’s open-source GTSAM is a commonly used implementation. It is the dominant formulation for the back end of modern SLAM and multi-sensor fusion systems — adding a new sensor just means adding a new type of factor.

ExampleLIO-SAM (IROS 2020) adds lidar odometry, IMU preintegration, and loop closure all as factors into the same factor graph, jointly optimizing the robot’s trajectory and using the optimization result to estimate IMU bias.

Also called

Factor Graph, Graph Optimization

Related

Simultaneous Localization and Mapping · GTSAM (Georgia Tech Smoothing and Mapping) · Bundle Adjustment · IMU Preintegration · Loop Closure Detection · Multi-Sensor Fusion

Sources

GTSAM: Factor Graphs and GTSAM tutorial
Dellaert & Kaess: Factor Graphs for Robot Perception (Foundations and Trends in Robotics, 2017)
LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping