Filter, window, or factor

Every real-time estimator has to decide what to do with the past. Keep every old state and the cost grows forever; throw them away carelessly and you lose information or invent it. Your two repos answer this differently, and so do FAST-LIO2 and VINS-Mono. This lesson gives you one tool, marginalization, which you'll do by hand on a 2×2 system, and one table to compare them all. It's a chance to defend a filter-vs-window choice at a whiteboard.

0 · Warm-up, from memory

From lesson 5. You'll see it again in section 3, applied to a whole state.

1 · The question every estimator answers

Strasdat, Montiel & Davison frame the choice in two sentences: "Filtering methods marginalise out past poses and summarise the information gained over time with a probability distribution. Keyframe methods retain the optimisation approach of global bundle adjustment, but computationally must select only a small number of past frames to process."Strasdat, Montiel & Davison, Visual SLAM: Why Filter?, Image and Vision Computing, 2012 (authors' PDF), abstract. Both are ways of forgetting. The difference is how, and that's where marginalization comes in.

2 · Marginalization, by hand

Write the normal equations in information form, split into a block \(m\) to remove and a block \(k\) to keep:

Assume a symmetric positive definite information matrix and a linear Gaussian model (or a fixed local linearization). Solve the top row for \(\mathbf{x}_m\) and substitute it into the bottom row. That's the Schur complement:

Solving \(\mathbf{H}'\mathbf{x}_k = \mathbf{b}'\) gives exactly the same \(\mathbf{x}_k\) as the full system. \(\mathbf{H}'\) is the prior the survivors carry: glass-core calls it "the EXACT summary of every factor that touched the marginalized variable".marginalization.hpp, lines 9–57, which solves against \(\mathbf{H}_{mm}\) with LDLT rather than inverting it. Try it on the smallest case, one scalar each:

• Full solve: \(\mathbf{x} = \mathbf{H}^{-1}\mathbf{b} = \tfrac{1}{8}\begin{bmatrix}3 & -2\\ -2 & 4\end{bmatrix}\begin{bmatrix}2\\ 1\end{bmatrix} = \begin{bmatrix}0.5\\ 0\end{bmatrix}\), so \(x_k = 0\).
• Marginalize \(m\): \(H' = 3 - 2\cdot\tfrac{1}{4}\cdot 2 = 2\) and \(b' = 1 - 2\cdot\tfrac{1}{4}\cdot 2 = 0\), so \(x_k = 0\). Exact.
• Hold \(x_m=0\) fixed and ignore its uncertainty: \(3x_k = 1\), so \(x_k = \tfrac{1}{3}\). Wrong, and with information 3 instead of 2: overconfident.

glass-core's test checks exactly these two claims on a 12-variable system: the Schur solve must match the full solve, and "hold-fixed (the freeze)" must not.test_marginalization.cpp, lines 43–67: match to 1e-9, and the naive solve must be off by more than 1e-2, "or the information the dropped block carried forward was zero". One cost to know: \(\mathbf{H}_{km}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mk}\) couples everything the removed variable touched, so the prior can become dense. In a nonlinear problem this summary is exact only for the chosen linearization; discarded nonlinear factors cannot subsequently be relinearized.

H_marg = Hkk - Hmk.transpose() * Hmm_inv_Hmk;
b_marg = bk - Hmk.transpose() * Hmm_inv_bm;

3 · Four answers to the question

The filter. Propagate \(\mathbf{P}\leftarrow\mathbf{F}\mathbf{P}\mathbf{F}^{\top}+\mathbf{G}\mathbf{Q}\mathbf{G}^{\top}\) across the IMU interval, the warm-up's shape, then fold in the measurement. The past is marginalized as you go, which is why a filter is cheap. It can never go back and re-linearise an old state. FAST-LIO2 is "a highly efficient tightly-coupled iterated Kalman filter", which re-linearises only the current state, a few times per update.Xu et al., FAST-LIO2, abstract.

The window. VINS-Mono keeps a "window of recent states by marginalizing out past states", turning the measurements of each removed state "into a prior".Qin, Li & Shen, VINS-Mono, IEEE T-RO 2018: the introduction, and the Marginalization subsection with its Fig. 7. the linked glass-vio version simply pops the oldest keyframe, keeping the anchor fixed.keyframe_window.cpp, lines 92–98; the design comparison is in glass-vio's doc/08-sliding-window.md, §6, whose "Stage B" is marginalization, with the warning "a wrong marginalization injects spurious information… Do not start here."

kfs_.push_back(std::move(kf));
while (static_cast<int>(kfs_.size()) > p_.max_keyframes + 1) {
  kfs_.pop_front();
}

The current-state factor solve. A factor is a term in an objective, not a mutually exclusive alternative to a filter or smoother. glass-lio optimizes the new state and gravity, inflates the IMU covariance by the previous state's uncertainty, and carries selected covariance blocks. Not revisiting old states does not by itself distinguish it from a filter. The important question is which joint distribution and correlations its prior represents.

ORB-SLAM3 also should not be described as a FIFO window that simply forgets old keyframes: it reuses covisible keyframes and maintains maps beyond the local optimization window. See the ORB-SLAM3 paper. The table describes the linked glass-vio implementation only.

Extracting covariance without inventing information

Let \(H_{\rm total}=H_{\rm prior}+H_{\rm data}\) include all four residual blocks. For the resulting nonsingular local Gaussian approximation,

Here \(b\) labels bias variables and \(o\) all other variables. The block \(H_{bb}^{-1}\) is the covariance conditional on the other variables being fixed, not their marginal covariance. Our toy system makes the difference visible: for the first variable, the marginal variance is \(3/8\), while the conditional variance is \(1/4\).

An earlier implementation added \(P_b^{-1}\) to \(H_{bb}\) even though that block already contained the bias prior, counting the prior twice. With a unit prior and no new information, that would halve the variance on every update. The revised implementation solves \(H_{\rm total}P=I\) once and extracts the nav and bias blocks from \(P\), including their coupling to gravity in this solve.

Eigen::Matrix<double, kTightDim, kTightDim> posterior;
if (r.posteriorCovariance(posterior)) {
  nav_cov_ = posterior.topLeftCorner<kNavDim, kNavDim>();
  bias_cov_ = posterior.block<6, 6>(kIdxBg, kIdxBg);
}

Read the current covariance extraction and caller; older commit-pinned links elsewhere document the historical implementation.

This is still a local covariance estimate at the last linearization and robust weights, not a complete Bayesian filter. The estimator carries selected blocks, uses a fixed gravity prior, and separately approximates previous-state and bias uncertainty; cross-factor correlations are not all preserved. Correct block extraction fixes these two algebraic errors, not every possible source of inconsistency.

4 · So which?

For visual SLAM, Strasdat et al. conclude that "keyframe bundle adjustment outperforms filtering, since it gives the most accuracy per unit of computing time", because "it is usually more profitable to increase the number of features than the number of frames."Strasdat et al., abstract and §8, Conclusion. But FAST-LIO2 is a filter, and it's the system glass-lio benchmarks itself against. The honest interview answer names the trade: a filter is cheap and never revisits; a window re-linearises recent history at a bounded cost; and whichever you pick, the uncertain states you remove should be marginalized if you intend to preserve their information. Dropping measurements loses information; fixing an uncertain state can invent certainty. Fixing a gauge to define the coordinate frame is a separate operation.

5 · Practice

From memory, no scrolling. The last question comes from lesson 3.

Apply it: complete the practical worksheet exercise before moving on. Derive an answer and test it on the supplied inputs.