Factor Graph

• Category: State estimation & navigation › Localisation
• Tags: state-estimation, slam, probabilistic-inference, optimization, sensor-fusion

A bipartite graph that represents a probability distribution as a product of factors, linking unknown variables such as poses and landmarks to the measurements and priors that constrain them.

Definition

A factor graph has two kinds of nodes. Variable nodes stand for the unknowns to be estimated, such as robot poses, velocities, sensor biases or landmark positions. Factor nodes stand for pieces of information about them, such as a prior, an odometry measurement between two poses or a camera observation of a landmark, and each factor connects only to the variables it involves. The graph describes a joint distribution equal to the product of all its factors, which makes the structure of a problem explicit: which unknowns are linked, and which measurements are sparse, local constraints.

Factor graphs were set out as a general framework for inference by Kschischang, Frey and Loeliger (2001) and were adopted widely in robotics, largely through the work of Dellaert and Kaess on smoothing and mapping. With Gaussian noise models, finding the most probable values of all variables given all factors is a nonlinear least-squares problem: each factor contributes a residual weighted by its information matrix. Solvers linearize the residuals, exploit the sparsity of the graph to factorize the resulting linear system efficiently, and iterate with Gauss-Newton or Levenberg-Marquardt. Incremental solvers such as iSAM2 update the solution as new factors arrive without re-solving from scratch. GTSAM implements factor graphs directly, and general least-squares libraries such as g2o and Ceres are used for the same kinds of problems.

The main practical appeal is modularity. A new sensor becomes a new factor type with its own residual and noise model, so IMU preintegration, GPS, wheel odometry, lidar scan matches, loop closures and learned measurement models can all live in one estimator. Pose graph optimization is a special case in which every variable is a pose and every factor a relative-pose constraint; a general factor graph can also hold landmarks, velocities, biases and calibration parameters. Fixed-lag smoothers marginalize old variables to bound computation, at the cost of freezing some linearization points. Outliers, such as false loop closures, need robust cost functions or switchable constraints, since a plain least-squares solution can be pulled far off by a single bad factor.

Why it matters

Factor graphs are the common language of modern SLAM and multi-sensor state estimation. They let engineers add sensors, priors and constraints without redesigning the estimator, and they make explicit which noise and independence assumptions each measurement carries.

Key applications

• SLAM back ends for lidar, visual and visual-inertial systems
• Multi-sensor fusion of IMU, GPS, wheel odometry and vision
• Calibration of camera, lidar and IMU extrinsics
• Structure from motion and bundle adjustment
• Trajectory optimization posed as probabilistic inference
• Multi-robot mapping with shared constraints

In practice

A ground vehicle localizes with ground-penetrating radar against a previously recorded subsurface map. Each time step adds a pose variable; inertial or wheel odometry adds a factor between consecutive poses; and a radar match against the prior map adds a factor on the current pose. Instead of hand-writing a sensor model for the radar, the measurement factor can be learned from data, as in work on learned factor-graph models for GPR localization. The team runs an incremental smoother so the latest pose is available in real time, and after each test drive inspects the factor residuals to find measurement types whose noise models are set too tight or too loose.

Common misconceptions

• A factor graph is a solver — it is a representation of the problem; the solver, such as batch Gauss-Newton or incremental smoothing, is a separate choice
• Factor graphs only describe pose graphs — they can hold landmarks, velocities, biases, calibration parameters and arbitrary measurement models
• Adding more factors always improves the estimate — factors with wrong noise models or outlier measurements can corrupt the whole solution unless robust costs are used

Who should know this

• SLAM and state estimation engineers
• Sensor fusion and navigation developers
• Researchers working on probabilistic inference in robotics
• Autonomous vehicle localization teams

Related technologies

• GTSAM
• iSAM2
• g2o
• Ceres Solver
• Bundle Adjustment
• Levenberg-Marquardt Algorithm

Related terms

• Pose Graph Optimization
• SLAM
• IMU Preintegration
• Loop Closure
• Sensor Fusion
• Tightly Coupled Sensor Fusion

Related projects

• LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
• oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
• Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO
• DimOS: Python robotics runtime with agents, SLAM and teleop, no ROS required
• evo: De-facto standard tool for evaluating and plotting odometry/SLAM trajectories

Videos

• Factor Graph - 5 Minutes with Cyrill: Cyrill Stachniss
• MIT Robotics - Frank Dellaert - Factor Graphs for Perception and Action: MIT Robotics