Incremental Factor Graph Optimization with Bayes Trees

20 concepts15 papers1 proposal

When a robot navigates an unknown environment, it faces a fundamental dilemma: how to maintain an accurate estimate of its entire trajectory without grinding to a computational halt.

Historically, roboticists chose between two compromises. They could use filtering (like the Extended Kalman Filter), which updates instantly but discards past history, making it prone to permanent linearization errors and drift. Alternatively, they could use batch optimization (smoothing), which retains the entire history to ensure global consistency but quickly becomes too slow for real-time operation because solving a massive optimization problem from scratch at every time step scales poorly.

The breakthrough that bridges this gap is incremental factor graph optimization. By re-framing trajectory estimation as a graphical problem and exploiting its underlying mathematical structure, we can achieve the global accuracy of full smoothing at the real-time speed of filtering.

The Factor Graph and Matrix Sparsity

To understand why this approach works, we must first look at how the estimation problem is structured. Instead of tracking just the current state, we can represent the robot's entire history as a factor graph—a bipartite graph containing variable nodes (the robot's poses at different times) and factor nodes (probabilistic constraints from sensor measurements).

As shown by Christian Förster et al. in On-Manifold Preintegration for Real-Time Visual-Inertial Odometry, representing state estimation this way maintains a sparse information matrix. Finding the most likely trajectory means computing the Maximum A Posteriori (MAP) estimate, which translates to solving a non-linear least-squares problem. We linearize this problem into a linear system:

$$Ax = b$$

where $A$ is a large matrix representing the constraints between states. Because a robot only observes nearby landmarks or receives sequential odometry measurements at any given moment, each factor only connects a few variables. Consequently, the matrix $A$ (and the information matrix $A^T A$) is highly sparse.

To solve this system, we typically perform matrix factorization, such as Cholesky or QR decomposition, to compute an upper triangular matrix $R$. In a traditional batch solver, any new measurement requires re-linearizing the entire graph and re-factoring this massive matrix from scratch—an operation that becomes prohibitively expensive over long missions.

The Core Insight: From Graphs to Bayes Trees

The magic of incremental optimization lies in a powerful connection between linear algebra and graph theory: matrix factorization is mathematically equivalent to variable elimination on a graph.

When we eliminate variables from a factor graph one by one to solve the linear system, we turn the graph into a chordal graph. This chordal graph can be uniquely organized into a directed tree structure called a Bayes Tree. In this tree, each node represents a "clique" of tightly coupled variables from the trajectory.

This tree structure completely changes how we handle new data. When a robot takes a new step or observes a landmark, it does not alter the entire history of the world; it merely adds a new factor to a few local variables.

As Xinyan Yan et al. demonstrate in Incremental Sparse GP Regression for Continuous-time Trajectory Estimation & Mapping, the Bayes tree isolates these changes. A new measurement only affects the cliques containing those specific variables. To update our global estimate, we do not need to re-factor the entire matrix. Instead, we only need to perform just-in-time relinearization and re-elimination along the path from the affected cliques up to the root of the tree.

The rest of the tree—representing parts of the trajectory and map completely unaffected by the new data—remains untouched. By recycling these previous computations, the computational cost remains proportional to the size of the new data rather than the total history of the mission. Thomas Läbe et al. leverage this exact mechanism in INCREMENTAL REAL-TIME BUNDLE ADJUSTMENT FOR MULTI-CAMERA SYSTEMS WITH POINTS AT INFINITY to execute real-time bundle adjustment, updating only the variables altered by new visual observations while maintaining a sparse representation of the global map.

Visualizing the Intuition

Imagine a robot driving down a long corridor. As it moves forward, new poses are added sequentially. In the Bayes tree, these new poses simply append new nodes to the bottom or sides of the tree. The path to the root is short, meaning updates take a fraction of a millisecond.

What happens when the robot recognizes a room it visited an hour ago? This is a loop closure. In the factor graph, a new edge connects the current pose to a very old pose. In the Bayes tree, this edge bridges two distant branches. When we re-eliminate this loop, the affected path to the root widens, propagating the correction through the historical trajectory.

Yet, even during loop closures, large portions of the tree remain completely unaffected. This sparse linear algebra approach ensures that the entire historical path stays globally consistent and corrects accumulated drift without the overhead of full batch optimization, a property highlighted by Donghwa Lee et al. in Image Feature-Based Real-Time RGB-D 3D SLAM with GPU Acceleration.

Extending the Paradigm

Because tree-based incremental optimization is a general mathematical framework, it has been adapted to handle complex, non-traditional estimation challenges across robotics.

Continuous-Time Trajectories

Traditional SLAM discretizes a trajectory into distinct poses at specific timestamps. However, high-frequency or asynchronous sensors do not align perfectly in time. By combining incremental factor graphs with continuous-time trajectory representations—such as Gaussian Process regression—the trajectory can be queried at any arbitrary timestamp.

Mustafa Mukadam et al. extend this concept to planning in STEAP: simultaneous trajectory estimation and planning, treating both a robot's past trajectory and its future intended path as a single continuous-time factor graph. When the robot senses a new obstacle, the Bayes tree allows it to update only the specific segments of the future plan affected by the environmental change. To make these continuous-time optimizations fast enough for real-time multi-sensor systems, Jiajun Lv et al. show in Continuous-Time Fixed-Lag Smoothing for LiDAR-Inertial-Camera SLAM that deriving analytical Jacobians drastically accelerates convergence within the factor graph.

Asynchronous and Event-Based Sensing

Event cameras fire millions of asynchronous pixel-level intensity changes per second, breaking traditional frame-based architectures. Incremental optimization is uniquely suited for this data.

In Asynchronous Optimisation for Event-based Visual Odometry, Daqi Liu et al. utilize Bayes trees to perform incremental MAP estimation directly on individual events without batching. Taking this a step further, Jun Nagata demonstrates in Tangentially Elongated Gaussian Belief Propagation for Event-Based Incremental Optical Flow Estimation that executing incremental belief propagation on sparse factor graphs allows the computational load to scale strictly with the activity level of the scene rather than the total pixel count, preserving the low-latency benefits of neuromorphic sensors.

Extreme Environments and Hybrid Problems

The efficiency of localized tree updates has enabled high-precision navigation in highly constrained or extreme environments:

The paradigm even handles problems containing discrete ambiguities—such as deciding whether a sensor measurement is an outlier. Ziqi Lu et al. introduce a hybrid framework in Discrete-Continuous Smoothing and Mapping, where an incremental continuous solver isolates updates to the affected sub-graphs while alternating with a discrete solver to resolve data association. Furthermore, Moshe Shienman et al. demonstrate in A Slices Perspective for Incremental Nonparametric Inference in High Dimensional State Spaces that even complex, nonparametric probability distributions can be updated incrementally by isolating re-sampling and re-elimination to localized sub-graphs within a Bayes tree.

Ultimately, whether a robot is tracking a pedestrian via smartphone signals, navigating around an asteroid, or planning an obstacle-free path in real-time, the core mechanism remains the same. By mapping sparse factor graphs to tree-based data structures, we bypass the need to re-solve the past, unlocking the power of global smoothing at an incremental cost.

Key Foundational Work

INCREMENTAL REAL-TIME BUNDLE ADJUSTMENT FOR MULTI-CAMERA SYSTEMS WITH POINTS AT INFINITY (2013) — contributed 2 concept elements to this direction.
Incremental Sparse GP Regression for Continuous-time Trajectory Estimation & Mapping (2015) — contributed 2 concept elements to this direction.
Image Feature-Based Real-Time RGB-D 3D SLAM with GPU Acceleration (2013) — contributed 2 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_incremental_factor_graph_optimization_411dad74,
  title = {incremental factor graph optimization},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/incremental-factor-graph-optimization/rdr_411dad7481f245949bc52960131d0585},
  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_incremental_factor_graph_optimization_411dad74_v1,
  title = {incremental factor graph optimization},
  author = {{Lacuna}},
  howpublished = {Lacuna research direction},
  year = {2026},
  url = {https://lacuna.tiptreesystems.com/direction/incremental-factor-graph-optimization/rdr_411dad7481f245949bc52960131d0585/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