Factor Graph Optimization

The sparse bundle adjustment (BA) problem

Sparsity of the Hessian structure

Hessian sparsity introduction

Schur Complement

Pose graph optimization introduction

Sliding Window Filter and Optimization

Why Control BA Size?

• Sliding window: keep the $K$ most recent keyframes; discard older ones.
• Covisibility graph (as in ORB-SLAM2): keep $C$ keyframes that share feature observations with the current frame.

Sliding Window Formulation

Suppose our sliding window contains $M$ keyframe poses in Lie algebra form and $N$ landmarks:

The full state vector has dimension $6M + 3N$:

After BA converges, the cost is locally quadratic, so the posterior is approximately Gaussian with information (Hessian) matrix:

where $\mathbf{H}_{PP}$ is block-diagonal (landmarks are conditionally independent). Applying the Schur complement to eliminate landmarks:

Why does $\mathbf{S}^{-1}$ give the covariance? Recall that a Gaussian can be written in two equivalent forms:

where $\boldsymbol{\Lambda} = \boldsymbol{\Sigma}^{-1}$ is the information (precision) matrix. At the BA optimum, the cost is locally quadratic:

Interpreting $\exp(-f)$ as a probability, this is exactly a Gaussian with $\boldsymbol{\Lambda} = \mathbf{H}$, so $\boldsymbol{\Sigma} = \mathbf{H}^{-1}$. After the Schur complement eliminates landmarks, $\mathbf{S}$ plays the role of $\mathbf{H}$ over poses only — it is the reduced information matrix, and $\mathbf{S}^{-1}$ is the pose covariance:

Intuitively, $\mathbf{S}$ encodes how tightly each pose is constrained. Two poses co-observing many landmarks will have large off-diagonal blocks in $\mathbf{S}$ — moving one forces the other to adjust.

Window update

When the window moves:

• New keyframe $\boldsymbol{\xi}_{M+1}$ arrives: append to $\mathbf{\mathcal{X}}$, re-run BA. Nothing special.
• Old keyframe $\boldsymbol{\xi}_1$ must go: two choices — \begin{equation} \text{Delete:} \quad \mathbf{S}_{\text{new}} = \mathbf{S}_{2:M,\;2:M} \qquad \text{(drop rows/cols — loses information)} \end{equation} \begin{equation} \text{Marginalize:} \quad \mathbf{S}_{\text{new}} = \mathbf{S}_{2:M,\;2:M} - \mathbf{S}_{2:M,\;1}\,\mathbf{S}_{1,1}^{-1}\,\mathbf{S}_{1,\;2:M} \qquad \text{(preserves info — causes fill-in)} \end{equation} Deletion throws away everything $\boldsymbol{\xi}_1$ knew. Marginalization conditions on $\boldsymbol{\xi}_1$'s estimate and propagates its constraints forward — but the Schur complement couples previously independent variables.

Adding a keyframe is easy; removing one forces a choice between information loss and structural damage (fill-in).

The Fill-In Problem

Maintaining Sparsity: Three Cases

• Landmark seen only by $\boldsymbol{\xi}_1$: it's isolated---just delete it. No impact on anything.
• Landmark seen in $\boldsymbol{\xi}_2$--$\boldsymbol{\xi}_4$ but will not reappear: marginalize both $\boldsymbol{\xi}_1$ and this landmark. The result is a prior on the pose--pose block, which is fine for sparsity.
• Landmark seen in $\boldsymbol{\xi}_2$--$\boldsymbol{\xi}_4$ and may reappear: we cannot marginalize it (we'll need to update it later). Engineering fix: pretend $\boldsymbol{\xi}_1$ never saw it---just drop that observation. This costs a tiny bit of information but keeps the landmark block diagonal.

Intuition for marginalization in SWF

What Is a Pose Graph?

Residuals and Jacobians

Overall Objective

References

1. Visual SLAM: From Theory to Practice, by Xiang Gao, Tao Zhang, Qinrui Yan and Yi Liu

@article{Hu2026learnvslam,
  author = {Hu, Yafei},
  title = {Learning Visual SLAM: Pose Graph Optimization},
  year = {2026}
}