Tightly Coupled Multi Sensor Factor Graph Optimization

44 concepts35 papers1 proposal

When a robot navigates the physical world, it relies on a heterogeneous suite of sensors: cameras capture rich visual scenes, IMUs track rapid accelerations, LiDARs measure precise geometric structures, and GNSS receivers provide global positioning. Historically, roboticists fused these streams using cascaded or "loosely coupled" filters. In a loosely coupled system, each sensor is processed by an isolated estimator, and the resulting high-level outputs (such as a position estimate from a GPS module and a trajectory from a visual odometry pipeline) are combined.

However, as noted by Silvio Giancola et al., loosely coupled strategies fail to capture the full statistical relationships between different data streams. If a single sensor degrades—such as a camera experiencing a sudden lighting change or a GNSS receiver losing sight of most satellites—the isolated estimator fails, dragging down the entire system.

To overcome this, modern state estimation has shifted toward tightly coupled factor graph optimization. Instead of fusing processed outputs, this paradigm treats all raw sensor measurements as simultaneous probabilistic constraints within a single, unified optimization problem.


The Architecture of a Factor Graph

At its core, a factor graph is a bipartite graphical model containing two types of nodes: variable nodes and factor nodes.

  1. Variable Nodes represent the unknown states we want to estimate over time, such as the robot's 3D pose (position and orientation), velocity, and internal sensor biases (like IMU drift).
  2. Factor Nodes represent probabilistic constraints or measurements. Every time a sensor records data, it adds a factor node connected to the relevant variable nodes. The factor defines an error function (or residual) based on the difference between the actual sensor measurement and what the robot should have observed given the current state estimate.
[ State X₁ ] -------- ( IMU Pre-integration Factor ) -------- [ State X₂ ]
     |                                                             |
(Visual Factor)                                             (Visual Factor)
     |                                                             |
[ Landmark L₁ ] -------------------------------------------- [ Landmark L₂ ]

As formulated by Jing Xu et al., this bipartite structure allows the system to model multi-sensor fusion without requiring explicit, rigid time-alignment or complex time-registration steps. Because measurements are treated simply as independent factors tied to specific states, asynchronous and multi-rate data streams can be seamlessly integrated into the same graph.

To find the optimal trajectory, a non-linear solver adjusts the variable nodes to minimize the sum of all error residuals across the entire graph. In a survey of the field, Ayoung Kim highlights that this joint optimization directly correlates raw sensor observations in a unified mathematical model, providing superior robustness during high-acceleration maneuvers.


Why Tight Coupling Built on Factor Graphs Works

The primary advantage of tightly coupled factor graphs is their ability to leverage cross-modal synergies to maintain accuracy when individual sensors degrade.

1. Simultaneous State and Bias Estimation

High-frequency sensors like IMUs are excellent for tracking rapid, aggressive movements, but they suffer from dead-reckoning drift due to underlying sensor biases. In a tightly coupled graph, such as the frameworks designed by Tixiao Shan et al. (LIO-SAM) and Thien-Minh Nguyen et al. (MILIOM), IMU pre-integration measurements and exteroceptive features (like LiDAR edges or planes) are optimized jointly within a sliding window of recent states.

Because the optimization runs globally across all factors, a stable visual or LiDAR feature acts as an anchor that bounds the drift of the IMU. Conversely, during rapid rotations where visual tracking might fail, the IMU factors provide immediate constraints that keep the visual state estimation from diverging, a synergy explicitly demonstrated in the direct visual-inertial odometry framework of Wenju Xu et al..

2. Exploiting Sub-Threshold Raw Measurements

In traditional systems, a GNSS receiver requires a line of sight to at least four satellites to compute a 3D position fix. If a vehicle drives through an urban canyon and only sees two satellites, a loosely coupled system discards the data entirely.

Tightly coupled factor graphs bypass this limitation by integrating raw measurements directly. As shown in the GVINS framework by Shaozu Cao et al. and the agricultural SLAM system by Javier Civera et al., raw GNSS pseudoranges and Doppler shifts are injected as individual factor nodes. Even if only one or two satellites are visible, those individual signals still provide valid geometric constraints that pull the optimization toward global consistency, preventing long-term drift without needing a full GNSS position fix.

Similarly, Muqing Cao et al. (LIRO) demonstrated that raw Ultra-Wideband (UWB) ranging factors can be combined with LiDAR and IMU residuals in a single cost function, allowing a handful of easily deployable anchors to eliminate long-term drift in feature-poor environments. Even standard, low-cost GPS data can be integrated as a unary constraint in a pose graph to achieve centimeter-level accuracy when fused properly, as shown by Linwei Zheng et al..


Expanding the Graph: Physics, Geometry, and Multi-Modal Alignment

Because a factor graph is fundamentally a framework for solving joint probability distributions, it is not limited to standard sensor streams. Any domain knowledge that can be expressed as a mathematical residual can be added as a factor node.

Incorporating Physical and Structural Priors

In industrial and construction environments, robots face unique physical challenges like structural flexing or mechanical backlash. Julien Kindle et al. demonstrated that physics-informed models of structural deflection and joint backlash can be integrated directly into a factor graph alongside high-accuracy accelerometers. By treating mechanical deformation as a probabilistic constraint, the framework slashes positioning errors by half on uneven surfaces.

Environmental geometry can also serve as a factor. In indoor environments, Danpeng Chen et al. (VIP-SLAM) introduced homography constraints from planar structures (like walls and floors) into the joint cost function. This structural prior allowed them to analytically remove thousands of individual point features from the optimization backend, doubling the speed of global bundle adjustment while maintaining high precision.

Cross-Modal and Multi-Pass Alignment

The flexibility of factor graphs also makes them ideal for cross-modality alignment and mapping. For instance, Shifan Zhu et al. utilized a non-linear optimization framework to jointly align visual maps and images derived from highly disparate modalities—RGB-D and event cameras—to track agile legged robot maneuvers.

For large-scale mapping and novel view synthesis, Ziqian Ni designed a two-phase pose optimization framework that aligns multi-pass sensor data. The system refines camera poses by constructing a factor graph that simultaneously optimizes structure-from-motion tracks, LiDAR depth priors, and photometric alignment metrics across different recording sessions.


Computational Efficiency and the Horizon

If factor graphs optimize over all historical states and measurements, why don't they grind to a halt as the robot moves? The answer lies in sparse linear algebra and graph management techniques.

In practice, systems use a sliding window approach, optimizing only the most recent states while summarizing older history into a fixed geometric prior through a process called marginalization. The authors of the Multi-sensor Fusion SLAM from the Nadir View for UAV Localization and Mapping framework leverage this backend optimization to continuously correct IMU bias and visual scale in real-time by bounding the optimization window.

Furthermore, hardware-aware optimizations can drastically reduce overhead. The authors of CLOi-Mapper showed that by representing rigidly synchronized sensors as fixed relative transformations with near-zero covariance within the pose graph, they could eliminate high-frequency synchronization logic entirely. This allowed them to deploy a robust multi-sensor fusion pipeline on low-power embedded systems with strict memory and processing constraints.

By shifting from isolated filters to unified, tightly coupled factor graphs, modern robotics achieves a level of state estimation that mirrors human perception: individual senses may be imperfect, but when bound together by the laws of geometry and physics in a single optimization framework, they provide a remarkably stable and resilient window into reality.

Key Foundational Work

Direct Visual-Inertial Odometry with Semi-Dense Mapping (2019) — contributed 1 concept elements to this direction.
Low-Cost GPS-Aided LiDAR State Estimation and Map Building (2019) — contributed 1 concept elements to this direction.
Kalman Filter, Sensor Fusion, and Constrained Regression: Equivalences and Insights (2019) — contributed 1 concept elements to this direction.
History & citationVersion 1 · 2026-10-04

Current · Permanent link

View frozen version 1

Cite this direction

Latest version (a living page):

@misc{lacuna_factor_graph_optimization_f3cf9a83,
  title = {factor graph optimization},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/factor-graph-optimization/rdr_f3cf9a830ac74fc188a5f30c9e9aea58},
  note = {Latest version of a living research direction; cite a version for a fixed text. Accessed 2026-11-20},
}

This exact version (version 1, frozen text):

@misc{lacuna_factor_graph_optimization_f3cf9a83_v1,
  title = {factor graph optimization},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/factor-graph-optimization/rdr_f3cf9a830ac74fc188a5f30c9e9aea58/v/1},
  note = {Version 1, frozen 2026-10-04. Accessed 2026-11-20},
}
About

These directions overlap with this one through shared papers.

See something wrong? Report a mistake