Factor-Graph Optimization

• Factor-graph optimization is a framework that represents high-dimensional estimation problems as sparse bipartite graphs using variable and factor nodes.
• It integrates diverse sensor modalities and robust loss functions, enhancing performance in GNSS, SLAM, and control applications.
• Incremental and sliding-window solvers, supported by modular open-source libraries, enable scalable, real-time implementations.

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 (Dong et al., 2019, Suzuki, 12 Feb 2025, Xie et al., 2020).

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 (Xie et al., 2020).

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 (Suzuki, 12 Feb 2025, Wen et al., 2021, Bai et al., 2021).
• 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 (Wisth et al., 2019, Tang et al., 2021).
• 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 (Shabanian et al., 2021).
• 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 (Irofti et al., 13 Sep 2025).

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 (Wen et al., 2021).

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 (Suzuki, 12 Feb 2025). 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 (Hu et al., 13 May 2025), 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 (Abdelkarim et al., 17 Jun 2025). 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 (Xie et al., 2020, Wisth et al., 2019). 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 (Song et al., 2023) and indoor localization (Zhang et al., 17 Mar 2025)), 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 (Suzuki, 12 Feb 2025, Xie et al., 2020). 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 (Dong et al., 2019).
• g2o/ecg2o/bipm_g2o: Focuses on SLAM/perception, recently extended to support equality (ecg2o) (Abdelkarim et al., 3 Mar 2025) and inequality (bipm_g2o) (Abdelkarim et al., 17 Jun 2025) constraints, with plug-ins to assemble KKT/barrier-augmented systems.
• Domain-specific packages: For example, gtsam_gnss (for GNSS) (Suzuki, 12 Feb 2025), FGO-ILNS for AUV navigation (Song et al., 2023), and FGLL for water leak localization (Irofti et al., 13 Sep 2025).

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 (Wen et al., 2021), lane-level accuracy with window carrier-phase constraints in urban canyons (Bai et al., 2021), and sub-meter smartphone fusion (Suzuki, 12 Feb 2025).
• 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 (Wisth et al., 2019).
• 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 (Shabanian et al., 2021).
• 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 (Yang et al., 2020, Abdelkarim et al., 3 Mar 2025, Abdelkarim et al., 17 Jun 2025).
• 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 (Rapp et al., 2022), or for end-to-end code design in belief-propagation decoders (Choukroun et al., 2024).

7. Limitations and Recent Extensions

Challenges remain in handling highly non-convex objectives, contact mode scheduling, large inequalities, and initialization sensitivity (Xie et al., 2020, Abdelkarim et al., 17 Jun 2025). Recent research has pushed the integration of constraint handling—via both equality (KKT augmentation (Abdelkarim et al., 3 Mar 2025), Gram-Schmidt elimination (Yang et al., 2020)) and inequality (log-barrier nodes (Abdelkarim et al., 17 Jun 2025), differentiable penalties (Hu et al., 13 May 2025))—and the emergence of differentiable FGO for machine-learned dense estimation and structural optimization (Yi et al., 2021, Rapp et al., 2022, Choukroun et al., 2024).

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.