---
title: Factor-Graph Optimization
url: https://www.emergentmind.com/topics/factor-graph-optimization
type: topic
---

# Factor-Graph Optimization

Factor-graph optimization (FGO) is a principled framework for formulating and solving high-dimensional estimation and control problems in which the relationships among variables can be explicitly represented as a sparse bipartite graph. FGO has become foundational in robotics, navigation, signal processing, computer vision, and networked systems, providing a unifying language for structure-exploiting inference and optimization. This article surveys the core principles, algorithmic structures, representative methodologies, and impact of factor-graph optimization, including both unconstrained and equality-/inequality-constrained variants, with reference to key research contributions.

## 1. Mathematical and Algorithmic Foundations

A factor graph is a bipartite graph \(G = (V, F, E)\), where variable nodes \(V = \{x_1, ..., x_n\}\) represent unknown quantities to be estimated, and factor nodes \(F = \{\phi_1, ..., \phi_m\}\) encode measurement likelihoods, physical constraints, or cost terms, each depending on a small subset of variables. Edges \(E\) connect each factor to the variables it concerns.

The global estimation or optimization objective takes the form
\[
J(x) = \sum_{i=1}^{m} \rho_i\left( \| r_i(X_i) \|^2_{\Lambda_i} \right)
\]
where \(r_i(\cdot)\) is the error (residual) vector for factor \(\phi_i\), \(\Lambda_i\) is the associated information (inverse covariance) matrix, and \(\rho_i(\cdot)\) (often identity) allows robust or non-quadratic losses. The structure of the factor graph enforces sparsity in the Jacobian and Hessian matrices arising in local Gauss–Newton or Levenberg–Marquardt iterations, making large-scale problems computationally tractable [1909.00903], [2502.08158], [2011.06194].

At each iteration:
- All residuals are linearized about the current estimate.
- The (block-sparse) normal equations \(H \Delta x = -g\) are assembled, where \(H = \sum J_i^\top \Lambda_i J_i\), \(g = \sum J_i^\top \Lambda_i r_i\).
- The increment \(\Delta x\) is solved using sparse Cholesky or other domain-specific solvers.
- The estimate is updated, and the process repeats until convergence.

Incremental solvers, such as iSAM2, exploit the persistent sparsity and local changes between timesteps to perform fast partial updates [2011.06194].

## 2. Model Construction and Domain Tailoring

The flexibility of factor-graph modeling enables the explicit encoding of diverse sensor modalities, physical constraints, and statistical structures. Representative domains and their factor constructions include:

- **GNSS Positioning:** Variable nodes represent receiver position, clock bias, velocity, and integer ambiguities at each epoch. Measurement factors encode pseudorange, Doppler, carrier-phase, and tightly coupled IMU constraints. Residuals are linear or non-linear in state increments, and robust loss functions (e.g., Huber, Geman–McClure) are used to suppress outliers such as NLOS multipath [2502.08158], [2109.00667], [2109.00683].
- **Robot State Estimation:** Visual-inertial-odometric SLAM systems bind 6D poses, velocities, biases, and landmark coordinates via factors corresponding to IMU preintegration, leg odometry, visual reprojection, and priors. Full-batch or sliding-window FGO achieves significant reductions in drift and improves robustness to poor contact or landmark loss [1904.03048], [2109.03010].
- **Dense Estimation (Stereo, Optical Flow):** Dense mapping of disparities or flows is formulated as a grid factor graph, with variable nodes (pixel disparities/flows), data factors (likelihoods per observation), and spatial smoothness factors with adaptive, edge-aware neighborhoods, allowing flexible regularization and efficient loopy belief propagation [2109.11077].
- **Water Network State and Fault Estimation:** Variables represent nodal pressures, demands, and leak-residuals, while factors model sensor observations, mass and hydraulic physics, temporal smoothness, and leak consistency. The two-stage estimation-localization architecture captures both slow and abrupt state changes [2509.10982].

These problem-specific factorizations afford the crucial ability to incorporate domain constraints and to leverage sparsity for algorithmic efficiency.

## 3. Robustness, Constraints, and Advanced Losses

Robust estimation in FGO is enabled via non-quadratic cost functions and adaptive loss weights, critical for applications facing outlier-prone data (GNSS, vision). The Geman–McClure estimator,
\[
\rho_{\mathrm{GM}}(r;\mu) = \frac{r^2}{r^2 + \mu^2}
\]
and its graduated non-convexity surrogate \(p_\omega(r)\), are used in GNSS positioning to globally downweight multipath-contaminated pseudorange factors, with the optimal weights iteratively updated by closed-form formulas derived via Black–Rangarajan duality [2109.00667].

Soft M-estimator models such as the Huber loss are integrated natively in major FGO libraries (GTSAM) and directly affect the residual weighting in normal equations [2502.08158]. Factor-graph-based robustification is essential for both outlier resistance and smoothing of estimation trajectories.

Equality constraints (algebraic, kinematic) are encoded using "hard" factors (zero-variance) or as residuals with infinite weights, supporting within-timestep and cross-timestep structure. Inequality constraints, classically absent from canonical FGO, have been addressed through (a) smooth penalties such as differentiable softmax-based terms for inter-body distance constraints in pedestrian inertial navigation [2505.08229], and (b) log-barrier factor nodes—embedding the interior-point penalty \(-\mu \ln(-g(x))\) into the factor graph—implemented for MPC and control contexts [2506.14341]. These extensions allow FGO to enforce state and input feasibility in large-scale optimal control and physical modeling tasks.

A general summary of constraint integration is presented below.

| Constraint type          | Encoding in FGO      | Representative Application         |
|-------------------------|----------------------|------------------------------------|
| Equality (hard)         | Delta/noninvertible  | LQR, periodicity, kinematic ties   |
| Equality (soft)         | Strong quadratic     | Kinematic/zero-velocity updates    |
| Inequality (soft)       | Smooth penalty/barrier | Step-length, actuator limits      |
| Inequality (hard)       | Barrier method/interior Pt. | MPC, path planning          |

## 4. Incremental, Sliding-Window, and Real-time Solvers

Factor-graph optimization is compatible with both full-batch and fixed-lag (sliding window) paradigms. Incremental methods (e.g., iSAM2) recursively update the Bayes-tree factorization under local problem changes, allowing real-time operation without sacrificing global estimation accuracy [2011.06194], [1904.03048]. Marginalization of variables leaving the window is performed via Schur complements, producing new Gaussian prior factors that preserve information.

These strategies enable plug-and-play sensor fusion with heterogeneous, asynchronous sensor streams (as in multi-sensor underwater navigation [2310.14163] and indoor localization [2503.12726]), while keeping computational resources bounded.

## 5. Modularity, Software Frameworks, and Implementation

Open-source FGO libraries have emerged to support flexible construction, prototyping, and deployment:
- **GTSAM:** Provides a C++/Python API for graph assembly, custom factors, manifold-aware optimization, sparse Cholesky/incremental solvers, and robustified loss models [2502.08158], [2011.06194]. Widely adopted in SLAM, GNSS, and sensor fusion.
- **miniSAM:** Offers a Python/NumPy front-end with configurable backends (CPU, multi-core, GPU), facilitating rapid definition of custom factor types and manifolds [1909.00903].
- **g2o/ecg2o/bipm_g2o:** Focuses on SLAM/perception, recently extended to support equality (ecg2o) [2503.01311] and inequality (bipm_g2o) [2506.14341] constraints, with plug-ins to assemble KKT/barrier-augmented systems.
- **Domain-specific packages:** For example, gtsam_gnss (for GNSS) [2502.08158], FGO-ILNS for AUV navigation [2310.14163], and FGLL for water leak localization [2509.10982].

Common to these architectures is a separation between the high-level graph/factor specification and the low-level sparse linear algebra, facilitating domain adaptation and composability.

## 6. Representative Applications and Empirical Impact

FGO is now standard in:
- **Autonomous navigation:** FGO enables robust GNSS positioning in multipath-rich environments, yielding 13–30% improvements in 2D RMS error over baseline FGO via graduated non-convexity robustification [2109.00667], lane-level accuracy with window carrier-phase constraints in urban canyons [2109.00683], and sub-meter smartphone fusion [2502.08158].
- **Mobile robotics:** Closed-loop quadruped navigation achieves a 76% reduction in absolute position error over classical state estimation, with significant drift reductions and no need for explicit contact modeling [1904.03048].
- **Dense visual estimation:** Factor-graph stereo optimization with adaptive, data-driven neighbor selection outperforms state-of-the-art MRF and deep learning methods on Middlebury benchmarks [2109.11077].
- **Control/MPC:** Equality- and inequality-constrained FGO frameworks enable direct optimization of control trajectories with analytical feedback synthesis, matching the scalability of Riccati recursions or interior-point solvers, while providing flexibility for general constraints [2011.01360], [2503.01311], [2506.14341].
- **Signal processing:** Machine learning methods have been used to differentiate through the factor-graph inference step itself, optimizing the structure (not just the parameters) of message-passing graphs for nearly-MAP symbol detection [2211.11406], or for end-to-end code design in belief-propagation decoders [2406.12900].

## 7. Limitations and Recent Extensions

Challenges remain in handling highly non-convex objectives, contact mode scheduling, large inequalities, and initialization sensitivity [2011.06194], [2506.14341]. Recent research has pushed the integration of constraint handling—via both equality (KKT augmentation [2503.01311], Gram-Schmidt elimination [2011.01360]) and inequality (log-barrier nodes [2506.14341], differentiable penalties [2505.08229])—and the emergence of differentiable FGO for machine-learned dense estimation and structural optimization [2105.08257], [2211.11406], [2406.12900].

Further directions include GPU-accelerated sparse optimization for very large graphs, plug-and-play incrementation for online estimation, learning-based factor/structure adaptation, and cross-domain graph composition, opening FGO to new applications in control, infrastructure monitoring, and autonomous systems.

Source: https://www.emergentmind.com/topics/factor-graph-optimization