When a robot navigates an unknown environment, it must simultaneously build a map of its surroundings and determine its location within that map. Historically, the most accurate way to solve this Simultaneous Localization and Mapping (SLAM) problem was through batch optimization (or global bundle adjustment), where the system jointly optimizes every camera pose and 3D landmark ever observed.
However, batch optimization hits a computational wall. As the robot continues to move, the number of states grows indefinitely, causing the computational cost to scale quadratically or cubically. For real-time systems, this is unacceptable.
To maintain a constant computational profile, we must bound the optimization problem. The most intuitive way to do this is through a sliding window or fixed-lag smoother, which restricts the active optimization to a small subset of recent states. But simply discarding old data creates a new problem: it throws away historical constraints, causing the system to rapidly drift and lose its sense of scale.
The elegant solution to this dilemma is probabilistic marginalization. Instead of forgetting the past, we mathematically distill the information from older states into a compact "prior factor" that constrains the remaining active states.
The Computational Wall and the Naive Window
To understand why marginalization is necessary, consider a standard feature-based visual odometry pipeline. As shown by Mouragnon et al., optimizing a small sliding window of recent camera poses and 3D points provides sufficient local consistency to suppress significant drift. This localized trajectory optimization keeps the optimization backend fast enough for real-time deployment (Paladini et al.; Han et al.).
However, if we implement a naive sliding window by simply dropping variables outside the window (a first-in-first-out strategy), we sever the geometric links connecting our current position to the historical map. For example, if an early frame established the absolute direction of gravity or the metric scale of the environment, dropping that frame entirely allows the current trajectory to bend and drift freely.
To preserve these crucial historical constraints without keeping the old states in our active optimization matrix, we need a way to "bake" their influence into the remaining variables.
The Schur Complement as a Mathematical Memory
Probabilistic marginalization achieves this compression using a classic linear algebra tool: the Schur complement.
In state estimation, we typically solve a non-linear least-squares problem by iteratively solving a linearized system of normal equations:
$$H \Delta x = g$$
Where $H$ is the Hessian matrix (representing the information or certainty we have about our states), $\Delta x$ is the update vector, and $g$ is the residual vector.
When a state is ready to exit our active sliding window, we can partition our state vector $\Delta x$ into two parts: the old states we want to eliminate ($\Delta x_o$) and the active states we want to keep ($\Delta x_a$). This allows us to rewrite the system as a block matrix equation:
$$\begin{bmatrix} H_{oo} & H_{oa} \\ H_{ao} & H_{aa} \end{bmatrix} \begin{bmatrix} \Delta x_o \\ \Delta x_a \end{bmatrix} = \begin{bmatrix} g_o \\ g_a \end{bmatrix}$$
Because we want to eliminate $\Delta x_o$, we can solve the top row for $\Delta x_o$:
$$\Delta x_o = H_{oo}^{-1} (g_o - H_{oa} \Delta x_a)$$
Substituting this into the bottom row yields an equation that depends only on our active states $\Delta x_a$:
$$(H_{aa} - H_{ao} H_{oo}^{-1} H_{oa}) \Delta x_a = g_a - H_{ao} H_{oo}^{-1} g_o$$
This transformation is the Schur complement. Notice what happened to the active Hessian $H_{aa}$: it has been modified by the term $- H_{ao} H_{oo}^{-1} H_{oa}$.
This subtraction does not mean we have lost information. Rather, it projects the information from the old states directly onto the active states. As Wang et al. demonstrate in Direct Sparse Odometry (DSO), this mechanism ensures that the historical influence of marginalized variables is perfectly preserved as a linear prior factor in the active optimization window.
By using this approach, multi-sensor systems can marginalize out past states while retaining critical cross-sensor constraints, such as gravity vectors and inertial biases (Lv et al.; Civera et al.).
[ Old States (xo) ] ---- (Connected via measurements) ----> [ Active Window (xa) ]
| |
v (Apply Schur Complement) v
[ Marginalized Out ] -------------------------------------> [ Modified Prior Factor ]
(Past info baked into xa)
The Danger of Chronology: Flexible Marginalization
The math of the Schur complement assumes that the states we are removing are the oldest ones chronologically. However, a strict first-in-first-out (FIFO) marginalization policy can cause catastrophic failures in real-world scenarios.
Imagine a robot that stops moving. If it continues to capture camera frames while stationary, a chronological sliding window will quickly fill up with identical, redundant poses. If we marginalize out the older frames simply because they are chronologically older, we will discard the frames that actually had spatial separation (parallax). Without parallax, the system can no longer triangulate 3D points, causing the metric scale of the system to collapse.
To prevent this, modern VIO and SLAM systems employ flexible or two-way marginalization schemes (Xu et al.; Zhang et al.). Instead of removing frames based purely on time, the system evaluates the information content of the frames:
- If the newest frame captures significant new motion (high parallax): The oldest frame in the window is marginalized out, committing its long-term geometric constraints to the prior.
- If the newest frame is redundant (low parallax): The system skips marginalizing the old keyframe. Instead, it marginalizes out the recent redundant frame or drops its visual measurements while keeping its inertial data (Xu et al.).
As Zhang et al. note, this flexible strategy preserves the system's sense of scale during degenerate motions, such as when a sensor is stationary or moving at a constant velocity. Similarly, the single-author work by Li leverages parallax thresholds to remove redundant frames, ensuring the optimization backend focuses exclusively on the most informative spatial data.
The Catch: Linearization and Inconsistency
If marginalization is mathematically exact, why doesn't a fixed-lag smoother perform exactly as well as a full batch optimizer? The catch lies in the non-linearity of the physical world.
The Schur complement requires us to linearize our non-linear measurement equations around our current best estimate of the states. Once a state $\Delta x_o$ is marginalized out, it is gone forever. This means we can never change its linearization point again.
As the remaining active states continue to be optimized in subsequent frames, their estimates will shift. However, the prior factor generated by marginalization remains locked to the old, frozen linearization point of the marginalized states. This mismatch introduces spurious information into the system—the estimator becomes "overconfident," mistakenly believing it has more certain information than it actually does.
To combat this estimator inconsistency, researchers have developed specialized mathematical frameworks. For instance, Huai et al. proposed a Right-Invariant Fixed-Lag Smoother (RI-FLS). By formulating the system using Lie groups and invariant error dynamics, the authors ensure that the linearization points of the system do not depend on the choice of the world coordinate frame, preventing the catastrophic divergence common in standard sliding-window filters.
Broad Applications: From Splines to Non-Rigid Shapes
While sliding-window marginalization was popularized in visual and inertial SLAM (Shan et al.; Li et al.), its core mechanism of bounding complexity while preserving history has spread across state estimation:
- Continuous-Time Trajectories: Instead of optimizing discrete poses at specific timestamps, modern multi-sensor systems often represent trajectories as continuous splines. In these setups, marginalization involves removing outdated control points from the active window while transferring their information to the remaining trajectory segments (Lang et al.; Wu et al.).
- Non-Rigid Structure-from-Motion (NRSfM): When tracking deforming objects (like a moving cloth or a human face), the state space includes both camera poses and deformation coefficients. Agudo et al. and Agudo et al. demonstrated that a short-duration sliding window allows sequential 3D reconstruction of non-rigid shapes with a constant computational cost per frame.
- Delayed Data Association: In dynamic environments where objects move and data association is uncertain, the authors of Bibby et al. showed that a sliding window can act as a temporal buffer. By delaying marginalization, the system can change its mind about whether a landmark is stationary or dynamic before permanently committing it to the global map.
Ultimately, fixed-lag smoothing via marginalization works because it treats memory not as an all-or-nothing choice between keeping a variable or deleting it, but as a continuous process of mathematical distillation.
Key Foundational Work
Real Time Localization and 3D Reconstruction (2006) — contributed 2 concept elements to this direction.
Incremental Estimation of Discrete Hidden Markov Models Based on a New Backward Procedure (2005) — contributed 1 concept elements to this direction.