M4-T8 — Factor Graphs and Incremental Smoothing

Learning objectives

After this lesson, you should be able to:

• Draw the factor graph of a SLAM problem and write the least-squares problem it represents.
• Read the sparsity pattern of the information matrix from the graph, and predict the fill-in caused by eliminating or marginalizing a variable.
• Marginalize a variable with the Schur complement and say what the resulting prior connects.
• Explain what iSAM2 recomputes when a new factor arrives, and why a loop closure costs more than an odometry step.
• Say why marginal covariances are needed and why they are not read directly from the information matrix.

The problem and the intuition

Think of every measurement as a small note that says something about a few unknowns. “The robot moved one metre between pose 1 and pose 2.” “From pose 0 the landmark was 2.1 m ahead.” “Pose 0 is at the origin.” No note mentions more than a handful of unknowns. A factor graph pins each note (a square, the factor) to the unknowns it mentions (circles, the variables). That is the whole structure: a pose graph is the special case where every variable is a pose and every note joins two of them; bundle adjustment (M4-T7) is the case with poses, points and one note per image observation.

Two things follow from the drawing. First, the estimation problem is the product of all the notes, so the cost is a sum of small terms and its matrix is mostly zeros. Second, a new note touches only the variables it names and those whose estimate depends on them. If the solver is organised around the graph, most of yesterday’s work is still valid today.

A factor is one measurement. A variable is one unknown. Two variables are coupled in the matrix only if some factor touches both.

The algorithm: from factors to an incremental solver

Problem. Given measurements z_i, each depending on a small subset X_i of the unknowns X, find the most probable X, and keep that estimate current as measurements arrive.

Assumptions. Measurement errors are independent and Gaussian after whitening (robust factors of M4-T6 relax this); a usable initial guess exists; one prior or anchor removes the gauge freedom (M4-T4).

From factors to least squares

The posterior is the product of the factors. With Gaussian errors each factor is the exponential of a squared, weighted residual, so the maximum of the product is the minimum of a sum:

Linearising every factor at the current estimate and dividing each residual by its standard deviation (whitening) gives a linear least-squares problem in the correction δ. Each factor contributes one block row to the matrix A; the row is non-zero only in the columns of that factor’s variables:

Λ is the information matrix. Its block (j, k) is non-zero exactly when a factor touches both variable j and variable k: the pattern of Λ is the adjacency of the graph. This is the same H as in M4-T4, now for any mixture of variables.

Elimination and fill-in

The system is solved by factorising, Λ = RᵀR (Cholesky) or A = QR, and back-substituting in the triangular R. Factorising is variable elimination: choose a variable, collect every factor that touches it, and replace them by (a) one row of R, which expresses that variable in terms of its neighbours, and (b) one new factor on those neighbours. The new factor couples all the neighbours with each other. Couplings that did not exist before are fill-in: extra non-zeros in R, which cost memory and time.

• The order matters. Eliminating a landmark seen from 50 poses first couples all 50 poses; eliminating the poses of a chain first, then the landmark, creates far less. Heuristics such as COLAMD and nested dissection choose an order with low fill-in; finding the best order is NP-hard.
• Loops cost fill-in. A chain factorises with none. Every loop (a loop closure, a landmark seen twice) adds some, whatever the order.
• R is a graphical model too. Row j of R is the conditional density of variable j given the variables eliminated after it. Elimination turns the factor graph into a Bayes net, and back-substitution solves it from the last variable to the first.

Marginalization

To drop a variable from the problem without losing its information (old poses in a fixed-lag smoother, the previous state in a filter), split the variables into kept (a) and removed (b). The information on the kept variables is the Schur complement:

The correction term is non-zero for every pair of variables that were both connected to the removed one, so marginalization is elimination with the result kept as a dense prior: it adds fill-in among the neighbours. A filter marginalizes every past pose at every step, which is why the EKF-SLAM information matrix over landmarks becomes dense (M3-T7), while a smoother that keeps the trajectory stays sparse. The prior is also frozen at the linearisation point used when it was computed.

Incremental smoothing: iSAM and iSAM2

A robot adds a few factors per step. Solving from the start each time grows with the length of the trajectory. Incremental smoothing reuses the factorisation:

1. iSAM (Kaess et al., 2008) keeps R and appends the rows of the new factors, restoring the triangular form with Givens rotations. Only entries from the affected variables onward change. Periodically it re-orders the variables and re-linearises everything, because fill-in accumulates and the old linearisation points go stale.
2. iSAM2 (Kaess et al., 2012) stores the factorisation as a Bayes tree: the cliques of the eliminated graph arranged as a tree, with the most recent variables near the root. A new factor affects only the cliques that contain its variables and the path from them to the root. That part is turned back into factors, re-ordered and re-eliminated; the rest of the tree is untouched.
3. Fluid relinearisation. A variable is re-linearised only when its accumulated correction exceeds a threshold, and only the cliques that depend on it are redone. There is no periodic batch step.
4. Partial solve. Back-substitution stops along branches where the change falls below a threshold, so distant, settled variables are not revisited.

An odometry factor touches the newest variables, near the root: the update is small and roughly constant. A loop closure names an old pose, deep in the tree: everything on the path between the old and the new variable is re-eliminated, so the cost of that step jumps. This is the spike seen in the timing plots of every incremental system.

Marginal covariances

The uncertainty of a variable is a block of Σ = Λ⁻¹, not of Λ. The inverse of a sparse matrix is dense, so it is never formed. The blocks that are needed (the diagonal ones, and pairs used to gate a data association, M2-T7) are recovered from R by a recursion that visits only its non-zero entries. Recovering covariances costs extra and is done on demand.

X, X_i

all variables; the subset that factor i touches.

f_i

a factor: one measurement, or a prior, as a function of X_i.

A, b

whitened Jacobian and residual: one block row per factor.

Λ, η

information matrix AᵀA and information vector Aᵀb.

R

upper-triangular square-root information matrix, Λ = RᵀR.

fill-in

non-zeros of R (or of a marginal prior) where Λ had zeros.

Bayes tree

the cliques of the eliminated graph as a tree; the data structure of iSAM2.

Strengths, limits and failure modes

• General: any sensor becomes a factor type; the solver does not change. LIO-SAM (M5-T8) combines IMU preintegration, LiDAR odometry, GPS and loop-closure factors in one graph.
• Exact smoothing at near-filter cost while the robot explores; cost rises at loop closures and in small spaces revisited continuously, where the tree becomes dense.
• Local: still Gauss-Newton underneath. A bad initial value for a new variable, or a wrong factor, damages the estimate exactly as in M4-T4 to M4-T6.
• Unbounded growth: the graph grows with time. Long missions need keyframes, sparsification or a fixed-lag window, and each of those is an approximation.
• Marginalization is irreversible: a prior computed from a poor linearisation point stays in the problem and makes the estimate overconfident.

A worked example: four unknowns on a line

A robot moves along a line. Unknowns: three positions x₀, x₁, x₂ and one landmark position l. Five factors, as in the figure: a prior x₀ = 0 (σ = 0.1 m); odometry x₁ − x₀ = 1.0 and x₂ − x₁ = 1.0 (σ = 0.2 m each); landmark ranges l − x₀ = 2.1 and l − x₂ = 0.2 (σ = 0.1 m each). The problem is linear, so one solve is exact.

1. Whiten and stack. Each row of A is the factor’s Jacobian divided by its σ: the prior gives (10, 0, 0, 0), the odometry factors (−5, 5, 0, 0) and (0, −5, 5, 0), the ranges (−10, 0, 0, 10) and (0, 0, −10, 10). b = (0, 5, 5, 21, 2).
2. Information matrix. Λ = AᵀA has rows (225, −25, 0, −100), (−25, 50, −25, 0), (0, −25, 125, −100), (−100, 0, −100, 200). The two zeros are the pairs with no common factor: x₀–x₂ and x₁–l. η = (−235, 0, 5, 230).
3. Solve. x₀ = 0.00, x₁ = 0.96, x₂ = 1.92, l = 2.11. Odometry alone gives 0, 1, 2. The two ranges say x₂ is 1.9 m from x₀, the odometry says 2.0 m; the ranges are more precise, so the solution sits close to them, and the correction is shared equally by the two odometry steps.
4. Uncertainty. From Σ = Λ⁻¹ the standard deviation of x₂ is 0.16 m; with odometry alone it is 0.30 m. The landmark has tied x₂ back to x₀. Note that Λ₂₂ = 125 would suggest 1/√125 = 0.09 m: the diagonal of Λ is not the marginal uncertainty.
5. Fill-in. Factorising in the order x₀, x₁, x₂, l gives R with rows (15, −1.67, 0, −6.67), (0, 6.87, −3.64, −1.62), (0, 0, 10.57, −10.02), (0, 0, 0, 7.25). The entry −1.62 couples x₁ and l, which share no factor: eliminating x₀ joined its neighbours. The four variables form one loop, and every order produces one such entry.
6. Marginalize x₁. The Schur complement on (x₀, x₂, l) has rows (212.5, −12.5, −100), (−12.5, 112.5, −100), (−100, −100, 200). The new −12.5 between x₀ and x₂ is the two odometry steps merged into one constraint of 2.0 m with σ = √(0.2² + 0.2²) = 0.283 m, since 1/0.283² = 12.5. Solving the reduced system gives x₂ = 1.92, l = 2.11 again: nothing was lost, and one variable’s worth of sparsity was spent.
7. Add a measurement. A new pose x₃ with odometry x₃ − x₂ = 1.0 is appended. With the order x₀, x₁, l, x₂, x₃ the first three rows of R are unchanged; only the row of x₂ is recomputed (its diagonal goes from 6.20 to 7.97) and a row for x₃ is added. The estimate becomes x₃ = 2.92 and the earlier values do not move. With x₂ placed before l in the order, the rows of both x₂ and l would change. Keeping recent variables last is what makes the update local.

In a real system the scalars are 6-dimensional poses and the rows are blocks, but the pattern, the fill-in and the locality of the update are exactly these.

Engineering implications and limitations

Engineering implications

• Design the graph before the code. List the variables (pose, velocity, bias, landmarks, extrinsics) and one factor per measurement type, with its noise model. Most back-end bugs are a missing prior, a factor attached to the wrong variable, or a noise model in the wrong units.
• Every variable needs enough factors. A variable touched by too few constraints makes Λ singular: the solver reports an indeterminate system. A landmark seen once by a monocular camera and a bias with no prior are the usual cases.
• Add keyframes, not every frame. The cost follows the number of variables and the fill-in. Preintegrate the IMU between keyframes (M6-T6) and summarise odometry between them.
• Expect timing spikes at loop closures and run the back end in its own thread, so tracking never waits for it (M4-T1).
• Choose the smoother to fit the mission. Full incremental smoothing while the map is small or the path is exploratory; a fixed-lag smoother with marginalization for long odometry-only runs; periodic batch optimisation when the incremental estimate is suspected of being stuck.
• Ask for covariances only where they are used (gating, reporting, active decisions). Recovering all of them every step can cost more than the solve.
• Use a library. GTSAM implements iSAM2 and the common factor types; Ceres and g2o solve the batch problem. Writing a factor is routine; writing a sparse incremental solver is not.

Limits

Incremental smoothing is as exact as the linearisation thresholds allow: with loose thresholds it trades accuracy for time. Marginalization fixes the linearisation point of the prior; when the estimate later moves, the prior is inconsistent with the remaining factors and the filter or window becomes overconfident, which is the reason for first-estimate Jacobians in visual-inertial systems (M6-T7). The Gaussian assumption is built in: a multi-modal belief (two plausible loop closures) is not represented. And the graph grows without bound unless variables are removed, which brings back the marginalization trade-off.

Common misconceptions

Engineering takeaways

1. Variables are unknowns, factors are measurements; the cost is the sum of the factors’ squared whitened residuals.
2. Λ = AᵀA is non-zero only between variables that share a factor: the graph is the sparsity pattern.
3. Eliminating or marginalizing a variable couples its neighbours (fill-in); the elimination order decides how much.
4. iSAM2 keeps the factorisation as a Bayes tree and redoes only the cliques that a new factor affects; odometry is cheap, a loop closure is not.
5. Uncertainty lives in Λ⁻¹ and is recovered from R on demand, never by inverting Λ.

References and further reading

Established fundamentals. The monograph by Dellaert and Kaess is free to read and is the place to continue. The resources page lists courses that teach this material, among them MIT VNAV lectures 23 and 24.

• Dellaert, F. and Kaess, M. (2017). Factor graphs for robot perception. Foundations and Trends in Robotics 6(1–2): the text this lesson follows, chapters 1 to 5.
• Dellaert, F. and Kaess, M. (2006). Square Root SAM: Simultaneous localization and mapping via square root information smoothing. International Journal of Robotics Research 25(12).
• Kaess, M., Ranganathan, A. and Dellaert, F. (2008). iSAM: Incremental smoothing and mapping. IEEE Transactions on Robotics 24(6).
• Kaess, M. et al. (2012). iSAM2: Incremental smoothing and mapping using the Bayes tree. International Journal of Robotics Research 31(2).
• Kschischang, F. R., Frey, B. J. and Loeliger, H.-A. (2001). Factor graphs and the sum-product algorithm. IEEE Transactions on Information Theory 47(2): where the notation comes from.
• Dellaert, F., Kaess, M. and Barfoot, T. (2026). Factor Graphs for SLAM. Chapter 1 of the SLAM Handbook, Cambridge University Press (public draft).

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Module 4 closes with labs that visualise a graph, run Gauss-Newton on it, diagnose a broken loop closure and compare the trajectories. Module 5 then applies this machinery to LiDAR, where LIO-SAM is a factor graph with four kinds of factor.

Related content

Nothing below is required to finish this lesson. The last 2 entries are lessons of other courses on this site that teach the background or go further.

• Before
  M4-T7: Bundle Adjustment

  Define reprojection-error optimisation over cameras and 3D points and its Schur-complement structure.

• Lesson
  M4-T4: Graph Optimization

  Apply Gauss-Newton to SE(2) pose graphs, with the sparse structure, gauge freedom and an anchor.

• Lesson
  M4-T2: Pose Graphs

  Define nodes, relative-pose edges and information matrices and build a pose graph from odometry and loop closures.

• Probability
  Factor Graphs

  Factor graphs introduced from probability: what a factor is and why the posterior factorises.

• Probability
  Smoothing: Using Future Measurements

  Filtering against smoothing: what is gained by keeping the past states.