Skip to main content
Contents5 / 6 ▾
  1. ~Cover & contents
  2. IThe problem, before any algorithm
  3. IIThe Kalman filter: the exact case
  4. IIIThe EKF: pretend it is a straight line
  5. IVThe UKF: do not differentiate, sample
  6. VFactor graphs: keep everything, solve it all
  7. VIThe three compared, and unified
V

Chapter five Factor graphs: keep everything, solve it all

A filter is a machine that forgets. It compresses the entire past into one mean and one covariance, and once that compression has happened the information is gone for good. The factor graph refuses to compress. It writes down every unknown and every measurement as a graph, and treats estimation as one large optimisation problem rather than a recursion.

5.1 · The graph

A factor graph is a bipartite graph with two kinds of node. Variable nodes are the things you do not know: robot poses at each time, IMU biases, landmark positions, camera calibration. Factor nodes are the things you measured, and each one connects only to the variables it actually constrains.

The joint posterior over the whole trajectory \(X = \{x_0 \dots x_K\}\) factorises exactly along the graph:

\[ p(X \mid Z) \;\propto\; \prod_{i} \phi_i\!\left( X_i \right) \]
(5.1)

where \(X_i\) is the small subset of variables that factor \(i\) touches. Assume each factor is Gaussian in its residual, take the negative logarithm, and the product becomes a sum. Maximising the posterior turns into minimising a sum of squares:

\[ X^\star = \arg\min_{X} \sum_i \left\lVert h_i(X_i) - z_i \right\rVert^2_{\Sigma_i} \]
(5.2)

The notation \(\lVert e \rVert^2_{\Sigma} = e^\top \Sigma^{-1} e\) is the Mahalanobis norm — squared error, weighted by how much you trust that sensor. This single equation is the whole of modern SLAM, bundle adjustment and visual-inertial odometry.

5.2 · Solving it

Equation (5.2) is a nonlinear least-squares problem, so solve it the standard way. Linearise every residual about the current estimate, stack them into one tall sparse matrix \(A\) and one vector \(b\), and solve for a correction \(\delta\):

\[ \delta^\star = \arg\min_{\delta} \lVert A\delta - b \rVert^2 \quad \Longleftrightarrow \quad \left(A^\top A\right)\delta^\star = A^\top b \]
(5.3)
\[ X \leftarrow X \oplus \delta^\star, \qquad \text{repeat until } \lVert\delta^\star\rVert < \varepsilon \]
(5.4)

That is Gauss–Newton; adding a damping term \((A^\top A + \mu I)\) gives Levenberg–Marquardt, which is what you actually use in practice. \(A^\top A\) is the information matrix, and it is extremely sparse because each factor touches only a couple of variables. Sparse Cholesky or QR factorisation exploits that, which is the only reason solving for ten thousand poses at once is feasible at all.

FILTER — the past is marginalised away x₀ x₁ x₂ x₃ only this survives; memory is O(1) FACTOR GRAPH — everything is retained loop closure — also corrects x₀ x₀ x₁ x₂ x₃ prior GPS fix circles = unknowns squares = measurements
Figure 5.1 — The structural difference. A filter is a chain that consumes itself. A factor graph is a record that can be re-solved, which is why a loop closure detected now can retroactively fix a pose from five minutes ago.

5.3 · Paying for the memory

Keeping everything cannot be free, and three techniques make it practical.

Marginalisation. Drop old variables but keep their information as a dense prior on whatever they touched, via the Schur complement. Marginalise everything except the newest pose and you have — precisely — a filter.

Fixed-lag smoothing. Keep a sliding window of the last few seconds, marginalise anything older. This is the pragmatic middle ground, and what most production visual-inertial systems actually run.

Incremental solving. iSAM2 and the Bayes tree notice that a new measurement disturbs only a small part of the graph, and re-factorise only that part. You get near-batch accuracy at near-filter cost, which is why GTSAM-based systems can run in real time.

22