Factor Graph Optimization

The factor graph is a bipartite graph with two distinct node types: variable nodes representing unknown states (e.g., robot poses, landmark positions) and factor nodes representing probabilistic constraints. An edge exists only between a factor and the variables it constrains. This explicit structure makes the underlying inference problem transparent and allows for the natural encoding of heterogeneous sensor measurements—such as GPS priors, visual odometry, and loop closures—as distinct factor types within a single unified framework.

Inference in a factor graph is formulated as finding the maximum a posteriori (MAP) estimate by minimizing the sum of squared error functions across all factors. Each factor encodes a cost proportional to the negative log-likelihood of a measurement. The optimization iteratively linearizes these nonlinear factors around the current estimate and solves a sparse linear system—typically via Gauss-Newton or Levenberg-Marquardt algorithms—to update the variable assignments until convergence to a global minimum.

Factor graphs inherently exploit the sparsity of the SLAM problem, where each measurement constrains only a small subset of variables. This leads to an information matrix with a characteristic arrowhead or block-tridiagonal pattern. Advanced solvers like iSAM2 (Incremental Smoothing and Mapping 2) use the Bayes tree data structure to perform incremental updates, re-linearizing and re-solving only the affected portion of the graph when new factors are added, enabling real-time performance on long-duration trajectories.

To handle data association errors and sensor outliers, factor graphs employ M-estimator robust loss functions that reduce the influence of high-error measurements during optimization. Common kernels include:

• Huber: Quadratic for small errors, linear for large errors.
• Cauchy: Suppresses outliers more aggressively with a logarithmic penalty.
• Geman-McClure: A redescending kernel that fully rejects extreme outliers. These are applied directly to the residual of each factor, preventing a single false loop closure from corrupting the entire state estimate.

Factor graph optimization is a smoothing approach, meaning it re-estimates the entire history of states given all available measurements, rather than only the current state as in Kalman filtering. This full batch or sliding-window optimization allows information from future measurements to refine past estimates, which is critical for correcting accumulated drift when a loop closure is detected. The result is a globally consistent trajectory that a causal filter alone cannot achieve.

The core inference objective solved by factor graph optimization. MAP estimation finds the most probable set of state variables given observed measurements and prior knowledge. In a factor graph, this is achieved by finding the variable configuration that maximizes the product of all factor potentials, which is equivalent to minimizing the sum of their negative log-likelihoods. This transforms a complex probabilistic inference into a sparse nonlinear least squares problem solvable with iterative methods like Gauss-Newton or Levenberg-Marquardt.

The mathematical optimization engine underlying factor graph solvers like GTSAM and g2o. Since sensor measurement functions are rarely linear, the MAP objective becomes a nonlinear least squares problem. Solvers iteratively linearize the factors around the current estimate, solve a linear system for an update step, and repeat until convergence. Key properties exploited for efficiency include the sparsity of the Hessian matrix and the use of robust loss functions like Huber or Cauchy to handle outlier measurements.

The canonical application domain for factor graph optimization. In pose-graph SLAM, robot poses are variables and constraints between them—derived from odometry or loop closures—are factors. Factor graphs elegantly handle the back-end optimization of SLAM by naturally representing the sparse, graph-structured nature of the problem. Modern systems like iSAM2 perform incremental smoothing and mapping, efficiently updating the full solution in real-time as new measurements arrive without recomputing from scratch.

A specific factor graph formulation from photogrammetry and computer vision. Bundle adjustment jointly optimizes 3D landmark positions and camera poses by minimizing the reprojection error of observed image points. In the factor graph, each observed 2D image feature generates a projection factor connecting a camera pose variable and a landmark variable. The sparsity patterns of the resulting Schur complement are heavily exploited to solve large-scale structure-from-motion problems efficiently.

The directed probabilistic graphical model from which factor graphs generalize. A Bayesian network represents a joint probability distribution as a directed acyclic graph where edges indicate conditional dependencies. Factor graphs convert this into a bipartite representation that is more flexible for inference. Unlike Bayesian networks, factor graphs can represent undirected or cyclic dependencies and make the factorization of the joint probability explicit, which is essential for formulating general sensor fusion problems.

A paradigm that treats the full robot trajectory and map as a single optimization problem, in contrast to filtering approaches. Smoothing considers all past states when estimating the current one, producing more accurate results than recursive filters by relinearizing past measurements. Factor graphs are the natural representation for SAM, where the entire history of poses and landmarks is a graph. Incremental smoothing techniques like iSAM2 exploit the graphical structure to provide real-time performance despite the full-history formulation.