SLAM Backend
Why the field left filters

Chapter 1: Filtering vs Optimisation

Start with the smallest problem that shows everything. Three robot poses in a line. Odometry between consecutive ones, a prior on the first. Write the matrix.

This is the derivation. It takes four minutes, it is entirely mechanical, and if you can do it cold you have already answered every follow-up in this chapter.

A Gaussian has two faces

Everything here rests on one fact: a Gaussian can be written two equivalent ways, and the two ways make different operations cheap.

The moment form is the one you learned first: a mean μ and a covariance Σ.

The information form (also called the canonical form) multiplies the exponent out and keeps the two things that survive:

Λ is the information matrix (sometimes the precision matrix) and η is the information vector. Nothing has been approximated — these are two parameterisations of the identical distribution. But look at what each one makes easy:

Why Λ is sparse: the one-sentence proof

A factor graph's cost is a sum of squared residuals, one per measurement:

Linearise each residual about the current estimate x: r_f(x + δ) ≈ r_f + J_fδ, where r_f is the residual evaluated at x and J_f = ∂r_f/∂x is evaluated there too. Now do the algebra in full — it is one substitution, one expansion and one derivative, and the object it produces is the matrix the entire rest of this lesson is about.

Step 1 — substitute. Rewrite the cost as a function of the step δ instead of the state x:

Step 2 — expand the product. Multiplying the bracket out gives four terms. The two cross terms, r^TΣ⁻¹Jδ and δ^TJ^TΣ⁻¹r, are 1×1 scalars and are transposes of each other, so they are equal and collapse into one term of twice the size, cancelling the ½:

The greyed term is the cost you already have. It contains no δ, so it cannot move the minimum — drop it. What survives is a plain quadratic in δ: a gradient term and a curvature term, and nothing else.

Step 3 — differentiate and set to zero. For a quadratic δ^Tg + ½δ^THδ with H symmetric (and J^TΣ⁻¹J is symmetric by construction), the gradient is exactly g + Hδ:

Move the constant vector across and you have the normal equations:

Two footnotes worth having ready, because both come up constantly. The left matrix is the Gauss-Newton Hessian: the exact Hessian carries an extra ∑_f (∂²r_f/∂x²)^TΣ_f⁻¹r_f term, which Gauss-Newton drops because it is multiplied by the residual and therefore small near a good solution — and large when the fit is bad, which is exactly why Gauss-Newton diverges on a bad initialisation (Chapter 4). And it is the same object as the Λ = Σ⁻¹ of the table above: the curvature of the negative log-likelihood is the inverse covariance. One symbol, two jobs, no coincidence.

Name the left matrix Λ and the right vector η. Now the key observation: J_f is zero everywhere except in the columns of the variables that factor f actually touches. A range measurement touches one pose and one landmark. An odometry factor touches two poses. So J_f^TΣ_f⁻¹J_f is a tiny outer product scattered into two or three block rows and columns, and everything else stays exactly zero.

Worked example 1: the three-pose chain, by hand

Work in one dimension so every number fits on a whiteboard. Three poses x₀, x₁, x₂ on a line. Three factors:

• A prior on x₀ with σ_p = 1.0 m, so its weight is w = 1/σ² = 1.
• Odometry x₀→x₁ with σ_u = 0.5 m, so w = 1/0.25 = 4.
• Odometry x₁→x₂, same σ, w = 4.

Take the odometry residual r = (x_j − x_i) − u. Its Jacobian with respect to (x_i, x_j) is J = [−1, +1]. So its contribution to Λ is:

Now scatter the three contributions into a 3×3 grid, one at a time, and add:

Adding those up entry by entry: Λ₀₀ = 1 + 4 = 5, Λ₁₁ = 4 + 4 = 8, Λ₂₂ = 4, Λ₀₁ = −4, Λ₁₂ = −4, and Λ₀₂ = 0, because no factor touches both x₀ and x₂.

Tridiagonal. That single structural zero at (0,2) is the entire argument of this lesson in one entry. Now watch what marginalisation does to it.

Marginalisation is a Schur complement — the derivation

Split the variables into the ones you want to drop (call them a) and the ones you want to keep (b). Write the information form blockwise:

Marginalising means integrating a out: p(b) = ∫ p(a, b) da. Complete the square in a inside the exponent. The exponent is

Group everything that contains a. Two of the five terms above do:

That is a quadratic in a with linear coefficient (η_a − Λ_abb), so its stationary point is a* = Λ_aa⁻¹(η_a − Λ_abb). Complete the square about it — add and subtract ½a*^TΛ_aaa* — and the whole exponent, both blocks, becomes:

Only the first term contains a, and ∫ exp(−½(a−a*)^TΛ_aa(a−a*)) da = (2π)^n/2|Λ_aa|^−1/2, a constant with no b in it. It disappears into the normalisation. The other three terms are constants as far as the a-integral is concerned and pass straight through it. That is the entire trick, and it is why the result is exact rather than approximate: nothing was dropped, one integral was evaluated in closed form.

Now expand the surviving a* term — this is where the two corrections come from, and it is the step most write-ups skip:

(using Λ_aaa* = η_a − Λ_abb, so one Λ_aa cancels against the two Λ_aa⁻¹). Multiply it out and sort the three pieces by their power of b, remembering Λ_ba = Λ_ab^T:

Collect the b-quadratic terms: −½b^TΛ_bbb + ½b^T(Λ_baΛ_aa⁻¹Λ_ab)b = −½b^T(Λ_bb − Λ_baΛ_aa⁻¹Λ_ab)b. Collect the b-linear terms: η_b^Tb − (Λ_baΛ_aa⁻¹η_a)^Tb. Compare against the standard information form −½b^TΛ'b + η'^Tb and read off both results:

Note the shared factor Λ_aa⁻¹Λ_ab and Λ_aa⁻¹η_a. In code you never form Λ_aa⁻¹; you solve Λ_aaX = Λ_ab and Λ_aay = η_a once, then take Λ_bb − Λ_baX and η_b − Λ_bay — which is exactly the np.linalg.solve pair in the CODE section below.

That is the Schur complement of Λ_aa in Λ. Two things to say about it while you write it:

1. It is exact, and you just watched why: the only operation performed on the exponent was completing a square, which is an identity, and the only integral performed had a closed form. No term was truncated, no series expanded. For a linear-Gaussian system marginalising loses nothing at all — the reduced system gives numerically identical estimates for the kept variables. (The linearisation that produced Λ in the first place is an approximation; the marginalisation of that linear system is not. Keeping those two straight is the whole of Chapter 3, where you will also check it numerically in the Code Lab.)
2. It is not free. The correction term Λ_baΛ_aa⁻¹Λ_ab is an outer product over all the neighbours of a. Every pair of neighbours that was structurally zero becomes nonzero.

Worked example 2: marginalise a leaf, then marginalise the middle

Same Λ = [[5, −4, 0], [−4, 8, −4], [0, −4, 4]]. Two cases, both by hand.

Case A — drop x₀ (a leaf, degree 1). Here a = {0}, b = {1, 2}.

The correction is

Subtract entry by entry: (0,0): 8 − 3.2 = 4.8. (0,1): −4 − 0 = −4. (1,1): 4 − 0 = 4.

The zero pattern is unchanged — there were no zeros to destroy, because x₀ had exactly one neighbour and a single neighbour cannot form a new pair. Marginalising a leaf is free.

Case B — drop x₁ (the middle, degree 2). Now a = {1}, b = {0, 2}.

Look at entry (0,1). It was 0. It is now −2.

The general rule falls straight out: eliminating a variable of degree d makes its d neighbours a clique, creating up to d(d−1)/2 edges. For d = 2 that is one edge. For d = 10 it is 45. For d = 50 it is 1,225.

Worked example 3: the landmark that densifies everything

This is the EKF-SLAM story in four numbers. One landmark l, observed from three poses, each observation with weight 1 and residual r = l − x_i (Jacobian [−1, +1] again). Assemble Λ over (x₀, x₁, x₂, l):

The three poses are mutually uncoupled — the top-left 3×3 block is the identity. Now marginalise the landmark: a = {l}, Λ_aa = [3], Λ_ab = [−1, −1, −1].

A completely dense 3×3. Six structural zeros became six nonzeros from a single elimination. Now imagine a thousand landmarks and a robot that marginalises its pose after every step, and you have derived the EKF-SLAM covariance from scratch.

Worked example 4: degree four — counting the clique entry by entry

The rule "d neighbours become a clique, up to d(d−1)/2 new edges" was asserted three paragraphs ago. Assertions are not proofs. Compute it — the same star as above with one more pose, so d = 4, small enough to write every entry and big enough that the count is not trivially 1.

Four poses x₀…x₃, one landmark l, four observations, weight 1 each, residual r_i = (l − x_i) − z_i so J_i = [−1, +1] over (x_i, l). Scatter each one exactly as in Worked example 1: Λ_ii += 1, Λ_ll += 1, Λ_il −= 1, Λ_li −= 1. Four factors later, over the ordering (x₀, x₁, x₂, x₃, l):

Count the nonzeros before. Five on the diagonal (four poses at 1, the landmark at 4, which is 1 per observation) plus eight off-diagonal (four −1 entries, each appearing twice by symmetry) = 13 nonzeros in 25 slots, 52% dense. The four poses are mutually uncoupled: the top-left 4×4 block is I₄, sixteen slots holding four nonzeros.

Now eliminate l. a = {l}, so Λ_aa = [4], Λ_ab = [−1, −1, −1, −1], Λ_ba is its transpose, and the correction is a 4×1 times 1×1 times 1×4 outer product:

Subtract it from Λ_bb = I₄. Every diagonal entry: 1 − 0.25 = 0.75. Every off-diagonal entry: 0 − 0.25 = −0.25. All sixteen:

Now the count the rule predicted. Twelve off-diagonal entries were structurally zero and are now −0.25. Twelve entries is six edges, because Λ is symmetric and one edge owns two entries — and d(d−1)/2 = 4·3/2 = 6. The rule is not an estimate; it is the exact count, and "up to" only because two neighbours that already shared a factor were already coupled.

The general form falls straight out of the same three lines. For a landmark seen by d poses with unit weights, Λ_aa = [d], Λ_ab = −1^T, and:

That matrix has a name outside robotics: it is the centering matrix. Applied to a vector it returns that vector minus its own mean. Which is exactly the physical statement: once the landmark is gone, the only thing the four poses know about each other is their deviation from the group average. The landmark's whole job was to be that average, and marginalising it did not delete that job — it redistributed it across six new edges.

The check that proves nothing was invented. Sum any row of Λ': 0.75 − 3(0.25) = 0. So Λ'·1 = 0 — the all-ones vector is in the nullspace, meaning "slide every pose by the same amount" costs nothing. Now check the original: row 0 of the 5×5 gives 1 − 1 = 0, and the landmark row gives −4 + 4 = 0, so Λ·1 = 0 too. The rank deficiency survived the elimination exactly — one gauge direction before, one after. Marginalisation moved information around; it neither created nor destroyed any. Contrast this with the EKF's inconsistency later in the chapter, where a nullspace of dimension 3 silently becomes 2. That is the difference between an exact operation and a broken one, and it is testable in one line.

python
import numpy as np
d   = 4
Lam = np.eye(d + 1); Lam[:d, d] = -1; Lam[d, :d] = -1; Lam[d, d] = d
S   = Lam[:d, :d] - np.outer(Lam[:d, d], Lam[d, :d]) / Lam[d, d]
print(S)                              # 0.75 on the diagonal, -0.25 everywhere else
print((Lam != 0).sum(), (S != 0).sum())  # 13 16  <- fewer variables, more nonzeros
print(np.allclose(S @ np.ones(d), 0))   # True — the gauge freedom survived exactly

Now scale the same three lines. This table is the order-of-magnitude drill: memorise the shape, not the numbers.

Two engineering consequences follow, and both show up in later chapters. One: the cost of eliminating a degree-d variable is O(d²) work to form the outer product and O(d²) memory to store the result, so elimination order matters enormously — eliminate low-degree variables first and the fill-in stays bounded, which is precisely what COLAMD does inside Ceres and GTSAM (Chapter 4). Two: in bundle adjustment you eliminate the points, not the cameras, because a point is seen by maybe 10–30 cameras while a camera sees 500–2,000 points; eliminating points costs a 30×30-ish fill per point, eliminating cameras would cost a 2,000×2,000 fill per camera. That asymmetry is the entire reason the reduced camera system in Chapter 2 is built the way it is.

The second leg: relinearisation

Sparsity is the leg everyone reaches. Relinearisation is the leg that separates a working understanding from a deep one.

Every one of these systems is nonlinear. A range measurement is a square root, a bearing is an arctangent, a rotation composition is a matrix product on a manifold. To use the linear machinery above, you evaluate the Jacobian J at some linearisation point — a specific value of the state.

A filter has one shot. It receives a measurement, linearises about its current estimate, folds the result into Λ (or equivalently into Σ), and throws the raw measurement away. If the estimate later improves by two metres, that Jacobian is still evaluated at the old, wrong point. It is welded in.

A smoother keeps the raw measurements. Every iteration it re-evaluates every residual and every Jacobian at the newest estimate and rebuilds Λ from scratch. When the estimate improves, the linearisation of the entire history improves with it.

Worked example 5: one bad linearisation, with numbers

A robot at unknown position x on a line measures its range to a landmark 3 m off to the side. The measurement model is a square root:

The true position is x = 4, so the noiseless range is √(16 + 9) = 5. Take z = 5.00 exactly, with σ = 0.2 m. Now suppose your prior estimate is bad: x̂ = 1.0.

Step 1 — evaluate at the prior. h(1) = √10 = 3.162278. Residual r = h − z = 3.162278 − 5 = −1.837722. Jacobian J = 1/3.162278 = 0.316228.

Step 2 — the filter's single update. The Gauss-Newton step is δ = −(J^TJ)⁻¹J^Tr = −r/J (the σ cancels for a scalar):

It overshot the true value of 4 by 2.81 m — because it took a step sized by the shallow slope at x = 1, where the range barely changes with position, and applied it in a region where the slope is steep.

Step 3 — and here is the damage. The filter also reports its confidence, and it computes that from the same frozen Jacobian:

It is 2.811 m wrong and claims 0.632 m of uncertainty. That is a 4.45σ error reported as if it were routine. A consistency monitor would flag it; the filter itself has no idea.

Step 4 — the smoother, same data, iterated. It does not throw the measurement away, so it just keeps going:

Three extra evaluations of the same measurement and it lands on 4.000000. And its reported uncertainty uses the Jacobian at the answer: Λ = 0.8²/0.04 = 16, so σ = 0.25 m — and this time the estimate really is that good.

Drive the slider to both ends and read the numbers under the plot — they are the point of the widget, and they say something more precise than "linearisation error is bad".

Pull the prior down to 0.7. The x-axis stretches to keep the filter's marker on the chart — at the far low end it has to run out past 20, which is itself the finding. The single step lands at 9.147 — 5.147 m past the truth, on the far side of the minimum — and because the frozen Jacobian there is J = 0.7/√9.49 = 0.2272, the reported σ is 0.2/0.2272 = 0.880 m. That is a 5.8σ error announced as routine. Push it further, to 0.3, and the step lands at 20.25 with σ = 2.01 m: still 8.1σ. The step size blows up because δ = −r/J divides by a Jacobian that is going to zero.

Now push the prior up to 8. The step lands at 4.215 — still short of the truth, still on the same side it started from, it does not overshoot — with J = 8/√73 = 0.9363, so σ = 0.214 m against a 0.215 m error: 1.0σ, an honest report. At 9 it is 4.270 with σ = 0.211 m, 1.3σ. One relinearisation from either would finish the job.

Press Relinearise & step repeatedly from the 0.7 setting: the teal parabola re-forms at each new estimate, the model tracks the true cost instead of a memory of it, and the estimate walks down to 4.000000 in four or five steps. The filter's warm marker never moves again, because the filter no longer has the measurement — only its linearised shadow.

The consistency argument, and the number that proves it

The relinearisation problem in SLAM is not merely inaccurate; it is structurally wrong in a way that has been proved.

In 2-D SLAM with only relative measurements — odometry and landmark observations — three degrees of freedom are fundamentally unobservable: global x, global y, and global heading. Translate and rotate the entire map and the robot together and every single measurement is unchanged. No amount of data can pin them down. The correct posterior therefore has an information matrix with a 3-dimensional nullspace.

Huang, Mourikis and Roumeliotis (IJRR 2010) showed that when an EKF evaluates its Jacobians at different state estimates at different times — which it must, because the estimate changes — the linearised system's unobservable subspace has dimension 2, not 3. The missing direction is global heading.

Two families of fix exist, and naming them is a strong signal:

• First-Estimates Jacobians (FEJ). Always evaluate the Jacobian of a given state at the first estimate you ever had for it, even after the estimate improves. Trading accuracy of the linearisation for consistency of the rank. Used in OpenVINS and in every serious sliding-window smoother's marginalisation prior (Chapter 3).
• Observability-constrained EKF (OC-EKF). Explicitly project the Jacobians so the unobservable subspace has the correct dimension by construction (Huang et al. 2010; Li & Mourikis IJRR 2013 for the visual-inertial case).
• Just relinearise everything. This is what a smoother does, and it sidesteps the entire problem — because all Jacobians are evaluated at the same estimate, the nullspace comes out the right dimension automatically.

That last bullet is the punchline of the chapter. The smoother does not solve the consistency problem by being cleverer. It solves it by never creating the inconsistency in the first place.

Where the two live in a real stack

A shipping VIO system does not choose. It runs both, at different rates, for different reasons.

The 16-versus-15 in that first row is a shape question worth stating precisely. The nominal state you store is 16 doubles because a unit quaternion needs 4. The covariance is 15×15 and never 16×16: a unit quaternion is constrained to the 3-sphere, so a 4-dimensional Gaussian over it would be singular by construction — the direction along q itself carries zero variance, giving a rank-deficient, non-invertible covariance that will produce a NaN the first time anything inverts it. The fix is the error-state formulation: the estimate lives in the 16-number manifold, the uncertainty lives in the 15-dimensional tangent space, and they are joined by a retraction, q ← q ⊗ exp(½δθ), applied after every solve. Layer two has the same split: its "≈ 307" is tangent-space dimension too. If you say "15-dimensional state" nobody blinks; "16×16 covariance" is the phrase that reveals the misunderstanding.

Bandwidth on the wire. The 200 Hz odometry topic carries a 7-double pose (56 B) plus a 36-double covariance (288 B) plus a header (~40 B) ≈ 384 B per message → 77 kB/s. The keyframe stream to the pose-graph thread carries a pose plus a 32-byte-per-feature descriptor set for ~150 features ≈ 5 kB per keyframe → 20 kB/s. Neither is a problem; the 20 Hz image stream at 640×480×1 B = 307 kB per frame = 6.1 MB/s is, and it is why the frontend and backend usually live in the same process.

Assemble Λ from factors, then marginalise

Twenty-eight lines, no library. This is what a whiteboard-to-keyboard translation looks like.

python
import numpy as np

def add_prior(Lam, eta, i, meas, sigma, x):
    w = 1.0 / sigma**2
    r = x[i] - meas                       # J = +1
    Lam[i, i] += w
    eta[i]    -= w * r                    # eta = -J^T w r

def add_between(Lam, eta, i, j, meas, sigma, x):
    """r = (x_j - x_i) - meas,  so J = [-1, +1] over (x_i, x_j)."""
    w = 1.0 / sigma**2
    r = (x[j] - x[i]) - meas
    Lam[i, i] += w;  Lam[j, j] += w       # (-1)(-1)w and (+1)(+1)w
    Lam[i, j] -= w;  Lam[j, i] -= w       # (-1)(+1)w — the coupling
    eta[i]    += w * r                    # -(-1) w r
    eta[j]    -= w * r                    # -(+1) w r

def marginalize(Lam, eta, drop):
    """Schur-complement the variables in `drop` out of the system."""
    keep = [i for i in range(len(eta)) if i not in drop]
    A = Lam[np.ix_(drop, drop)]           # Lam_aa
    B = Lam[np.ix_(keep, drop)]           # Lam_ba
    X = np.linalg.solve(A, Lam[np.ix_(drop, keep)])   # never form an inverse
    y = np.linalg.solve(A, eta[drop])
    return Lam[np.ix_(keep, keep)] - B @ X, eta[keep] - B @ y

x   = np.array([0.0, 1.0, 2.0])
Lam = np.zeros((3, 3)); eta = np.zeros(3)
add_prior(Lam, eta, 0, 0.0, 1.0, x)
add_between(Lam, eta, 0, 1, 1.0, 0.5, x)
add_between(Lam, eta, 1, 2, 1.0, 0.5, x)
print(Lam)              # [[ 5 -4  0] [-4  8 -4] [ 0 -4  4]]
print(marginalize(Lam, eta, [1])[0])   # [[ 3 -2] [-2  2]]  <- the zero is gone

The production form, for comparison. GTSAM hides the assembly entirely, but the object it builds is precisely the matrix above:

python
import gtsam
from gtsam import symbol_shorthand as S

graph = gtsam.NonlinearFactorGraph()
graph.add(gtsam.PriorFactorPose2(S.X(0), gtsam.Pose2(0, 0, 0),
          gtsam.noiseModel.Diagonal.Sigmas([1.0, 1.0, 0.1])))
graph.add(gtsam.BetweenFactorPose2(S.X(0), S.X(1), gtsam.Pose2(1, 0, 0),
          gtsam.noiseModel.Diagonal.Sigmas([0.5, 0.5, 0.05])))
# ... more factors ...
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
marg   = gtsam.Marginals(graph, result)
print(marg.marginalCovariance(S.X(1)))   # 3x3, recovered from the factor, not inverted

The filter that becomes certain of its own heading

Field report: an indoor AMR running EKF-SLAM in a repetitive aisle. Over twenty minutes the reported 1σ heading uncertainty falls to 0.2°. The true heading error, against a survey, is 4°. No divergence, no crash, smooth trajectory.

The metric that reveals it. Stack the Jacobians of every measurement over a window into one matrix M, and compute its singular values. For 2-D SLAM the correct answer is exactly three singular values at machine zero (the three unobservable gauge directions).

python
sv = np.linalg.svd(M, compute_uv=False)
tol  = sv[0] * max(M.shape) * np.finfo(float).eps
null = int((sv < tol).sum())
print(f"nullspace dim = {null}  (expect 3 for 2-D SLAM)")
# healthy smoother : 3   —  x, y, yaw gauge freedom
# EKF-SLAM         : 2   —  yaw has been silently "measured"

What is changing at the frontier

Three live threads, all worth naming with a year:

• Fluid relinearisation. iSAM2 (Kaess et al., IJRR 2012) does not relinearise everything — it relinearises only the variables whose estimate moved more than a threshold, using the Bayes tree to find which parts of the factorisation that invalidates. This is what makes "keep everything" affordable in real time.
• Square-root everything. Working with the factor R instead of Λ = R^TR halves the condition number in the exponent. Dellaert & Kaess started it in 2006; Demmel et al., Square Root Bundle Adjustment (CVPR 2021), applied nullspace marginalisation in square-root form to BA and reported single-precision solutions matching double-precision Schur.
• The filter, rebuilt on symmetry. Equivariant filter design (Fornasier, van Goor, Mahony & Weiss, ICRA 2022 / T-RO 2023) chooses a state parameterisation whose symmetry group makes the linearisation error structurally correct, recovering consistency without FEJ hacks. If someone asks "is the filter dead", this is the paper that says no.

The four-minute drill

Everything above is inert until your hand can produce it unaided. Run this drill against a timer until all six steps come out without recall pauses. The step budgets keep you honest.

Chapter 2: Graphs, Bundles & Sparsity

A robot drives in a square, comes back to where it started, and the loop does not close by twenty centimetres. Draw the graph and predict — before you solve anything — where that twenty centimetres ends up.

The answer is predictable from the structure alone, and being able to predict it is the proof that a pose graph is not a black box to you.

The pose graph, stated once

A pose graph is a factor graph in which the only variables are robot poses and the only factors are relative pose measurements. Landmarks do not appear — they have already been converted into pose-to-pose constraints by the frontend (scan matching, PnP, ICP; see SLAM Frontend).

Two kinds of edge, and they differ only in where they came from:

• Odometry edges connect consecutive poses. They are plentiful, short-range and accurate individually; their errors accumulate.
• Loop-closure edges connect poses that are far apart in time but close in space. They are rare, and each one is worth thousands of odometry edges — because it is the only kind of measurement that can remove accumulated drift.

The cost is the sum over edges of the squared relative-pose error:

Where X_i is pose i as an element of SE(2) or SE(3), Z_ij is the measured relative transform, and log(·) maps the residual transform into the tangent space so it becomes a plain vector you can square. If that line is unfamiliar, the derivation lives in Coordinate Frames & Rigid-Body Math — here we care about the structure, which is identical in one dimension.

Worked example: a four-pose loop, one exact Gauss-Newton step

Strip everything to 1-D so the arithmetic is whiteboard-sized and the structure is untouched. Four poses in a cycle:

• Odometry x₀→x₁, x₁→x₂, x₂→x₃, each measuring +1.0 with weight w = 1.
• A loop closure x₃→x₀ measuring −2.8, weight w = 1. (It should be −3.0 if the odometry were perfect — so the loop is off by 0.2.)
• A prior on x₀ with weight w = 1, to fix the gauge.

Step 1 — build Λ. Each edge (i, j) adds w to Λ_ii and Λ_jj and subtracts w from Λ_ij and Λ_ji. Four edges plus the prior:

Notice what that is: the graph Laplacian of a 4-cycle, plus 1 in the top-left corner from the prior. The diagonal is each node's degree; the off-diagonals are −1 where an edge exists. The two structural zeros, (0,2) and (1,3), are the two pairs of poses that are diagonally opposite in the loop and therefore never measured against each other.

Step 2 — initialise from odometry. x = (0, 1, 2, 3). Residuals: the three odometry edges are exactly satisfied (r = 0). The loop-closure residual is

Step 3 — build η = −J^TWr. Only the loop edge has a nonzero residual. Its Jacobian with respect to (x₀, x₃) is [+1, −1] (because the residual is written x₀ − x₃ − z), so:

Step 4 — solve Λδ = η by hand. Write the four equations with δ = (a, b, c, d):

From the second, c = 2b − a. Substitute into the fourth: −a − (2b − a) + 2d = −0.2, so 2d = 2b − 0.2 and d = b − 0.1. Substitute both into the third:

Finally the first: 3a − (a − 0.05) − (a − 0.15) = a + 0.2 = 0.2, so a = 0. Back-substituting: b = −0.05, c = 2(−0.05) − 0 = −0.10, d = −0.05 − 0.10 = −0.15.

Step 5 — check the residuals afterwards. This is the payoff. r₀₁ = 0.95 − 0 − 1 = −0.05. r₁₂ = 1.90 − 0.95 − 1 = −0.05. r₂₃ = 2.85 − 1.90 − 1 = −0.05. r₃₀ = 0 − 2.85 + 2.8 = −0.05.

That is the prediction you make before solving, and it generalises: in a cycle of n edges with equal weight, an error e is distributed as e/n per edge and the cost drops from ½e² to ½e²/n.

The weighted version, which is what actually happens

Real edges do not have equal weight, and the general law is worth memorising because it makes you sound like you have watched a solver run.

For a cycle where edge k has weight w_k = 1/σ_k², the residual e is shared inversely to the weight — each edge absorbs the share

Do it with numbers. Five odometry edges with σ = 0.10 m (w = 100) and one loop closure with σ = 0.02 m (w = 2500). The reciprocal weights sum to 5×0.01 + 1×0.0004 = 0.0504. For a loop error of e = 0.13 m:

• Each odometry edge absorbs 0.13 × 0.01 / 0.0504 = 0.02579 m.
• The loop-closure edge absorbs 0.13 × 0.0004 / 0.0504 = 0.00103 m.
• Sanity check: 5 × 0.02579 + 0.00103 = 0.12998 ≈ 0.13. ✓

The tight loop-closure edge absorbs almost nothing, because it is the most trusted measurement in the graph. Those are exactly the numbers the Code Lab below will produce, so you can check the solver against your own arithmetic.

Bundle adjustment: the same idea, a different shape

Bundle adjustment (BA) keeps the landmarks as variables instead of collapsing them into pose-to-pose constraints. Two variable types: C camera poses (6 numbers each) and P 3-D points (3 numbers each). One factor type: a reprojection residual connecting one camera to one point.

Because no factor ever touches two cameras, and no factor ever touches two points, the information matrix has a very particular shape:

• U is 6C×6C and block-diagonal — one 6×6 block per camera, nothing off it.
• V is 3P×3P and block-diagonal — one 3×3 block per point.
• W is 6C×3P and holds a 6×3 block wherever camera c observed point p. It is the only place coupling lives.

Because P is usually a hundred times larger than C, this matrix is drawn as an arrowhead: a thin dense spine along the top and left, an enormous block-diagonal body, and nothing else.

The Schur trick, with the flops written out

You never solve the full system. You eliminate all the points at once — which is legal and cheap precisely because V is block-diagonal, so V⁻¹ is P independent 3×3 inversions. The Schur complement leaves the reduced camera system:

Now the arithmetic that makes it worth doing. Take C = 100 cameras, P = 20,000 points, each point seen by k = 4 cameras.

And note the fill-in is still there, obediently: S is 600×600 and dense, because eliminating a point makes all k cameras that saw it a clique. The trick is not that fill-in was avoided — it is that the fill-in landed in the small block. Choosing which block gets the fill-in is the entire art of elimination ordering, and Chapter 6 lets you feel it.

Drag P down to a few hundred and watch the advantage shrink — with few points and many cameras the reduced system is no smaller than the original and the Schur bookkeeping is pure overhead. That crossover is why Ceres exposes DENSE_SCHUR, SPARSE_SCHUR and SPARSE_NORMAL_CHOLESKY instead of picking one for you.

The two backends in a shipping visual SLAM stack

ORB-SLAM-family systems run both structures, and knowing why is a design lesson in itself.

Memory, in bytes. The local BA problem holds a 960×960 information matrix, but only the 60×60 reduced system is ever dense: 3,600 doubles = 29 kB. The pose graph holds nnz(Λ) = 36 × [5,000 + 2(4,999) + 2(800)] = 597,528 nonzeros; in compressed-sparse-column form with 8-byte values and 4-byte indices that is about 7.2 MB. Its Cholesky factor with a good ordering runs 3–8× that. Budget 50 MB and you will not be surprised.

From scratch, then Ceres

The from-scratch Schur complement, exploiting the block-diagonal V:

python
import numpy as np

def schur_solve(U, W, V_blocks, eta_c, eta_p, obs):
    """U: (6C,6C) block-diagonal.  W: dict (c,p) -> 6x3.  V_blocks: list of 3x3.
    obs: list of (camera, point) pairs.  Returns (dcam, dpt)."""
    S  = U.copy()
    rc = eta_c.copy()
    Vi = [np.linalg.inv(Vb) for Vb in V_blocks]      # P independent 3x3 inverses
    # by point, because a point's cameras form the clique
    for p, Vip in enumerate(Vi):
        cams = [c for (c, q) in obs if q == p]
        for ci in cams:
            Wi = W[(ci, p)]                          # 6x3
            rc[6*ci:6*ci+6] -= Wi @ Vip @ eta_p[3*p:3*p+3]
            for cj in cams:                          # k^2 blocks: the fill-in
                S[6*ci:6*ci+6, 6*cj:6*cj+6] -= Wi @ Vip @ W[(cj, p)].T
    dcam = np.linalg.solve(S, rc)                    # 6C x 6C — small and dense
    dpt  = np.zeros_like(eta_p)
    for p, Vip in enumerate(Vi):                     # back-substitute, embarrassingly parallel
        acc = eta_p[3*p:3*p+3].copy()
        for (c, q) in obs:
            if q == p: acc -= W[(c, p)].T @ dcam[6*c:6*c+6]
        dpt[3*p:3*p+3] = Vip @ acc
    return dcam, dpt

And the production form, where the same decision is one enum:

cpp
ceres::Solver::Options opts;
opts.linear_solver_type = ceres::DENSE_SCHUR;      // <~100 cameras
// opts.linear_solver_type = ceres::SPARSE_SCHUR;  // 100..1000 cameras
// opts.linear_solver_type = ceres::ITERATIVE_SCHUR; // 1000+, with:
// opts.preconditioner_type = ceres::SCHUR_JACOBI;
opts.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
opts.num_threads = 8;
// This is the line that tells Ceres the arrowhead exists.
// Without it, Ceres does not know your points are points.
ceres::ParameterBlockOrdering* ord = new ceres::ParameterBlockOrdering;
for (auto& pt  : points)  ord->AddElementToGroup(pt.data(),  0);   // eliminate FIRST
for (auto& cam : cameras) ord->AddElementToGroup(cam.data(), 1);   // eliminate LAST
opts.linear_solver_ordering.reset(ord);

"Cholesky failed: matrix is not positive definite"

The single most common backend crash, and almost never a numerical accident.

A pose graph with only relative measurements is gauge-free: translate and rotate the whole solution and every residual is unchanged. That means Λ is singular by construction, with a nullspace of a very specific dimension:

The Schur complement is under attack

• Power Bundle Adjustment (Weber, Demmel, Chan, Cremers, CVPR 2023). Rather than forming and factoring S, expand S⁻¹ as a power series in the small correction term. Reported an order of magnitude speed-up over Ceres on BAL-scale problems, with the same accuracy, because it never materialises the dense reduced matrix.
• Square Root Bundle Adjustment (Demmel, Sommer, Cremers, Usenko, CVPR 2021). Marginalise the landmarks with a QR nullspace projection instead of a Schur complement, so the condition number is never squared — single precision then matches double-precision Schur, halving memory and bandwidth.
• MegBA (Ren et al., ECCV 2022). A distributed GPU BA library that keeps the arrowhead structure but partitions it across devices, solving 100k-camera problems in minutes.
• Certifiable optimality. SE-Sync (Rosen, Carlone, Bandeira, Leonard, IJRR 2019) solves a convex relaxation of pose-graph SLAM and returns a certificate that the answer is the global optimum. In realistic noise regimes the relaxation is tight, which means the local-minimum worry that motivates the pose-graph-then-BA sequencing above is, for pose graphs specifically, now provably unnecessary.

Chapter 3: Marginalisation & Sliding Windows

So keep everything. Great. Except this drone flies for thirty minutes at four keyframes a second, which is 7,200 keyframes and about a million landmarks, on a processor with two gigabytes of RAM and a 250 millisecond budget. Now what?

The honest answer is that you do not keep everything. Every shipping visual-inertial system bounds its state, and the real question is how you bound it and what that costs you. Get this chapter right and you can answer any question about VINS-Mono, OKVIS, MSCKF or Kimera without having read their source.

Three ways to bound the state, and only three

Worked example: marginalisation really is lossless

Take the chain from Chapter 1 with actual measured values so η is nonzero. Prior says x₀ = 0.1 with σ = 1 (w = 1). Odometry x₀→x₁ = 1.0 and x₁→x₂ = 1.0, both with σ = 0.5 (w = 4). These are mutually consistent, so you can read the answer straight off the measurements — x₀ = 0.1 from the prior, then +1.0 and +1.0 — giving μ = (0.1, 1.1, 2.1), and the η computation below doubles as a check that Λμ really does reproduce it. That makes it a perfect test case, because you know the truth in advance.

Step 1 — the information form. Λ is the same as Chapter 1, and η = Λμ. Do the matrix-vector product one row at a time:

Step 2 — marginalise x₀ out of the window. From Chapter 1, Λ' = [[4.8, −4], [−4, 4]]. For the information vector:

Step 3 — solve the reduced system. Two equations: 4.8x₁ − 4x₂ = −3.12 and −4x₁ + 4x₂ = 4. Add them: 0.8x₁ = 0.88, so x₁ = 1.1, and then x₂ = x₁ + 1 = 2.1.

What it actually costs, part one: fill-in

Chapter 1 established the rule: eliminating a variable of degree d makes its d neighbours a clique. In a sliding window this compounds every single keyframe.

Consider a window of 10 keyframes where each keyframe observes 150 landmarks and consecutive keyframes share about 60% of them. When the oldest keyframe slides out:

• Landmarks still visible in the remaining window survive. That is the 60% overlap fraction: 0.6 × 150 = 90 landmarks stay in the state, but the observation made from the departing keyframe must be marginalised, because the keyframe pose it was expressed in is about to stop existing.
• The other 150 − 90 = 60 landmarks were seen only by that keyframe. They have nothing left to constrain them, so they are dropped — not marginalised. Dropping them is correct and not a loss: a landmark with one observation is unconstrained along the bearing ray, so its marginal carries no information about anything else. Schur-complementing it would add a rank-0 correction — arithmetic that produces exactly zero.
• The departing keyframe's 15 states are connected to the next keyframe's 15 states (through the IMU factor) and to the previous marginalisation prior. Eliminating them produces a dense block over everything they touched.

In VINS-Mono the resulting marginalisation prior is a dense matrix over the remaining window states. "State" here means one scalar degree of freedom, and a keyframe block is 15 of them (p, v, q, b_a, b_g = 3 + 3 + 3 + 3 + 3). So the prior spans 30 states when it touches two surviving keyframes and 60 when it touches four. Do the bytes at both ends, in doubles:

So 7–29 kB, and doubling D quadrupled it — the prior is a dense square, so its footprint is O(D²). That sounds trivial, and at this size it is: fill-in is not the expensive part in a fixed-lag window, because the window is small by construction. Extrapolate the same square to a 7,200-keyframe trajectory, though, and D = 108,000 gives 108,000² × 8 B = 93 GB. That is the EKF-SLAM disaster in one multiplication, and it is why the bound on the window is the only thing making this legal.

What it actually costs, part two: the frozen linearisation

This is the expensive part, and it is the part that gets asked about.

When you marginalise, you Schur-complement the linearised system. The Jacobians that went into Λ_aa, Λ_ab and Λ_bb were all evaluated at one specific estimate — the one you held at the moment of marginalisation. The resulting prior is a quadratic in the kept variables, and that quadratic is welded to that linearisation point forever. The raw measurements are gone; you cannot rebuild it.

So on the next iteration, when the kept states move, you face a choice:

How big is the effect? Monocular visual-inertial odometry has exactly 4 unobservable directions: global x, y, z and yaw about gravity. (Roll and pitch are observable because gravity gives you an absolute reference; scale is observable because the accelerometer is metric.) Without FEJ, the linearised system's unobservable subspace collapses to 3, and the missing direction is yaw — the result is due to Huang, Mourikis & Roumeliotis, "Observability-based rules for designing consistent EKF SLAM estimators", IJRR 29(5), 2010, and its visual-inertial form to Li & Mourikis, "High-precision, consistent EKF-based visual-inertial odometry", IJRR 32(6), 2013. That is the sentence everyone repeats. The rest of this section derives it, because a sentence you cannot derive is a sentence you will fumble under follow-up.

The four nullvectors, written out

An unobservable direction is a perturbation of the whole state vector that leaves every residual unchanged. Write ĝ for the unit gravity direction in the global frame, and let the state hold keyframes (p_i, v_i, θ_i, b_a, b_g) and features p_f. The four are:

Read the fourth row carefully, because it is the one that causes all the trouble. The three translation vectors are constant — they do not depend on the estimate at all. The yaw vector's entries are built out of p_i, v_i and p_f: it moves when the estimate moves. Two blocks of your Jacobian linearised at two different estimates are therefore being asked to annihilate two different vectors, and no single vector satisfies both. That is the entire mechanism, and everything below is that sentence with numbers attached.

The biases are zero in every row for a reason worth saying out loud: biases live in the body frame, and rotating or sliding the whole world does not change what an accelerometer strapped to the robot reads. Roll and pitch are absent because rotating the world about any non-gravity axis does change the measured gravity vector, which is exactly why they are observable.

Worked example: watch the yaw direction disappear, entry by entry

Full 3-D VIO has 15 states per keyframe, so let us do the planar analogue — same mechanism, small enough to write on a whiteboard. Planar world, states (p^x, p^y, θ) per keyframe, (f^x, f^y) for one landmark. Scale is metric because the IMU is, so the unobservable set is global x, global y and global yaw: 3 directions, the planar shadow of the 4.

The window. Two keyframes and one landmark, so 3 + 3 + 2 = 8 states, in the column order

The linearisation point x_lin. p₁ = (0, 0), θ₁ = 0; p₂ = (2, 0), θ₂ = 0; f = (2, 1). Keyframe 2 is two metres ahead of keyframe 1; the landmark is level with keyframe 2 and one metre to its left.

The three factor blocks. Keyframe 0 has already been marginalised, so:

• P (2 rows) — the marginalisation prior, which is keyframe 0's folded-in view of the landmark expressed against keyframe 1. Its residual is r = R(θ₁)^T(f − p₁) − z̄, and its Jacobian is frozen at x_lin forever.
• V (2 rows) — the live visual factor, keyframe 2 observing the same landmark: r = R(θ₂)^T(f − p₂) − z̄.
• O (3 rows) — the live IMU/odometry factor between the keyframes: r = ( R(θ₁)^T(p₂ − p₁) − Δp̄,   θ₂ − θ₁ − Δθ̄ ).

The derivatives, once. With R(θ) = [[c, −s], [s, c]] and u = f − p_i:

At θ₁ = θ₂ = 0 every R^T is the identity, so the yaw column is simply (u^y, −u^x). For block P, u = f − p₁ = (2, 1), giving (1, −2). For block V, u = f − p₂ = (0, 1), giving (1, 0). For block O the same identity applies with w = p₂ − p₁ = (2, 0), giving (0, −2). Assemble:

The three nullvectors, at x_lin. Sliding the world: n_x = (1,0,0, 1,0,0, 1,0) and n_y = (0,1,0, 0,1,0, 0,1) — every position moves, no angle does. Spinning the world about the origin by ε sends a point p to p + ε(−p^y, p^x) and every heading to θ + ε, so

Now multiply, one row at a time. This is the arithmetic that makes the claim yours:

Seven zeros. Repeat for n_x and n_y and you get seven zeros each (row P1: −1 + 1 = 0; row V3: −1 + 1 = 0; row O5: −1 + 1 = 0; the rest touch no x column). So rank J = 8 − 3 = 5, nullity 3. The singular values are 3.570, 2.000, 1.732, 1.732, 1.120, 0, 0 — and the seventh is absent because J has only 7 rows. Three exact zeros in an 8-column problem, which is the correct answer for planar visual-inertial.

The same window, one linearisation point out of step

Now run one LM iteration. The landmark is the least-constrained variable in the window, so it moves the most: re-triangulation shifts it 5 cm in y, from f = (2, 1.00) to f = (2, 1.05). The keyframes barely move — the odometry factor pins them — so take them as unchanged, which isolates the effect in a single number.

Relinearise the live blocks at x_now. Only one entry changes: block V's yaw column is (u^y, −u^x) with u = f − p₂ = (0, 1.05), so J[V3, θ₂] goes from 1.00 to 1.05. Block P cannot follow — its raw measurement was destroyed at marginalisation time — so it stays at 1.00. Block O does not depend on f at all, so it is untouched.

Multiply the same n_ψ through again. Six rows are unchanged and still give zero. Row V3 is now:

And if you instead try the new yaw nullvector n_ψ(x_now) = (0,0,1, 0,2,1, −1.05, 2), block V is happy but the frozen prior is not: row P1 gives (1)(1) + (1)(−1.05) = −0.05. Neither vector is annihilated by every block, and no combination is either, because the two blocks disagree by a fixed offset.

How fast does the fiction accumulate? The information gained in the yaw direction from one such observation is ‖W^1/2Jn_ψ‖² / ‖n_ψ‖². Here ‖n_ψ‖² = 1 + 4 + 1 + 1 + 4 = 11, and with a landmark-position sigma of 2 cm the weight is w = 1/0.02² = 2500, so:

Information adds. One keyframe of the sliding-window design below carries ~900 reprojection factors, so after a single keyframe I_yaw = 0.568 × 900 = 511, giving σ_yaw = 1/√511 = 0.044 rad = 2.53°. After 100 keyframes — twenty-five seconds of flight — I_yaw = 0.568 × 90,000 = 51,120 and σ_yaw = 0.253°. It keeps falling as 1/√N forever, because nothing in the estimator knows the information is invented. Meanwhile the true yaw error is doing a random walk upward. That crossing — reported sigma falling as 1/√N while true error grows as √N — is the signature, and now you can quote both curves.

Keyframes: how you decide what is worth keeping

A sliding window over every frame would be useless — twenty frames at 20 Hz is one second of motion, during which a slow-moving robot barely translates, and a window with no baseline cannot triangulate anything. So the window slides over keyframes, and the keyframe policy is a real design decision with real numbers.

Where the 10-pixel threshold comes from, in two steps

Nobody picked 10 px because it sounded round. It falls out of a depth-uncertainty budget, and being able to reproduce that derivation at the whiteboard is worth more than remembering the number.

Step 1 — pixels to degrees. A displacement of δ pixels at focal length f subtends an angle arctan(δ/f). With the VINS-Mono threshold δ = 10 px and a typical 640×480 fisheye-rectified f = 460 px:

The arctan barely matters at this size — the small-angle correction is δ³/3 = 3.4×10⁻⁶ rad, about 0.016% — so "pixels divided by focal length, in radians" is the version to do in your head.

Step 2 — degrees to depth uncertainty. Triangulation from a baseline b gives Z = f b / d, where d is the disparity in pixels. Differentiate: dZ/dd = −f b/d² = −Z²/(f b), so

Take the flight case: b = 0.2 m of travel between keyframes, Z = 10 m, f = 460 px, and a feature-tracker standard deviation σ_d = 0.5 px. Then

Now the tidy part. Since d = f b/Z, that whole expression collapses to σ_Z/Z = σ_d/d. The relative depth error is just the tracker noise divided by the parallax, in pixels, and depth cancels out entirely. Check it: d = 460 × 0.2 / 10 = 9.2 px, and 0.5 / 9.2 = 5.4%. Same answer. So VINS-Mono's 10 px threshold is exactly the statement "do not create a keyframe until the geometry supports roughly 5% relative depth" — 0.5/10 = 5.0%.

What that costs at range. Because the pixel threshold is depth-independent, holding 5.4% at Z = 30 m still needs d = 9.2 px, and d = f b/Z gives b = 9.2 × 30 / 460 = 0.6 m of baseline — three times as far travelled, so at fixed speed three times as long between keyframes. At Z = 30 m with the original b = 0.2 m you only earn d = 460 × 0.2 / 30 = 3.07 px, and 0.5/3.07 = 16.3% relative depth: 4.9 m of uncertainty on a 30 m point. That is the honest answer to "why does my VIO get worse outdoors" — the scene got further away, the parallax per metre travelled fell as 1/Z, and the same keyframe policy now emits keyframes carrying a third of the depth information they used to.

A real sliding window, sized in numbers

This is the VINS-Mono shape, which is close enough to OKVIS and Kimera that it answers for all of them.

Factors in the window: 9 IMU preintegration factors (one per consecutive keyframe pair, each a 15-dimensional residual), roughly 900 reprojection factors (150 features × ~6 keyframes each, 2-dimensional), and one marginalisation prior. That one prior is dense over the states it touches; everything else is a two- or three-variable factor.

Timing on an ARM Cortex-A72 class core, per keyframe (250 ms budget):

Where the 50 marginalised states come from, and what they cost

The marginalise row in that table is the one almost nobody can break down, so break it down. VINS-Mono parameterises each feature by an inverse depth in the keyframe that first observed it — its anchor. When the oldest keyframe leaves, two things must go with it:

• The keyframe's own block: p, v, q, b_a, b_g = 3 + 3 + 3 + 3 + 3 = 15 states.
• Every inverse depth anchored to it, because an inverse depth is meaningless once its anchor frame no longer exists. On an even split, 150 features over 10 keyframes is 15 anchors each; in practice the oldest keyframe holds more than its share, because features that survived 10 keyframes of selection are long-lived and the recent frames anchor mostly churn. Budget ~35.

Now price the Schur complement Λ' = Λ_bb − Λ_baΛ_aa⁻¹Λ_ab, in three pieces:

Eight million flops — the same order as one 307×307 Cholesky, which is why marginalisation lands at 1–3 ms rather than being free or being the bottleneck. And note which term dominates: 2ab². The cost is quadratic in what you keep and only linear in what you remove, so marginalising more aggressively barely helps; shrinking the window does.

VINS-Mono's actual policy: two-way marginalisation

Here is the detail about VINS-Mono that comes up more than any other, and the one that anyone who only knows "marginalise the oldest keyframe" gets wrong roughly half the time. VINS-Mono does not have one marginalisation rule. It has two, and it picks between them by asking whether the second-newest frame was declared a keyframe.

Why branch 2 is not "just be consistent and marginalise". Two independent reasons, and you want both:

• Marginalising it would weld a dense prior onto a frame with no parallax. The frame failed the keyframe test, which by construction means the median parallax was under 10 px — under 5% relative depth by the derivation above. Schur-complementing it produces a dense prior block that is almost entirely noise, and that prior then freezes a linearisation point and can never be relinearised. You would be paying the full FEJ price for information that was not worth a variable in the first place.
• Dropping the IMU factor instead would break the preintegration chain. The IMU factor is a five-variable constraint on (pose, velocity, bias) at both ends. Delete the interval and velocity and bias go unconstrained across the gap — and velocity and bias are exactly the states that have no other observation. Reprojection sees position and orientation; only the IMU sees velocity. So you keep the inertial constraint and throw away the visual one, which is the opposite of what "drop the frame" sounds like it means.

MSCKF does not do this at all — the left-nullspace contrast

MSCKF appears in the "marginalise" row above, and it does marginalise features — but not with a Schur complement, and this is a genuine trap. MSCKF never puts a feature in the state vector in the first place. Its state is a sliding clone of camera poses only. So there is nothing to Schur-complement out; instead it eliminates the feature from the measurement equation, algebraically, before the update.

Take a feature tracked across M frames. Stack all its observations and linearise:

Compute A = left-nullspace(H_f), shape 2M × (2M − 3), by taking the last 2M − 3 columns of Q from a QR factorisation of H_f. By construction A^TH_f = 0, so multiplying through by A^T:

The feature is gone. What remains is a constraint on the poses alone, with the feature never having entered the state and never needing a covariance entry.

Do the dimension count for M = 6 (a feature tracked across six cloned frames): the stacked system has 2M = 12 rows. H_f is 12×3 with rank 3, so its left nullspace has dimension 12 − 3 = 9. After projection you have 9 rows of pose-only constraint, the 3-D feature is eliminated, and the state dimension is unchanged at 15N — no fill-in in the state at all, ever. Compare with the sliding-window smoother, which would have carried that feature as 1 to 3 extra state dimensions and then paid 2ab² to remove it.

Marginalisation with FEJ, from scratch

One line of the class below carries the whole idea, and it is the line people copy without understanding: return self.Lam, self.eta - self.Lam @ dx. Derive it first, because "the residual moves and the Jacobian does not" is a slogan, and a slogan will not survive "show me why the update to η is exactly −Λδx."

Setup. When you marginalise you get a quadratic in the delta coordinates about the linearisation point. Write d = x − x_lin. The prior's contribution to the cost is

Every solver iteration, though, the optimiser wants its normal equations expressed about the current estimate x_now, not about x_lin. Let δx = x_now − x_lin be how far you have drifted since marginalisation, and let d_new = x − x_now be the step the solver is about to choose. Then d = δx + d_new. Substitute and expand:

Expand the square. Λ is symmetric, so the two cross terms are equal and combine:

Collect the terms by their order in d_new:

Matching the standard form ½d_new^TΛd_new − η_new^Td_new + const gives η_new = η − Λδx, which is the line of code. Remember the constant-term point — it is the reason a fixed-lag smoother's reported chi-square jumps when you re-anchor a prior even though the estimate does not move at all, which is a real and confusing artefact in logs.

Do not take that on trust. Four lines confirm it numerically: evaluate the same physical point x* two ways, once as a delta about x_lin using (Λ, η) and once as a delta about x_now using (Λ, η_new), and check the difference is exactly the predicted constant.

python
# Verify eta_new = eta - Lam @ dx by re-expressing the SAME quadratic twice.
rng = np.random.default_rng(0)
M    = rng.standard_normal((5, 5)); Lam = M @ M.T + 5 * np.eye(5)   # SPD
eta  = rng.standard_normal(5)
dx   = rng.standard_normal(5) * 0.1          # drift since marginalisation
eta_new = eta - Lam @ dx                     # the line under test

cost = lambda L, e, d: 0.5 * d @ L @ d - e @ d
d_star  = rng.standard_normal(5)              # any test point, as a delta from x_lin
const   = 0.5 * dx @ Lam @ dx - eta @ dx      # the discarded constant
assert abs(cost(Lam, eta, d_star) - (cost(Lam, eta_new, d_star - dx) + const)) < 1e-12

# and the thing that actually matters: the MINIMISER is the same point in the world.
x_lin = np.zeros(5)
assert np.allclose(x_lin + np.linalg.solve(Lam, eta),
                   (x_lin + dx) + np.linalg.solve(Lam, eta_new), atol=1e-12)
print("re-anchoring is exact; cost differs by the constant %.6f only" % const)

The second assertion is the one to point at: the argmin expressed in world coordinates is identical. Re-anchoring a marginalisation prior to a new linearisation point is not an approximation and does not lose information — it is a change of variables. What FEJ freezes is Λ, not the anchor.

python
import numpy as np

class MargPrior:
    """A dense Gaussian prior produced by Schur-complementing states out.
    The linearisation point is stored WITH it and never updated. That is FEJ."""

    def __init__(self, Lam, eta, drop, keep, x_lin):
        A = Lam[np.ix_(drop, drop)]
        B = Lam[np.ix_(keep, drop)]
        self.Lam   = Lam[np.ix_(keep, keep)] - B @ np.linalg.solve(A, Lam[np.ix_(drop, keep)])
        self.eta   = eta[keep]              - B @ np.linalg.solve(A, eta[drop])
        self.x_lin = x_lin[keep].copy()     # FROZEN. This is the whole trick.
        self.keep  = keep

    def contribute(self, x_now):
        """Return (Lambda, eta) to add to the current normal equations.
        The RESIDUAL moves with the estimate; the JACOBIAN does not."""
        dx = x_now[self.keep] - self.x_lin      # how far we have drifted since
        # the prior in delta coordinates about x_lin, re-expressed about x_now:
        return self.Lam, self.eta - self.Lam @ dx

    # The bug you must NOT write:
    #   def relinearize(self, x_now):  self.x_lin = x_now[self.keep]
    # It looks like an improvement. It collapses the unobservable subspace.

And the same idea in GTSAM, where the fixed-lag smoother owns the bookkeeping:

python
import gtsam
from gtsam import BatchFixedLagSmoother, ISAM2, ISAM2Params

# Option A — fixed lag. States older than `lag` seconds are marginalised.
smoother = BatchFixedLagSmoother(lag=3.0)
smoother.update(new_factors, new_values, key_timestamps)
#   internally: build the elimination ordering so the expiring keys come FIRST,
#   eliminate them, keep the resulting clique as a LinearContainerFactor.

# Option B — incremental, keep everything, relinearise only what moved.
p = ISAM2Params()
p.relinearizeThreshold = 0.01     # relinearise a variable once it has moved this far
p.relinearizeSkip      = 1        # check every update, not every 10th
isam = ISAM2(p)
isam.update(new_factors, new_values)
est = isam.calculateEstimate()

The window that drifts while every residual looks healthy

Field report: a sliding-window VIO on a ground robot. Over a ten-minute mission the yaw estimate drifts about 6°. The reported yaw standard deviation falls monotonically to 0.1°. Reprojection errors are 0.4 px RMS throughout — textbook. IMU residuals are inside their expected band. No solver failures, no rejected steps, no divergence.

The metric that reveals it. Two options, one cheap and one definitive:

python
# Definitive: count the unobservable directions of the linearised window.
# Monocular VIO must have exactly 4 (global x, y, z, and yaw about gravity).
sv   = np.linalg.svd(J_stacked, compute_uv=False)
tol  = sv[0] * max(J_stacked.shape) * np.finfo(float).eps
print("nullspace dim =", int((sv < tol).sum()))     # FEJ: 4.  no FEJ: 3.

# Cheap field test: drive a closed loop and stop where you started.
# Plot reported yaw sigma against accumulated |yaw rate| dt.
# FEJ:    sigma grows, slowly. No FEJ: sigma falls, monotonically.
# A yaw sigma that DECREASES while the robot is turning is impossible.

A second failure: the prior that quietly stops being positive definite

The FEJ bug above is a modelling failure and it lives in every fixed-lag textbook. This second one is a numerical failure, it is specific to sliding windows, and it will not appear in any textbook because it only shows up after a few hundred keyframes on a real robot.

Field report: an embedded VIO in single precision, running fine for four to six minutes. Then, intermittently, one keyframe where the solver reports Cholesky failed: matrix is not positive definite, LM rejects every step and ratchets λ up by 10× each time until it hits its cap, the pose output freezes at the last accepted estimate — and then the next keyframe solves normally and everything looks fine again. Reprojection errors are healthy on either side. There is nothing wrong with the data.

The mechanism. Each marginalisation applies Λ' = Λ_bb − Λ_baΛ_aa⁻¹Λ_ab. That is a subtraction of two nearly equal quantities along whatever directions were weakly constrained — and the gauge directions are, by construction, the weakest ones present. Two things then compound:

• Catastrophic cancellation. Forming Λ = J^TJ squares the condition number: κ(Λ) = κ(J)². A perfectly ordinary κ(J) = 10⁴ gives κ(Λ) = 10⁸, and float32 carries only 2⁻²⁴ ≈ 6×10⁻⁸ of relative precision. You have no significant digits left in the smallest direction after one Schur complement, let alone four hundred.
• The error is not zero-mean, it accumulates. The output of marginalisation k is the input to marginalisation k+1. The tiny negative perturbation you introduced is now part of the matrix being subtracted from next time, so it compounds rather than averaging out. Asymmetry does the same: Lam - B @ solve(A, C) is not exactly symmetric in floating point, and an asymmetric matrix has complex eigenvalue pairs that Cholesky simply refuses.

The metric that reveals it — log it per keyframe, it costs microseconds:

python
# One line per keyframe. On a 50x50 prior this is tens of microseconds.
ev = np.linalg.eigvalsh(prior.Lam)          # symmetric eigenvalues, ascending
ratio = ev[0] / ev[-1]                       # min / max
print("kf %d  lam_min %.3e  lam_max %.3e  ratio %.3e  asym %.3e" %
      (kf, ev[0], ev[-1], ratio,
       np.abs(prior.Lam - prior.Lam.T).max() / np.abs(prior.Lam).max()))

# HEALTHY : ratio stays above ~1e-6 and wanders without trend; asym ~1e-16 (float64).
# BROKEN  : ratio walks DOWN monotonically - 1e-6 -> 1e-9 -> 1e-12 -> ~1e-14 over a
#           few hundred keyframes - then ev[0] crosses zero and Cholesky refuses.
#           asym climbs to ~1e-7 in float32 long before ev[0] goes negative, so it
#           is the EARLIER alarm of the two. Alarm on asym > 1e-9.

Why the monotone walk is the whole diagnosis. A one-off numerical accident gives you a bad eigenvalue on one keyframe and a good one on the next. A drift from 10⁻⁶ to 10⁻¹⁴ over 400 keyframes is an accumulator, and the only accumulator in the window is the prior — every other object is rebuilt from raw measurements each keyframe. Plot λ_min/λ_max on a log axis against keyframe index; a straight downward line is a signed diagnosis.

The frontier: making the prior sparse again

• Generic Linear Constraints (Carlevaris-Bianco & Eustice, IROS 2013) and Nonlinear Factor Recovery (Mazuran, Tipaldi, Spinello & Burgard, IJRR 2016). Instead of keeping the dense marginal, approximate it with a sparse set of nonlinear factors chosen to match the marginal's information as closely as possible in KL divergence. You give up exactness to get sparsity and relinearisability back.
• Square-root marginalisation (Demmel, Schubert, Sommer, Cremers, Usenko, ICCV 2021). Do the marginalisation on the QR factor rather than the information matrix, so the condition number is not squared. They report a fixed-lag smoother that runs in single precision without loss.
• Learned keyframe policies. A small line of 2023–24 work replaces the parallax/count heuristics with a learned "will this frame reduce uncertainty?" predictor. Worth knowing exists; worth being sceptical about in practice, because the failure mode — a policy that stops emitting keyframes in a scene it has not seen — is silent and catastrophic.

Chapter 4: Solvers & Libraries

You have used GTSAM. Why not Ceres? And why does Ceres default to Levenberg–Marquardt rather than plain Gauss–Newton — what actually goes wrong without the damping?

This is a two-part question wearing one hat. The first part is about what each library assumes. The second is a numerical-methods question that most robotics engineers have never had to answer with a number, and it is trivially answerable if you have ever watched Gauss–Newton diverge.

Gauss–Newton, and the one line where it goes wrong

Everything so far has been Gauss–Newton: linearise, solve the normal equations, apply the step, repeat.

It is the Newton method with the second-derivative term dropped — the true Hessian of ½r^TWr is J^TWJ + ∑_i w_ir_i∇²r_i, and Gauss–Newton throws away the second sum. That is a good trade when the residuals are small (near the solution) and a terrible one when they are large (far from it).

The failure is not subtle. Gauss–Newton computes the step that would be exactly right if the model were linear, and then takes all of it. When the linearisation is only valid over a centimetre and the step is five metres, the step is nonsense.

Worked example: Gauss–Newton blowing up, with numbers

The smallest possible instance. One residual r(x) = x² − 4, so the answer is x = 2, and the cost is C(x) = ½r². Start at x = 0.1.

Evaluate. r = 0.01 − 4 = −3.99. J = dr/dx = 2x = 0.2. So Λ = J² = 0.04 and η = −Jr = −(0.2)(−3.99) = +0.798.

Gauss–Newton step.

Evaluate the new cost. r(20.05) = 402.0 − 4 = 398.0, so C = ½(398)² = 79,202, up from C(0.1) = ½(3.99)² = 7.96. The step made the cost ten thousand times worse.

Levenberg–Marquardt: a leash on the step

LM adds a damping term and, crucially, checks the result before committing:

Read the two limits, because they are the whole intuition:

• λ → 0: the step is pure Gauss–Newton. Fast, quadratic convergence near the solution.
• λ → ∞: δ → η/λ, which is a small step in the direction of steepest descent. Slow, but it cannot overshoot.
• Marquardt's D = diag(Λ) makes the damping scale-invariant: a variable measured in millimetres and one measured in radians get damped proportionally to their own curvature rather than by a single absolute number. This is why Ceres and g2o use it.

The algorithm is then: take the step, evaluate the true new cost, and if it went down, accept and reduce λ (trust the model more); if it went up, reject, increase λ, and try again from the same point.

Worked example: the same problem, LM, five iterations by hand

Start at x = 0.1 with λ = 100 and D = diag(Λ). The damped system is (Λ + λΛ)δ = η, so δ = η / (Λ(1 + λ)).

Verify the first row by hand: Λ = J² = (0.2)² = 0.04, so δ = 0.798 / (0.04 × 101) = 0.798 / 4.04 = 0.197525. Then x = 0.297525, r = 0.088521 − 4 = −3.911479, and C = ½(3.911479)² = 7.649834 — lower than 7.960050, so the step is accepted.

Dogleg, and when it beats LM

Powell's dogleg is the third option every library ships. Instead of damping, it defines an explicit trust region of radius Δ and picks the best step inside it: the Gauss–Newton step if it fits, the Cauchy (steepest-descent) point if even that is too far, and a blend of the two along a two-segment path otherwise — hence "dogleg".

Worked example: computing a dogleg step, every number

"How do you actually compute the dogleg step?" is the guaranteed follow-up, and the answer is four short stages. Two variables keeps every intermediate printable. Take the linearised system

This is a deliberately awkward block: its eigenvalues are 8.531 and 0.469, so κ(Λ) = 18.2. One direction is stiff, one is soft — the shape you get whenever a well-observed image-plane direction sits next to a badly-observed depth.

Stage 1 — the Gauss–Newton step, and does it fit? det Λ = (8)(1) − (2)(2) = 4, so

‖δ_GN‖ = √(0.0625 + 2.25) = √2.3125 = 1.520691. That is bigger than Δ = 1.0, so the full GN step is outside the trust region. Stage 1 fails, and we go on.

Stage 2 — the Cauchy point, derived not quoted. The Cauchy point is the minimiser of the same quadratic model restricted to the steepest-descent ray δ = αη. Substitute that ray into the model m(δ) = −η^Tδ + ½δ^TΛδ:

Now evaluate the two inner products as scalars. η^Tη = 1² + 1² = 2. For the denominator, first Λη = [8(1)+2(1), 2(1)+1(1)]^T = [10, 3]^T, then η^T(Λη) = 1(10) + 1(3) = 13. So

0.217571 < 1.0, so the Cauchy point is comfortably inside the region. We are in the interesting case: ‖δ_C‖ < Δ < ‖δ_GN‖. The step lives on the second leg of the dogleg.

Stage 3 — the blending scalar τ, as an explicit quadratic. Walk from δ_C toward δ_GN and stop when you hit the boundary:

Write d = δ_GN − δ_C = [−0.250000 − 0.153846, 1.500000 − 0.153846]^T = [−0.403846, 1.346154]^T. Expanding ‖δ_C + τd‖² − Δ² = 0 gives aτ² + bτ + c = 0 with

Discriminant: b² − 4ac = 0.0840660 − 4(1.9752219)(−0.9526627) = 0.0840660 + 7.5268807 = 7.6109467, so √(b²−4ac) = 2.7587944. The positive root is

Stage 4 — the step.

Check it: ‖δ_DL‖ = √(0.0097101 + 0.9902897) = √0.9999998 = 1.000000. Exactly on the trust-region boundary, which is what stage 3 solved for.

Was it worth the arithmetic? Score each candidate step by the reduction the model predicts, η^Tδ − ½δ^TΛδ:

The dogleg step captures 89.4% of the full Gauss–Newton reduction while obeying a trust radius the GN step violates by 52%. Taking the Cauchy point instead — which is what a naive "too far, fall back to gradient descent" implementation does — would have thrown away three quarters of the available progress.

Now the sentence about rejection, made concrete. Suppose the true cost does not fall by anything like 0.559 and the step is rejected. Dogleg halves the radius to Δ = 0.5 and re-solves. Nothing about Λ, δ_GN, δ_C, a or b changes — only c:

That retry cost two multiplies, one subtraction and one square root — call it ten flops. The equivalent LM retry means picking a new λ and factorising (Λ + λD) again. On the 5,000-pose graph from the earlier design tables that factorisation is 40 ms, and a bad iteration commonly needs three tries: 120 ms of pure re-factorisation versus ~10 flops. That is the whole argument, and it is why the row above says a rejected dogleg step is free.

For completeness, the λ that would give LM a step of norm exactly 1.0 on this system is λ = 0.2107 with Marquardt damping D = diag(Λ) — but LM has no way to know that in advance. It has to guess λ, factorise, measure ‖δ‖ after the fact, and guess again. Dogleg parameterises the same curve by the thing you actually want to control.

python
import numpy as np

def dogleg_step(Lam, eta, Delta):
    """Powell's dogleg. Lam (n,n) SPD, eta (n,), trust radius Delta.
       Returns (step, which_leg). ONE factorisation, reused for every Delta."""
    L      = np.linalg.cholesky(Lam)                       # the expensive part — do it once
    d_gn   = np.linalg.solve(L.T, np.linalg.solve(L, eta))
    if np.linalg.norm(d_gn) <= Delta:
        return d_gn, "gauss-newton"                        # stage 1: it fits
    # stage 2: Cauchy point = model minimiser along the steepest-descent ray
    alpha  = (eta @ eta) / (eta @ Lam @ eta)
    d_c    = alpha * eta
    if np.linalg.norm(d_c) >= Delta:                       # even that is too far
        return Delta * d_c / np.linalg.norm(d_c), "cauchy"
    # stage 3: walk d_c -> d_gn, stop on the boundary. a*t^2 + b*t + c = 0
    d      = d_gn - d_c
    a      = d @ d
    b      = 2.0 * (d_c @ d)
    c      = d_c @ d_c - Delta * Delta                     # < 0 by the check above
    tau    = (-b + np.sqrt(b*b - 4*a*c)) / (2*a)          # exactly one positive root
    return d_c + tau * d, "dogleg"

# the worked example above, to the digit
Lam = np.array([[8., 2.], [2., 1.]]); eta = np.array([1., 1.])
print(dogleg_step(Lam, eta, 1.0))   # ([-0.09854,  0.99513], 'dogleg')
print(dogleg_step(Lam, eta, 0.5))   # ([ 0.05077,  0.49742], 'dogleg')  <- no new Cholesky
print(dogleg_step(Lam, eta, 2.0))   # ([-0.25   ,  1.5    ], 'gauss-newton')

The line that carries the idea: L is computed before the Delta loop, not inside it. Every rejected step re-enters at the tau line. In a real implementation you hoist the Cholesky and both candidate steps out of the trust-region loop entirely, so a rejection touches nothing but three scalars.

Under LM sits a linear solve, and that is the real choice

Every iteration of every method above ends in "solve Λδ = η". How you solve it is where the orders of magnitude live.

The condition-number argument, in digits

This is the one piece of numerical analysis every SLAM backend engineer genuinely needs, and it fits in three lines.

Forming the normal equations squares the condition number: κ(J^TJ) = κ(J)². In IEEE double precision the machine epsilon is 2.2 × 10⁻¹⁶, roughly 16 significant digits. The digits you keep after a solve are approximately log₁₀(1/ε) − log₁₀κ.

κ(J) = 10⁵ is not exotic. It happens the moment your problem mixes a well-observed direction (along-track position, constrained by a hundred features) with a badly observed one (scale in a nearly-constant-velocity segment). Six surviving digits is still fine for metres and radians; in single precision (ε ≈ 1.2 × 10⁻⁷, 7 digits) the same problem returns nothing.

The libraries, and what each one assumes about you

Trust region vs line search, said once

Both LM and dogleg are trust-region methods: they decide how far to trust the model, then compute a step inside that region. The alternative family is line search: compute a direction first, then hunt along it for a good step length.

Robotics backends are almost universally trust-region, and the reason is worth knowing. A line search needs several cost evaluations per iteration, and in a SLAM problem one cost evaluation means re-evaluating every residual in the graph — hundreds of thousands of them. A trust-region method makes one proposal, evaluates it once, and either takes it or shrinks. That is a single cost evaluation per attempt.

Ceres exposes both (TRUST_REGION and LINE_SEARCH) and its own documentation recommends trust region for essentially every bundle-adjustment-shaped problem. Being able to say why — the cost of a residual sweep — is a better answer than naming the default.

Where the time actually goes

Anyone who has not profiled a backend assumes the linear solve dominates. In a small window it does not, and the profile is worth memorising because it changes what you would optimise.

Read the summary, not the wall clock. Ceres' Solver::Summary::FullReport() prints exactly this breakdown, plus the iteration table with per-step cost, cost change, gradient norm, step norm and trust-region radius. GTSAM's ISAM2Result gives you variablesRelinearized and variablesReeliminated — the two numbers that tell you whether the incremental machinery is actually helping. Naming these fields is a cheap and very reliable signal that you have shipped something.

Robust kernels, and the correction everyone forgets

Every backend wraps its data-association-derived factors in a robust kernel, because one false loop closure with a quadratic cost will destroy a map. The theory of Huber and Cauchy lives in Uncertainty, Least Squares & Robust Costs; what belongs here is how the solver actually implements it, because that is the follow-up.

A robust cost replaces ½s with ρ(s), where s = r^TWr is the squared Mahalanobis norm. The solver does not rewrite its linear algebra — it rescales the block:

The first term is just iteratively reweighted least squares — multiply the block by the weight ρ'(s). The second term is the Triggs correction (Triggs et al., Bundle Adjustment — A Modern Synthesis, 2000), and it is what makes the linearisation a true second-order model of the robustified cost.

Worked example: watching the Triggs block go indefinite

That table row makes a failure claim, so here is the failure, in one scalar. Take the Cauchy kernel with scale c = 1:

Differentiate it twice — both derivatives, shown, because the second one is the whole point:

Now a residual that is a genuine outlier: r = 3 with W = 1, so the squared Mahalanobis norm is s = r^TWr = 9 — three sigma out. Evaluate both derivatives there:

Take a one-dimensional Jacobian J = 2 so the arithmetic is visible. The two pieces of the rescaled block are J^TWJ = 2(1)(2) = 4 and J^TWr = 2(1)(3) = 6. Substitute into the equation above:

The clamp, and what it restores. Ceres' Corrector (internal/ceres/corrector.cc) tests rho[2] <= 0 and, when it is, drops the rank-1 term entirely and scales by √ρ' alone. That is max(ρ'', 0) by another name:

Positive, factorisable, and identical to plain IRLS with weight 0.1. You lose the second-order term — convergence near the solution goes from quadratic to linear in the weights — and you keep a solver that runs.

Where the boundary actually sits. In one dimension the whole expression factors, which turns the clamp from a hack into a threshold you can quote:

For Cauchy, 2sρ''/ρ' = 2s · (−(1+s)⁻²) · (1+s) = −2s/(1+s), so the bracket is 1 − 2s/(1+s) = (1 − s)/(1 + s). It is negative whenever s > 1, i.e. whenever the residual exceeds the kernel scale c. Check it against our numbers: at s = 9 the bracket is (1−9)/(1+9) = −0.8, and (0.1)(4)(−0.8) = −0.32 — the same answer by a different route.

Read the last column downward. Every residual past the kernel scale produces a negative block. The reason a hand-rolled Triggs implementation appears to work on a clean dataset and dies the first time a false loop closure lands is that the clean dataset never left the first row of this table.

Picking the solver from the problem shape

n is the number of columns of J (the state dimension, so Λ is n×n); m is the number of rows of J (the total residual dimension). Where every m comes from — because "n = 307" tells you the size of Λ and nothing about the work that builds it, and you cannot size a problem you can only half-count:

Note that m > n in every row, which is the whole reason least squares is the right frame: you always have more equations than unknowns, and the ratio m/n (6.5 for the window, 3.75 for local BA, 1.16 for the pose graph, 3.0 for global BA) is a direct read on how over-constrained the problem is. A pose graph at m/n = 1.16 is the least redundant of the four, which is exactly why a single bad loop closure moves it so far.

The number to volunteer: a 307×307 dense Cholesky is 307³/3 ≈ 9.6 million flops — about 1 ms at 10 GFLOP/s on the reference core named above, and in practice 2–4 ms because the assembly and the memory traffic dominate at that size. Anyone who tells you the linear solve is the bottleneck in a sliding window has not profiled it; linearising 900 reprojection factors costs more than factoring the matrix they build.

Now back that claim with the second number, because on flops alone it looks false. Count one reprojection factor with analytic Jacobians: R p_w + t is 9 multiplies and 9 adds = 18; the perspective divide and K are ~7; the weighted residual is ~8; the 2×3 ∂π/∂p_c is ~10; chaining it into the 2×6 pose block is 36 multiplies and 24 adds = 60; the 2×3 point block is ~30. Call it ~180 flops per factor. So:

Levenberg–Marquardt in thirty lines

python
import numpy as np

def lm(residual, jacobian, x0, iters=30, lam=1e-3, tol=1e-10):
    """residual(x) -> (m,) ; jacobian(x) -> (m, n). Marquardt damping."""
    x    = np.asarray(x0, dtype=float).copy()
    r    = residual(x)
    cost = 0.5 * r @ r
    nu   = 2.0                            # Nielsen's rejection multiplier — DOUBLES each reject
    for it in range(iters):
        J   = jacobian(x)
        Lam = J.T @ J                       # the information matrix
        eta = -J.T @ r
        D   = np.diag(np.maximum(np.diag(Lam), 1e-12))   # Marquardt: scale-invariant
        while True:
            try:
                L  = np.linalg.cholesky(Lam + lam * D)          # never inv()
                dx = np.linalg.solve(L.T, np.linalg.solve(L, eta))
            except np.linalg.LinAlgError:
                lam *= nu; nu *= 2; continue                     # not PD: damp harder
            r_new = residual(x + dx)
            c_new = 0.5 * r_new @ r_new
            # gain ratio: actual reduction / reduction the model PREDICTED
            pred  = 0.5 * (dx @ (lam * D @ dx + eta))
            rho   = (cost - c_new) / max(pred, 1e-300)
            if rho > 0:                                       # accept
                x, r, cost = x + dx, r_new, c_new
                lam = max(lam * max(1/3, 1 - (2*rho - 1)**3), 1e-12)
                nu  = 2.0                                    # reset the ratchet on every accept
                break
            lam *= nu; nu *= 2                              # reject: 2x, then 4x, then 8x…
            if lam > 1e12: return x, cost, it              # genuinely stuck
        if np.linalg.norm(dx) < tol: break
    return x, cost, it

Two lines worth defending. The rho line is the gain ratio — the ratio of the cost reduction you actually got to the one the quadratic model promised. Nielsen's update rule (the 1 - (2ρ-1)3 factor) is what Ceres uses; a rho near 1 means the model is trustworthy, so loosen the leash a lot. And the except LinAlgError branch is not defensive programming — a Cholesky failure is exactly the signal that the damped matrix is not positive definite, which means λ is too small.

Nielsen's rule is a pair, and half of it is the part people drop. The decrease factor above is only the accept half; the reject half is the nu ratchet — lam *= nu; nu *= 2, with nu reset to 2 on every accept. Ceres implements exactly this pair (see TrustRegionMinimizer: radius_ /= decrease_factor_ then decrease_factor_ *= 2). Quoting the accept half alone and pairing it with a bare lam *= 10, as an earlier draft of this code did, is a claim of fidelity to Ceres that the code does not have — and it is precisely the level of detail worth checking whenever you cite a library's default.

The two rules differ in a way you can put a number on. After k consecutive rejections from the same point:

The flat rule is geometric, 10^k. Nielsen's is super-geometric, 2^k(k+1)/2, so it is gentler for the first five rejections and then overtakes hard. That shape is the point: one unlucky step (a single outlier that just entered the window) barely moves the leash, so you do not throw away a good λ over noise; but a genuinely broken linearisation escalates out of the region in a handful of iterations instead of grinding. A flat 10× is neither patient nor decisive.

The analytic Jacobian that is subtly wrong

Field report: a new residual type was added by hand with hand-derived Jacobians. The solver still converges, the cost still goes down, and the map still looks right. But an optimisation that used to take 5 iterations now takes 180, and λ ends the run at 10⁴ instead of 10⁻⁶.

The metric that reveals it: a finite-difference gradient check, and the threshold matters.

python
def check_jacobian(residual, jacobian, x, eps=1e-6):
    J  = jacobian(x)
    Jn = np.zeros_like(J)
    for k in range(len(x)):                          # CENTRAL differences: O(eps^2)
        dp = x.copy(); dp[k] += eps
        dm = x.copy(); dm[k] -= eps
        Jn[:, k] = (residual(dp) - residual(dm)) / (2 * eps)
    rel = np.abs(J - Jn).max() / max(1.0, np.abs(Jn).max())
    print(f"max relative error {rel:.3e}")
    return rel
# central differences at eps=1e-6 are good to ~1e-9, so:
#   rel < 1e-7   correct
#   rel ~ 1e-3   a sign flip, a missing chain-rule term, or degrees vs radians
#   rel ~ 1.0    wrong variable ordering, or a transpose

Ceres ships this as ceres::GradientChecker and will run it on every residual block if you set Solver::Options::check_gradients = true. GTSAM has gtsam::numericalDerivative11 and friends. Run it in a unit test on every new factor type, once, forever. A wrong Jacobian is the single most expensive bug in this codebase because it degrades gracefully — it never crashes, it just quietly makes everything worse.

Where solvers are going

• SymForce (Martiros, Miller et al., RSS 2022). Symbolic residual definition, common-subexpression elimination, and code generation to flat C++. The pitch is that most robotics solve time is spent evaluating Jacobians rather than factoring matrices — and once you accept that, symbolic optimisation of the residual is the lever nobody was pulling.
• Certifiable optimality. SE-Sync (Rosen, Carlone, Bandeira & Leonard, IJRR 2019) solves a rank-restricted semidefinite relaxation of pose-graph SLAM by Riemannian staircase optimisation and returns a certificate that the solution is the global minimum. For realistic noise the relaxation is tight, which turns "did LM find a local minimum?" from a worry into a checkable property.
• GPU and distributed. MegBA (Ren et al., ECCV 2022) partitions the arrowhead across GPUs; DBA-Fusion and similar 2023–24 systems fuse learned dense-flow frontends with classical BA backends on-GPU.
• Differentiable solvers. Theseus (Pineda et al., NeurIPS 2022) is a PyTorch library that makes the entire nonlinear least-squares solve differentiable, so the noise models and even the residuals can be learned end-to-end while the optimisation stays classical and inspectable.

Chapter 5: VIO & IMU Preintegration

Your IMU runs at 200 hertz and your keyframes at four. So there are fifty samples between keyframes. Put them in the graph.

This is the problem where people who can draw a factor graph fall over, because the obvious answer — "add a factor per IMU sample" — is wrong for a reason that only becomes visible when you count the work.

Why the obvious answer fails: count the integrations

Suppose you add one variable per IMU sample. A ten-keyframe window then holds 10 × 50 = 500 states of 15 numbers each = 7,500 unknowns instead of 150. That alone kills it — 7,500³/3 = 1.4 × 10¹¹ flops per iteration, about 14 seconds.

So instead you keep only the keyframe states and make the IMU produce a single constraint between consecutive keyframes. But now you hit the real problem. Integrating the IMU forward from keyframe i to keyframe j requires knowing the state at i:

Every term in that sum depends on R_k, the orientation at sample k, which depends on R_i. So the moment the optimiser adjusts pose i — which it does on every iteration — all fifty integration steps must be redone.

The change of variables, in words first

Write the integral in the body frame of the first keyframe rather than the world frame. That sentence is the summary; the algebra underneath it is four lines long, and you should be able to produce all four at a whiteboard, because "derive preintegration for me" is a question that gets asked exactly this way.

Derive it in four lines — do not memorise the boxes

Line 1 — the exact world-frame propagation. Nothing clever has happened yet. This is Newton's second law integrated over a window of N steps of length Δt, with Δt_ij = NΔt, in the world frame:

where Δv^W_ik = ∑_m<k R_m(a_m − b_a)Δt is the world-frame velocity change accumulated inside the window. Gravity has already been pulled out front, and that step is worth doing once so you never have to think about it again: each of the N steps contributes ½gΔt² directly plus gΔt of carried velocity to every later step, so the total is ½gΔt²[N + 2·N(N−1)/2] = ½g(NΔt)² = ½gΔt_ij². Exactly. Gravity is constant in the world frame, so it never needed to be inside the sum at all. (Notice this is the same two-term structure — carried velocity plus a half-acceleration term — as the hand-worked table further down. The derivation and the arithmetic are the same object.)

Line 2 — chain the rotations and factor R_i out. Every orientation inside the window is the keyframe orientation followed by whatever the gyro has measured since:

ΔR_ik is built from gyro samples and the gyro bias and nothing else. Substitute it everywhere R_k appears. R_i carries no k, so it comes straight out of the sum on the left:

with Δv_ik = R_i^TΔv^W_ik = ∑_m<kΔR_im(a_m−b_a)Δt, the same velocity change expressed in frame i.

Line 3 — this is the whole trick. Left-multiply both sides by R_i^T. On the right, R_i^TR_i = I and the pose vanishes:

Line 4 — name the right-hand side and read off the residual. Call the right-hand side Δp_ij. Every symbol in it is a raw measurement or a bias. Every symbol that depends on where the robot actually is — p_i, v_i, R_i, g — is trapped on the left. Subtract Δp_ij from both sides and the equation becomes a residual that is zero when the states agree with the measurements:

Which gives the three preintegrated deltas in their usual boxed form:

Notice what is not in those expressions: p_i, v_i, R_i, and gravity. The deltas depend only on the raw measurements and the biases. The optimiser can move the keyframe states as much as it likes; the deltas are unaffected.

Gravity and the initial state re-enter only when the factor is evaluated — which is line 4 of the derivation, read right to left:

That residual is cheap: a few matrix-vector products. It is the sum over fifty samples that you avoided. And it is worth saying out loud that this is not a new equation anyone had to invent — it is the world-frame propagation you wrote in line 1, rearranged so the measurement-only part sits on one side and the state-only part sits on the other. Every factor in this whole lesson has that shape. The art was noticing that the rearrangement was possible.

Worked example: preintegrate four samples, entirely by hand

Drop to one dimension — no rotation, no gravity — so the arithmetic is visible. Four accelerometer samples at Δt = 0.01 s, with an accelerometer bias estimate b_a = 0.10 m/s²:

The velocity delta is a plain sum:

The position delta needs care, because each step carries the velocity accumulated so far. Step k contributes Δv_kΔt + ½(a_k−b)Δt², where Δv_k is the delta-v before that sample:

Now prove it is reusable. Take an initial state p₀ = 5.0 m, v₀ = 2.0 m/s, over T = 4 × 0.01 = 0.04 s.

Direct step-by-step integration from (5.0, 2.0) gives 5.08074000. Identical. Now change the initial state to p₀ = 9.0, v₀ = −1.0 without touching the deltas:

Direct integration gives 8.96074000. Zero re-integration, exact answer. That is the whole idea, and you can demonstrate it on a whiteboard in ninety seconds.

The catch: the deltas do depend on the bias

Look back at the definitions. b_a and b_g are inside the sums. And the bias is a state the optimiser estimates — it will change on every iteration. If a bias change forced a re-integration, you would be back where you started.

The fix is a first-order Taylor expansion in the bias, using Jacobians accumulated during the same integration pass:

Compute the Jacobian for our example. Each sample subtracts b once, and each contributes Δt of velocity, so:

For position, each sample's bias error contributes both a direct ½Δt² and the accumulated velocity error times Δt:

Check that against the closed form: for constant Δt the answer must be −½T² = −½(0.04)² = −0.0008. ✓ That cross-check is exactly the kind of thing to do out loud.

Apply a bias update of δb_a = +0.02 m/s²:

Full re-integration with b = 0.12 gives Δv = 0.0352 and Δp = 0.000724. Exactly equal — because in one dimension without rotation the bias enters linearly, so the first-order correction is not an approximation at all.

Worked example 3: the same correction on SO(3), where it is not exact

Everything above lives in the regime where the answer is exact by construction, so it proves the mechanism and nothing about the threshold. Citing "0.01 rad/s" without a number behind it is trivia. So here is the rotation arithmetic, done the same way — by hand, every entry — and then the closed form it hands you, which is the thing you actually want to carry away.

Setup. Two gyro samples, constant rate ω = (0, 0, 2.0) rad/s about the body z axis, Δt = 0.01 s, so T = 0.02 s. Current gyro bias estimate b_g = 0. The optimiser hands back a bias change δb_g = (0.02, 0, 0) rad/s.

Step 1: the incremental rotation, all nine entries. The per-step rotation vector is φ = (ω − b_g)Δt = (0, 0, 0.02), so ‖φ‖ = 0.02 rad. Rodrigues with K = hat(φ/‖φ‖) = hat(ẑ):

Both steps are identical because ω is constant, so ΔR(b_g) = Exp(φ)² = R_z(0.04): cos 0.04 = 0.999200107, sin 0.04 = 0.039989334.

Step 2: the right Jacobian of SO(3). This is the object that does not exist in the 1-D story, and it is where the whole difference lives. J_r(φ) is defined by Exp(φ + δφ) ≈ Exp(φ)·Exp(J_r(φ)δφ) — "how does a small nudge to the rotation vector show up as a small rotation applied on the right of the result":

At t = 0.02 the two scalar coefficients are (1 − cos t)/t² = 0.499983333 and (t − sin t)/t³ = 0.166663333 (the series ½ − t²/24 and 1/6 − t²/120 — which is also the small-angle branch you must code, because both expressions are 0/0 at t = 0). With hat(φ) having entries ±0.02 and hat(φ)² = diag(−0.0004, −0.0004, 0):

Step 3: accumulate the gyro-bias Jacobian. The recursion is J ← ΔR_k,k+1^T J − J_r(φ)Δt. Starting from J = 0:

After step 0: J = −J_r(φ)(0.01) = rows (−0.009999333, −0.000099997, 0), (+0.000099997, −0.009999333, 0), (0, 0, −0.01).

After step 1: J = Exp(φ)^T·J_step0 − J_r(φ)(0.01), giving

Sanity-check the (3,3) entry against the flat case: −0.02 = −T, exactly the accelerometer answer, because the z axis is the rotation axis and along it nothing rotates. The off-diagonal −0.000399947 terms are the entire SO(3) correction, and they are three orders of magnitude smaller — which already tells you the answer is going to be small.

Step 4: apply the correction. J_ΔR^g·δb_g is the first column times 0.02 = (−0.000399893, +0.0000079989, 0). Exponentiate that tiny vector (‖·‖ = 4.0×10⁻⁴) and right-multiply:

Step 5: the ground truth. Re-integrate from raw samples at the new bias. ψ = (ω − δb_g)Δt = (−0.0002, 0, 0.02), ‖ψ‖ = 0.020001000, and ΔR_exact = Exp(ψ)²:

Step 6: the residual angle. Compare the two the only way rotations may be compared — compose one with the inverse of the other and take the Log. Seven of the nine entries agree to eleven decimals; the disagreement is entirely in the (1,2)/(2,1) pair, and it is 5.33×10⁻¹⁰:

Now repeat at δb_g = 0.2 rad/s — ten times larger, twenty times VINS-Mono's threshold. Same machinery, and the residual comes out at 5.3329×10⁻⁸ rad = 3.06×10⁻⁶ degrees. Exactly one hundred times bigger for a ten-times-bigger bias step. That factor of 100 is not a coincidence and it is the most useful thing on this page: the first-order correction is correct to first order by construction, so the residual is second order in δb_g. It is not "approximately linear"; the linear part is exact, and what is left is a clean quadratic.

The closed form, so you can defend the threshold instead of quoting it

Run that arithmetic over a whole family of ω, T and δb_g (the widget below does exactly this, and it was checked against 75 numerically integrated cases with the worst disagreement under 1%) and the residual collapses onto a formula. For a constant rate ω with the bias change perpendicular to it, writing Θ = ωT for the total rotation swept in the window:

and for a window short enough that Θ is at most about 1 rad — which is every keyframe interval — the bracket is just Θ/12, so:

Check it against the hand example. Θ = 2.0 × 0.02 = 0.04, so (Θ − sinΘ)/(2Θ²) = (0.04 − 0.039989334)/(0.0032) = 1.0666×10⁻⁵/0.0032 = 3.3333×10⁻³, and (δb·T)² = (0.02×0.02)² = 1.6×10⁻⁷. Product = 5.333×10⁻¹⁰ rad, against the 5.3329×10⁻¹⁰ we integrated. ✓

Three exponents, and they are the whole answer to "when do you re-propagate": quadratic in the bias step, linear in the rotation rate, and cubic in the window length. The window is the dangerous one, because it is the term nobody thinks about.

Read the "specific force it leaks" column, not the "ΔR error" one. Nobody cares about a hundredth of a degree in the abstract. A tilt error ε in ΔR rotates gravity into the horizontal channel of the specific force, leaking g·sinε ≈ 9.81ε m/s² of fake acceleration into Δv and Δp. So there is a natural, physical criterion for "when does the truncation matter", and it needs no citation at all:

Two things to notice before moving on. The three curves are parallel: one delta, three initial states, and the shape never changes — that is the independence from line 3 of the derivation, drawn. And the first-order/re-integration difference stays at machine epsilon for every position of the bias slider, including ±0.5, because in one dimension the bias enters linearly and "first order" is not an approximation at all. To see the correction actually fail you need rotation, which is a different widget, because it is a different phenomenon.

Drive it in this order, and read the numbers — they are the argument.

1. Pull T down to 0.25 s (one keyframe interval) and push δb_g to the far right, 0.05 rad/s — five times VINS-Mono's threshold. The curve flattens onto the axis and the ε* rule leaves the top of the plot entirely. The readout says 2.9×10⁻⁴ degrees, which is 200× below the tilt that would leak a single accelerometer bias. A keyframe interval does not care about the gyro bias step. At all.
2. Put δb_g back to 0.01 and drag T from 0.25 s to 5 s. The residual goes 1.16×10⁻⁵° → 7.96×10⁻³°, a lift of 684× for a 20× longer window. From 0.25 s to 1 s it is 57× against the cubic law's 64× — the T³ term, visible — and then it flattens off, because Θ = ωT has passed a radian and the (Θ − sinΘ)/(2Θ²) coefficient stops growing. Two behaviours, one formula, both on the screen.
3. Now find the crossing that actually matters. Set δb_g = 0.05 and walk T up: 1.00 s gives 0.017°, 1.50 s gives 0.048°, and at 1.75 s the curve crosses the dotted 0.058° rule and the verdict flips to RE-PROPAGATE. Above that line the truncation is injecting more fake specific force than the accelerometer bias you are estimating, so the bias state absorbs it and every residual stays healthy while the estimate quietly degrades.

The dotted horizontal rule — not the dashed vertical one — is the real threshold. The vertical rule at 0.01 rad/s is a fixed heuristic with no knowledge of your window length; the horizontal rule is physics, and it moves the answer by more than an order of magnitude across the T slider. The verdict line under the plot tells you which of the two you are on the wrong side of, and when they disagree it says so, because "the rule fires but the arithmetic says you are fine" is the honest state of a 0.25 s keyframe at 0.02 rad/s — and being able to say that out loud, with the factor, is the difference between reciting a threshold and owning one.

Observability: what VIO can and cannot know

The other half of understanding VIO. Monocular visual-inertial odometry has exactly four unobservable degrees of freedom, and being able to list them — and say why the others are not on the list — is the whole story.

The degeneracies are where it gets interesting. "With excitation" is doing a lot of work in that table.

The inertial half of the stack, in numbers

Know which of those two factors you are describing. The code below builds gtsam.ImuFactor — a 9-dimensional residual with a 9×9 information matrix — and then adds the bias random walk as a separate BetweenFactorConstantBias. It is explicitly not the combined form. Quoting "15×15" while pointing at an ImuFactor is the kind of half-remembered detail that undermines everything after it, because a 9-dimensional residual cannot carry a 15×15 information matrix; the dimensions have to agree.

Why pick the split form? Because the two noise models have nothing to do with each other and you will want to tune them independently. The preintegration covariance comes from the gyro and accelerometer white noise densities and is propagated by the integration itself; the bias covariance comes from the random-walk densities and grows as √Δt. Keeping them in separate factors means you can loosen the bias walk on a thermally noisy unit without touching the measurement model at all — and when the bias estimate misbehaves, you can drop the bias factor's sigma to zero and re-run to see whether the bias state was the sink for your error. Pick CombinedImuFactor when you want the correlation between the preintegration error and the bias error carried explicitly (it is real, and the split form throws it away), or when factor count itself is the cost — on a very long window, halving the number of factors halves the graph-construction overhead. GTSAM's own examples default to the combined form; VINS-Mono, which is not GTSAM, is closer in spirit to the split one.

The dataflow, with the rate on every arrow. IMU callback at 200 Hz → append to the current preintegration accumulator (2 µs) → also propagate the published state forward (10 µs) → publish odometry at 200 Hz. Image callback at 20 Hz → track features (8 ms) → keyframe test → if yes, close the accumulator into a factor and hand the window to the optimiser (40–80 ms, own thread) → on completion, reset the propagation origin to the new optimised state.

The accumulator, then the library

python
import numpy as np

def hat(w): return np.array([[0,-w[2],w[1]],[w[2],0,-w[0]],[-w[1],w[0],0]])

def Exp(w):
    th = np.linalg.norm(w)
    if th < 1e-9: return np.eye(3) + hat(w)          # first order near zero
    K = hat(w / th)
    return np.eye(3) + np.sin(th)*K + (1-np.cos(th))*K@K   # Rodrigues

def rightJacobian(w):
    """Jr(w): Exp(w + dw) ~= Exp(w) @ Exp(Jr(w) @ dw). The object that makes the
    gyro-bias Jacobian differ from the flat accelerometer one."""
    t = np.linalg.norm(w); W = hat(w)
    if t < 1e-8: return np.eye(3)      # both coefficients below are 0/0 at t=0
    return (np.eye(3)
            - ((1 - np.cos(t)) / t**2) * W          # -> 1/2 - t^2/24
            + ((t - np.sin(t)) / t**3) * (W @ W))    # -> 1/6 - t^2/120

class Preintegrator:
    def __init__(self, ba, bg):
        self.ba, self.bg = ba.copy(), bg.copy()   # the bias this delta was built AT
        self.dR, self.dv, self.dp, self.dt = np.eye(3), np.zeros(3), np.zeros(3), 0.0
        self.JR_bg = np.zeros((3,3))       # d(dR)/d(bg)
        self.Jv_ba = np.zeros((3,3)); self.Jp_ba = np.zeros((3,3))
        self.Jv_bg = np.zeros((3,3)); self.Jp_bg = np.zeros((3,3))

    def add(self, a, w, dt):
        f  = a - self.ba
        # ORDER MATTERS: p uses the v from BEFORE this sample, v uses the R from before
        self.dp    += self.dv*dt + 0.5*self.dR@f*dt*dt
        self.Jp_ba += self.Jv_ba*dt - 0.5*self.dR*dt*dt
        self.Jp_bg += self.Jv_bg*dt - 0.5*self.dR@hat(f)@self.JR_bg*dt*dt
        self.dv    += self.dR@f*dt
        self.Jv_ba += -self.dR*dt
        self.Jv_bg += -self.dR@hat(f)@self.JR_bg*dt   # gyro bias reaches v THROUGH dR
        # --- the rotation and its bias Jacobian, LAST, using this step's increment ---
        phi = (w - self.bg)*dt
        dRk = Exp(phi)
        self.JR_bg  = dRk.T @ self.JR_bg - rightJacobian(phi)*dt
        self.dR     = self.dR @ dRk
        self.dt    += dt

    def corrected(self, ba_new, bg_new):
        """First-order bias update. Cheap. Valid only for SMALL dbg."""
        dba, dbg = ba_new - self.ba, bg_new - self.bg
        if np.linalg.norm(dbg) > 0.01:              # ~0.57 deg/s
            raise ValueError("gyro bias moved too far - re-propagate from raw samples")
        return (self.dR @ Exp(self.JR_bg @ dbg),
                self.dv + self.Jv_ba @ dba + self.Jv_bg @ dbg,
                self.dp + self.Jp_ba @ dba + self.Jp_bg @ dbg)

# The test that catches the bug this code used to have. Perturb the bias, correct
# first-order, and compare against a full re-integration at the new bias. If JR_bg
# is never accumulated the rotation correction is a silent no-op and the error stays
# FIRST order in db; with the recursion above it drops to SECOND order:
#   db scale   1e-4     1e-3     1e-2
#   correct    2.5e-11  2.5e-9   2.5e-7    <- x100 per decade  (second order)
#   broken     3.7e-5   3.7e-4   3.7e-3    <- x10  per decade  (first order)
# Assert the SLOPE, not a magnitude. A no-op correction still looks small.

The GTSAM form, where the same object is a factor:

python
import gtsam
params = gtsam.PreintegrationParams.MakeSharedU(9.81)      # U = z-up
params.setAccelerometerCovariance(np.eye(3) * 0.01**2)
params.setGyroscopeCovariance(np.eye(3) * 0.0017**2)
params.setIntegrationCovariance(np.eye(3) * 1e-8)          # discretisation error

pim = gtsam.PreintegratedImuMeasurements(params, bias_estimate)
for (a, w, dt) in imu_batch:
    pim.integrateMeasurement(a, w, dt)                        # once, on arrival

graph.add(gtsam.ImuFactor(X(i), V(i), X(j), V(j), B(i), pim))
graph.add(gtsam.BetweenFactorConstantBias(B(i), B(j),
          gtsam.imuBias.ConstantBias(),
          gtsam.noiseModel.Isotropic.Sigma(6, sigma_bias_rw * np.sqrt(dt_total))))
# ^ the bias RANDOM WALK factor. Forgetting it lets the bias jump arbitrarily
#   between keyframes, which absorbs real motion and destroys scale.

Scale that collapses on a motorway

Field report: a VIO stack on a delivery vehicle. In the city it is excellent, under 0.5% drift. On a straight motorway segment the estimated scale drifts by 8% over four minutes and the accelerometer bias estimate wanders to an implausible 0.4 m/s². Reprojection errors stay at 0.35 px throughout. No warnings.

The metric that reveals it, and the value that distinguishes it from its nearest decoy:

python
# Excitation monitor - runs online, needs no ground truth.
exc = np.std(np.linalg.norm(acc_window, axis=1) - 9.81)
#   > 0.5  m/s^2  : healthy, scale well constrained
#   0.05-0.5      : marginal, widen the scale prior
#   < 0.05        : scale UNOBSERVABLE - freeze it or bring in another sensor

# The nearest decoy is "too few features", which looks similar in the
# trajectory but has the OPPOSITE covariance signature:
#   too few features  -> position covariance GROWS, feature count falls
#   no excitation     -> position covariance looks fine, only SCALE degrades,
#                        and the accel bias state absorbs the error

Inertial estimation is moving fast

• Analytic and continuous preintegration. Eckenhoff, Geneva & Huang (IJRR 2019) derive closed-form preintegration under a piecewise-constant-measurement model, removing the discretisation error that the IntegrationCovariance term above is papering over.
• Learned inertial odometry. TLIO (Liu et al., RA-L 2020) trains a network to regress displacement from raw IMU over 1 s windows and feeds it as a measurement into an EKF, achieving usable pedestrian odometry with no camera at all. RoNIN (2020) is the same idea for handheld. The 2023–24 direction is learning the IMU noise model rather than the motion (AirIMU and similar), which is more honest: it keeps the physics and learns only what the datasheet gets wrong.
• Equivariant filtering. Fornasier, van Goor, Mahony & Weiss (ICRA 2022, T-RO 2023) build the VIO filter on the symmetry group of the problem so the linearisation error is structurally correct. This is the strongest current argument that the filter-versus-smoother question is not settled.
• Dense learned frontends, classical backends. DROID-SLAM (Teed & Deng, NeurIPS 2021) and its visual-inertial successors put a learned recurrent flow estimator in front of an unchanged bundle-adjustment layer. The backend in this lesson is exactly the layer they kept.

Chapter 6: The Sparsity Inspector

Everything in the last five chapters comes down to one number that nobody ever plots: the number of nonzeros in the Cholesky factor. Not in the matrix — in the factor. It is what you actually pay for, it is entirely determined by the elimination ordering, and it can differ by three orders of magnitude between two orderings of the same graph.

This chapter is an instrument for feeling that. Build a pose graph, choose an ordering, and step the elimination forward one variable at a time. Watch the fill-in edges appear in the graph and the fill-in entries appear in the factor. The flop counter at the bottom is the bill.

Six things to do with it

1. Set loop closures to 0 and landmarks to 0, then eliminate everything in natural order. Both sliders, not just the first one. You get 0 fill edges and 60 flops — and you can check that by hand: 16 poses in a chain give c_j = 1 for the first fifteen and 0 for the last, so 15×(1²+3·1) + 0 = 15×4 = 60. A chain eliminated end to end is a perfect elimination ordering — every variable you remove has exactly one surviving neighbour, so there is no pair to connect and no clique can form. This is why odometry-only estimation is cheap.
2. Keep landmarks at 0, add one loop closure, repeat. The chain became a cycle, and a cycle cannot be eliminated without fill: 6 fill edges, 102 flops, up from 60 on the identical number of variables. Watch the fill edge chase the elimination front around the loop — every step, the front node connects to the far end of the loop, and that edge is new. One measurement, +70% arithmetic.
3. Now put landmarks back to 4 with loop closures still at 0, and look again. The pose chain has no cycle in it at all, and yet you get 34 fill edges and 404 flops. Where did the cycles come from? Every landmark is a cycle. A landmark seen by three consecutive keyframes creates the triangle {p_k, p_k+1, m} — and, through the chain edges, a 4-cycle p_k–p_k+1–p_k+2–m–p_k. Any shared observation is a loop closure in the graph-theoretic sense; "loop closure" is just the name we give the ones that span a long time gap. This is the single most common misreading of a sparsity plot: engineers attribute the fill to the labelled loop closures and never notice the structure factors underneath, which on a feature-rich session are producing most of it.
4. Set landmarks to 8 and compare "Natural" against "Landmarks first". Natural eliminates poses and landmarks interleaved by index; landmarks-first eliminates every landmark before touching a pose. That is the Schur complement of Chapter 2, expressed purely as an ordering — and the flop bar shows what it buys.
5. Turn loop closures up to 8 and compare all four orderings. This is the situation in the debugging scenario: a graph that was cheap becomes expensive not because it grew, but because its connectivity changed and the ordering never adapted.
6. Watch the degree readout as you step. Fill created at a step is exactly (d choose 2) minus the edges that already existed among the neighbours. Eliminating a degree-2 node costs at most one edge; a degree-10 node costs up to 45. Minimum degree is greedy on exactly that number.

Check the instrument against arithmetic before you trust it

An instrument you have not calibrated is a decoration. So here are the exact numbers the widget produces at its default settings — 16 poses, 2 loop closures, 4 landmarks, which is 20 variables and 29 edges — for all four orderings. Every one of these is reproducible by hand with the table you will build two sections from now, and by the twenty-line Python function you will write one section from now.

Do one of them by hand so the rest are credible. Take the minimum-degree row and bucket its column counts: one c_j = 1 at the start and one at the end, so 2×(1+3) = 8; thirteen c_j = 2, so 13×(4+6) = 130; four c_j = 3, so 4×(9+9) = 72; one c_j = 0 contributing nothing. Total 8 + 130 + 72 = 210 — the number in the bar chart. Now the natural row the same way: five 3s → 5×18 = 90, ten 5s → 10×40 = 400, two 4s → 2×28 = 56, one 2 → 10, one 1 → 4, one 0. Total 560. Worst over best is 560/210 = 2.67×, which is what the readout under the bar chart prints.

Two things are worth noticing in that table beyond the totals. First, natural is not merely worse on average, it is worse in shape — its middle is a plateau of 5s, which is the elimination front carrying the outstanding loop closures with it, while minimum degree never once exceeds 3. Second, reverse and landmarks-first are nearly tied and both beat natural comfortably, which is the honest version of "the Schur complement is an ordering": landmarks-first is not magic, it is one particular good ordering, and on this graph a plain reverse sweep happens to be marginally better. On a real bundle-adjustment graph — thousands of landmarks, tens of cameras — landmarks-first wins by orders of magnitude, because there the structure block is 99% of the variables. Which ordering wins is a property of the graph, not of the name of the ordering. That sentence is the difference between having read about the Schur complement and having used it.

The data that actually flows through the solver

The instrument draws a graph, but a backend does not consume a graph — it consumes arrays with shapes and dtypes. Here is the entire pipeline for the 5,000-pose warehouse session used later in this chapter, so the counting above is anchored to something you could allocate.

What the flop model is doing — derived, not asserted

The bar chart is not a stopwatch, it is arithmetic you can check. Derive it in three steps from what a single elimination step physically does. Take the matrix at the moment column j is about to be eliminated, and split it by the pivot:

Step 1 — forming the column costs ~3c_j. The pivot is L_jj = √d: one square root, a constant, so it does not scale. The rest of the column is ℓ = b / √d, which is c_j divisions. Counting the load of b and the store of ℓ alongside the divide — because on a sparse factorisation you are memory-bound far more often than you are divide-bound — that is three touches per entry, 3c_j. The coefficient 3 is a memory-traffic bookkeeping constant, not a theorem; what matters is that this term is linear in c_j.

Step 2 — the rank-1 update costs c_j². Eliminating the pivot Schur-complements it out of the trailing block: S ← S − bb^T/d = S − ℓℓ^T. The outer product ℓℓ^T of a length-c_j vector with itself is a c_j×c_j matrix, and every one of those entries is one multiply and one subtract — one fused multiply-add. So the update touches exactly c_j × c_j = c_j² entries. (Exploiting symmetry halves it to c_j(c_j+1)/2; the model keeps the full square because the constant is irrelevant next to the shape, and because supernodal codes do work on full blocks.)

Step 3 — sum over columns. Every column is eliminated exactly once and the costs are additive:

That ℓℓ^T in step 2 is also where the fill comes from, which is why one derivation buys both facts. S may have a structural zero at (a, b), but ℓ_aℓ_b is generically nonzero whenever both a and b are neighbours of j — so the subtraction writes a value into a slot that was empty. Fill and flops are the same event counted two ways: fill counts the slots that changed from zero, flops counts every slot that was written at all.

Sanity-check the model against the dense case, with the sum done. If every column is full, c_j = n − 1 − j for j = 0 … n−1, so as j sweeps forward c_j sweeps the integers n−1, n−2, …, 1, 0. The quadratic term is therefore the sum of the first n squares:

The leading term is 2n³/6 = n³/3 — the textbook dense Cholesky count, recovered exactly. Put a number on the approximation so you know what "≈" is worth here. At n = 100:

The leading term overshoots by 4,983, which is 1.5% — and the gap is O(n²)/O(n³) = O(1/n), so it shrinks to 0.15% by n = 1,000. The model is the same formula as the dense count; sparsity just makes most c_j small, and that is the entire difference between four seconds and forty milliseconds.

And c_j is exactly the number of surviving neighbours variable j has at the moment it is eliminated. That is why the ordering is the algorithm: choosing an ordering is choosing the sequence of c_j, and the cost is the sum of their squares.

Cite the NP-hardness properly, because it is the sentence that ends the "why not just search for the best ordering" follow-up. The result is Yannakakis, Computing the Minimum Fill-In is NP-Complete, SIAM J. Algebraic & Discrete Methods 2(1):77–79, 1981 — proved by reduction from optimal linear arrangement. The vocabulary around it is worth a sentence each, because the literature uses it: an ordering with zero fill is a perfect elimination ordering, a graph that has one is chordal (Rose, Triangulated graphs and the elimination process, JMAA 1970), and elimination is precisely the act of chordal completion — adding edges until the graph is chordal. So "minimum fill-in" is "cheapest chordal completion", and Yannakakis says you cannot have it. Trees and chains are chordal already, which is the formal reason odometry is free; a cycle of length ≥ 4 is the smallest non-chordal graph, which is the formal reason loop closures are not.

The heuristics, dated, so you can name them in order: minimum degree is Tinney & Walker (1967), sharpened to the near-linear-time AMD by Amestoy, Davis & Duff, An Approximate Minimum Degree Ordering Algorithm, SIAM J. Matrix Anal. Appl. 17(4):886–905, 1996. Its unsymmetric sibling COLAMD is Davis, Gilbert, Larimore & Ng, ACM TOMS 30(3), 2004 — that is the one GTSAM defaults to, because it orders the columns of the Jacobian rather than the rows of Λ. Nested dissection is George (SIAM J. Numer. Anal. 1973), made practical at scale by METIS (Karypis & Kumar, SIAM J. Sci. Comput. 1998). And the solver that assembles all of it is CHOLMOD — Chen, Davis, Hager & Rajamanickam, Algorithm 887: CHOLMOD, Supernodal Sparse Cholesky Factorization and Update/Downdate, ACM TOMS 35(3), 2008, which is what is running underneath Ceres, g2o and GTSAM when you pick a direct solver.

Trace a six-cycle by hand, once

Do this on paper before you touch the sliders. It takes two minutes and it is the whole chapter.

Six poses in a loop: edges (0,1), (1,2), (2,3), (3,4), (4,5), (5,0). Eliminate them in index order, writing the surviving-neighbour set at each step.

So the column-count sequence for the six-cycle in natural order is c = (2, 2, 2, 2, 1, 0) and the bill is 44 flops. That is the chapter's central equation evaluated end to end on a graph you can draw on a napkin. Note that step 4 still costs 10 flops even though it creates no fill: the outer product is written into S whether or not the slot was previously zero. Fill is a storage event; flops are an arithmetic event; they are correlated but they are not the same number, and confusing them is the single most common error in this material (there is one in this chapter's own prose further down, and it is flagged when we get to it).

Watch what happened at step 2: variable 1's neighbour set included variable 5, and 5 was not one of its original neighbours — it arrived from the fill edge created at step 1. Fill begets fill. The elimination front drags a growing frontier around the loop until it reaches the far side.

Now delete the (5,0) edge so it is a chain instead of a cycle, and run the identical table:

Chain: c = (1, 1, 1, 1, 1, 0), 0 fill, 20 flops. Cycle: c = (2, 2, 2, 2, 1, 0), 3 fill, 44 flops. Identical variable count, one extra measurement, and the bill goes from 20 to 44 — the loop closure costs 24 extra flops on a six-node graph, a 2.2× increase for a 17% increase in edges. That leverage is the entire subject. One edge — one loop closure — is the difference between a free factorisation and a frontier that sweeps the graph, and on a 5,000-pose session the same effect is what turns 40 ms into 4 s.

Now write the symbolic phase yourself

The hand trace above is an algorithm, and it is short enough to write from memory at a whiteboard — which is a realistic ask, because "implement symbolic elimination" is a genuine 20-minute coding-round problem. Note what it does not take as an argument: any floating-point data. It sees only the pattern.

python
def symbolic(adj, order):
    """adj: {v: set(neighbours)}.  order: the elimination sequence.
       Returns (fill_edge_count, flops, column_counts).  No float data anywhere."""
    adj   = {v: set(nb) for v, nb in adj.items()}   # never mutate the caller's graph
    alive = set(adj)
    fill, cols = set(), []
    for v in order:
        alive.discard(v)
        nb = sorted(alive & adj[v])                 # surviving neighbours  ->  c_j
        for a in range(len(nb)):
            for b in range(a + 1, len(nb)):
                if nb[b] not in adj[nb[a]]:          # a pair no sensor ever measured
                    adj[nb[a]].add(nb[b])              # fill BEGETS fill: the graph
                    adj[nb[b]].add(nb[a])              # we eliminate from is the
                    fill.add((nb[a], nb[b]))           # one we keep editing
        cols.append(len(nb))
    return len(fill), sum(c * c + 3 * c for c in cols), cols

def graph(edges, n):
    g = {i: set() for i in range(n)}
    for i, j in edges:
        g[i].add(j); g[j].add(i)
    return g

cycle = [(0,1),(1,2),(2,3),(3,4),(4,5),(5,0)]
print(symbolic(graph(cycle,      6), range(6)))   # (3,  44, [2,2,2,2,1,0]) the cycle
print(symbolic(graph(cycle[:-1], 6), range(6)))   # (0,  20, [1,1,1,1,1,0]) the chain

star = [(0, i) for i in range(1, 7)]                 # hub 0, six leaves
print(symbolic(graph(star,7), [1,2,3,4,5,6,0]))  # (0,  24, [1,1,1,1,1,1,0]) leaves
print(symbolic(graph(star,7), range(7)))         # (15, 154, [6,5,4,3,2,1,0]) hub

Four lines of output, four numbers you derived by hand two paragraphs ago. That is the check — if your implementation disagrees with the napkin, one of them is wrong and you know which trace to walk.

What to say while typing it. "The complexity here is bad on purpose — the double loop is O(c_j²) per step, so a degree-1,000 variable does 500,000 set lookups. Production AMD never materialises those cliques; it stores each clique as a set element and represents a variable's neighbourhood as a union of elements, which is why it is called approximate minimum degree — the degree it computes is an upper bound obtained without expanding the union. Amestoy, Davis & Duff (SIAM J. Matrix Anal. Appl. 1996) is the paper; the trick is quotient-graph representation with supervariable and element absorption, and it takes the ordering from O(n·m) to near-linear in practice."

And the library form, which is what you would actually ship. The point of showing both is that the one-liner hides exactly the split you just implemented — analyze is the pattern-only pass, cholesky_inplace is the numeric pass, and they are separate calls precisely so you can call the first one once:

python
import numpy as np, scipy.sparse as sp
from sksparse.cholmod import analyze          # SuiteSparse CHOLMOD via scikit-sparse

Lam = sp.csc_matrix(...)                        # 30000 x 30000, 597528 nonzeros

# --- symbolic phase: runs ONCE, reads the pattern, never the values ---
F = analyze(Lam, ordering_method='amd')       # 'natural' / 'amd' / 'colamd' / 'nesdis'
print(F.P().shape)                               # (30000,)  int32 permutation

for it in range(n_gauss_newton_iters):
    Lam, eta = relinearise(x)                   # NEW VALUES, SAME PATTERN
    F.cholesky_inplace(Lam)                     # numeric phase reuses the ordering
    dx = F(eta)                                 # two triangular solves
    x  = retract(x, dx)
print(F.L().nnz)                                 # ~1.1e6  <- the number nobody logs

# pure-scipy fallback if SuiteSparse is not available:
from scipy.sparse.csgraph import reverse_cuthill_mckee
p = reverse_cuthill_mckee(Lam, symmetric_mode=True)   # bandwidth, not fill

The star, and the landmark you eliminated too early

Second hand trace, and it is the Schur complement in miniature. Six leaves all connected to one hub — a landmark seen by six keyframes.

Every entry in that last column, so nothing is asserted: 6²+18 = 36+18 = 54; 5²+15 = 25+15 = 40; 4²+12 = 16+12 = 28; 3²+9 = 9+9 = 18; 2²+6 = 4+6 = 10; 1²+3 = 1+3 = 4; 0. Total 154 against 24.

Same graph, same answer, 15 extra edges and 154/24 = 6.4× the arithmetic. Those two ratios are different numbers and it is worth understanding why, because reading one as the other is the mistake this section exists to prevent. Fill counts edges — each new nonzero is worth exactly 1 no matter which column it lands in. Flops weight each column quadratically — one column of count 6 costs 36, while six columns of count 1 cost 6 between them. A fill count is a linear functional on the elimination; a flop count is a quadratic one. They move together in direction and never in the same proportion, so quoting a fill ratio as a speed ratio (or the reverse) will be wrong every single time. Here it is wrong low: 6.4 < 15. On a graph where the fill concentrates into one enormous frontal matrix the flop ratio races ahead of the fill ratio instead — which is exactly what the 5,000-pose experiment below shows, where the nonzeros differ by 24× and the time by 100×.

And here is the subtlety worth saying out loud: in bundle adjustment you deliberately eliminate the hubs first, because the resulting clique lands on the cameras — which is a 600-dimensional block instead of a 60,000-dimensional one. The same operation is a disaster in one context and the entire optimisation in another. What changes is where the fill lands, not how much of it there is.

What the instrument's readouts are called in real tools

Why minimum degree usually wins, and when it does not

Constrained ordering is the practical wrinkle. In a sliding window you cannot use a pure fill-reducing ordering, because the variables you are about to marginalise must be eliminated first — that is what marginalising them means. So you use a constrained COLAMD: "these variables go last, order the rest however you like." Ceres calls it ParameterBlockOrdering with groups; GTSAM calls it Ordering::ColamdConstrainedLast. Knowing that this API exists is a genuine signal of having shipped a backend.

The twenty-four-fold experiment

One concrete comparison, worth doing in the instrument and worth memorising as a number. Twenty-four is the ratio of the nonzeros; keep reading, because the ratio of the time is not twenty-four, for exactly the reason the star taught.

Take a graph of 5,000 poses with 800 loop closures — a realistic warehouse session. Set the ordering to natural, and the elimination front carries a clique whose size grows with the span of the outstanding loop closures. Set it to AMD and the front stays local.

First, derive nnz(Λ) rather than being told it. This is a 30-second back-of-envelope computation:

• Variables: 5,000 poses × 6 DOF = 30,000, so Λ is 30,000 × 30,000.
• Edges: a chain of 5,000 poses has 5,000 − 1 = 4,999 odometry edges, plus 800 loop closures = 5,799 edges.
• Diagonal blocks: one 6×6 per pose, and 6×6 = 36 scalars → 5,000 × 36 = 180,000.
• Off-diagonal blocks: one 6×6 per edge, and it appears twice because Λ is symmetric → 2 × 5,799 × 36 = 11,598 × 36 = 417,528.
• Total: 180,000 + 417,528 = 597,528 nonzeros, or 0.066% of the 9×10⁸ entries of a dense 30,000² matrix.

Read the last three rows carefully, because they are the payoff of the whole chapter. The nonzeros differ by 2.6e7/1.1e6 = 24×. The time differs by 4 s / 40 ms = 100×. Both statements are true and they are not in conflict: flops are ∑c_j², so they depend on the shape of the column profile and not only on its sum. For a fixed nnz(L), a flat profile minimises ∑c_j² (Cauchy–Schwarz), so the banded natural ordering is as cheap as 2.6e7 nonzeros can possibly be — and it is still a hundred times slower. AMD's profile is the skewed one, which costs it a factor of RMS/mean ≈ 2.2 squared ≈ 4.8 above its own flat lower bound of 8 ms, and it wins anyway by more than an order of magnitude. You cannot read a speed ratio off a fill ratio. If you need the flop number, get it from the symbolic phase — cm->fl after cholmod_analyze — which computes ∑c_j² exactly instead of estimating it from a mean.

Five ways this goes wrong in production, and the number that names each one

Everything above is the happy path. Here is the debugging half, which is what actually gets asked. Each row gives the symptom you would observe from outside, the cause, the metric to print, and — the part that matters — the value that distinguishes it from the row above it, because in a real incident all five present as "the backend got slow".

Reproduce rows 1 and 5 in the instrument right now, because they are the two you will be asked about. For row 1: leave every slider alone and click between Natural and Minimum degree. nnz(Λ) stays at 78 in both — it is printed and it does not move — while the flop bar goes 560 → 210 and the fill ratio goes 2.21× → 1.28×. That is row 1 in miniature: the matrix is identical, the bill is not. For row 5: pin the ordering to Minimum degree, set loop closures to 8, and sweep the pose slider from 6 to 34. The flop count does not merely scale with the number of variables — watch it accelerate as the loop closures start to overlap in span, which is the log–log slope of row 5 rendered as a slider you can push.

Now budget it — the design question this chapter answers

"Architect the backend for a 33-minute warehouse session. What do you choose, and what does it cost?" The chapter has now handed you every number needed to answer that in a single pass, with no hand-waving. Keyframes at 2.5 Hz for 2,000 s gives the 5,000 poses we have been costing all along, so the latency budget is 1 / 2.5 Hz = 400 ms per update — the backend must be done before the next keyframe lands or the queue grows without bound.

And the bandwidth budget, because "it's memory-bound" is a claim you should be able to check. A full numeric factorisation writes L, which is 1.1×10⁶ nonzeros × 8 B = 8.8 MB. At 2.5 Hz that is 8.8 MB × 2.5 = 22 MB/s of write traffic. A single DDR4 channel delivers roughly 15 GB/s, so the factorisation is using 0.15% of available bandwidth. Memory bandwidth is not the constraint at this size. The constraint is the serial dependency chain in the elimination tree — a column cannot be factored until its children are — which is exactly why the GPU work in the frontier section optimises tree depth rather than fill, and exactly why throwing more cores at a deep tree does nothing. Being able to say "I checked, and it isn't bandwidth" with the arithmetic attached is worth more than any amount of confident intuition.

And the observability line item, since it is the cheapest thing in the design: the five numbers from the callout above are four int64s and one double, so ~40 B per solve. At 2.5 Hz for 33 minutes that is 5,000 records × 40 B = 200 kB per session — less than one camera frame. There is no budget argument against logging them.

What this instrument does not show, and why that matters

Three honest limitations, and volunteering them is exactly the kind of thing that reads as senior:

• Blocks, not scalars. Every node here is one scalar. A real 6-DOF pose is a 6×6 block, so every count multiplies by 36 and every flop count by roughly 216. The structure is identical, which is why block-oriented solvers exist — they run the symbolic analysis on the block graph, exactly as drawn here, and then do dense 6×6 kernels underneath.
• Symbolic only. The fill pattern shown is the worst case, assuming no numerical cancellation. Real cancellation happens and is never relied on, because relying on it makes the factorisation data-dependent.
• Static. A real backend does not re-run AMD from scratch every update. The Bayes tree of iSAM2 (Kaess, Johannsson, Roberts, Ila, Leonard & Dellaert, iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree, IJRR 31(2):216–235, 2012) maintains the elimination incrementally, re-eliminating only the part of the tree that a new factor touches. That is why a loop closure in iSAM2 costs 100–500 ms while a normal update costs 1–5 ms: the loop closure invalidates a large subtree, and that subtree is exactly the set of variables between the two ends of the loop — the same frontier you watched sweep the six-cycle, now paid for lazily instead of eagerly.

Where the ordering problem is going — and it is not 2012 any more

The classical citations above stop between 1981 and 2012, and if you leave the topic there you will sound like you learned it from a textbook and stopped. Three live threads, dated:

• Stop factoring at all. Power Bundle Adjustment (Weber, Demmel, Chan & Cremers, CVPR 2023) expands the inverse of the Schur complement as a power series instead of forming and factoring the reduced matrix, and reports order-of-magnitude speed-ups on BAL-scale problems at equal accuracy. If you never materialise the dense block, its fill is irrelevant — the ordering question is replaced by a convergence question. The same move in square-root form is Square Root Bundle Adjustment (Demmel, Sommer, Cremers & Usenko, CVPR 2021), which applies nullspace marginalisation to the Jacobian directly and gets single-precision solutions matching double-precision Schur — because working on the factor halves the condition number in the exponent.
• Parallelism changes what "good ordering" means. MegBA (Ren et al., ECCV 2022) distributes bundle adjustment across GPUs with a distributed preconditioned conjugate gradient rather than a direct solve, and NVIDIA's cuDSS (2023–) brings supernodal sparse direct factorisation onto the GPU. Both shift the objective: on a GPU you do not want the minimum-fill ordering, you want the ordering with the shallowest, widest elimination tree, because tree depth is a serial dependency chain and fill in a wide frontal matrix is free parallel work. That is precisely the regime where nested dissection beats AMD — not because it produces less fill, but because it produces a more balanced tree. Say this if someone asks "why would you ever not use AMD".
• Learning the ordering. Since roughly 2020 there has been a steady line of work applying GNNs and reinforcement learning to fill-reducing orderings and to elimination-tree construction, on the reasonable grounds that AMD is a greedy heuristic on a problem where the graphs from any one deployment are drawn from a narrow distribution. Nothing here has displaced AMD in a shipped robotics stack as of 2026, and the honest position — the one that reads as senior rather than credulous — is that the symbolic phase is already a small fraction of the total, so a better ordering has to beat AMD by a lot before it pays for the inference latency. The interesting version of the idea is not "learn a better permutation" but "learn when the pattern has drifted enough to be worth re-analysing", which is the actual cost centre in an incremental backend.

The instrument is the other test. If you can predict the flop bar before you press the button — and now you can, because you have the formula, the derivation and four hand-checked examples — you understand elimination ordering. The Field Guide is next.

Chapter 7: The Field Guide

Tuesday morning, and the nightly regression dashboard is red. A teammate puts a laptop on your desk, turns it round, and says: “Solve time went from twenty milliseconds to four seconds overnight. Same dataset. Same residuals. Same answer to nine decimal places. What do you want me to plot?”

Nobody in that conversation wants a lecture on sparse linear algebra. They want one metric, named in about eight seconds, with the number that would confirm it and the number that would rule out the obvious decoy. You either have that pair loaded or you do not.

Everything in this chapter exists to make those eight seconds automatic. Read it on the train. Then close it and try to reproduce the numbers drill from memory — that is the actual test, because the gap between knowing that ordering changes nnz(L) and saying “forty times on nnz(L), and nnz(Λ) barely moved, so it is the ordering and not the graph” is the gap between understanding and fluency.

One structural note before the tables. Six of the nine sections below are compressions — they assume you have already done the derivations in Chapters 1 to 6 and they exist to make retrieval fast under pressure. Two of them (the worked numbers in section 6 and the drills in section 3) are expansions: they are written out longer than you would ever say them out loud, because the only way to produce a one-line answer under pressure is to have produced the ten-line version at least once at a desk.

1 · The cheat sheet

Where those symbols come from

A cheat sheet that you can only recite is worth nothing, because the second question is always “what is U?” Every equation in that table is reconstructible in one or two sentences from the residual alone. Here is each one rebuilt from scratch, in the order a whiteboard would want them.

• Λ = ∑_f J_f^TΣ_f⁻¹J_f. Start from one factor: a measurement z with noise covariance Σ_f, a predicted measurement h(x), a residual r = z − h(x), and J_f = ∂r/∂x. Its negative log-likelihood is ½ r^TΣ_f⁻¹r; take two derivatives in x and drop the second-derivative term (that is the Gauss–Newton approximation) and you get J_f^TΣ_f⁻¹J_f. Factors are independent, so the log-likelihoods add and so do the Hessians. J_f is zero in every column except the two or three variables that factor touches, so the outer product is zero everywhere except the blocks where those variables meet. That is the whole sparsity argument, and it takes fifteen seconds to say.
• η is the other half of the same object. The linear system is Λδ = η with η = −∑_f J_f^TΣ_f⁻¹r_f — minus the gradient of the cost. The minus sign is why you add δ afterwards (x ← x ⊕ δ) rather than subtract it. In filtering language η is the information vector, the partner of the information matrix Λ, and the pair (Λ, η) is exactly the Gaussian N(μ, Σ) rewritten with Σ⁻¹ = Λ and Σ⁻¹μ = η. If you can say that sentence, you have explained the entire information form.
• Λ' = Λ_bb − Λ_baΛ_aa⁻¹Λ_ab: name a and b out loud. a indexes the variables you are removing (the keyframe falling out of the window, the landmark you no longer track); b indexes the variables you are keeping. Order the state as [a; b] so Λ = [[Λ_aa, Λ_ab], [Λ_ba, Λ_bb]], then complete the square in δ_a and integrate it out; what survives on b is Λ'. The correction term −Λ_baΛ_aa⁻¹Λ_ab is an outer product over every pair of surviving neighbours of a — which is why it is dense on that neighbourhood, and why marginalisation and fill-in are the same event described twice.
• c_j and ∑c_j². Eliminate variables in some order; when you reach variable j, let c_j be the number of its neighbours that have not yet been eliminated. Its column of L then has c_j nonzeros below the diagonal, and rank-1 updating the remaining matrix by that column costs about c_j² multiply-adds. Sum over j and you have the flop count of the whole factorisation. The ordering does not change the answer; it changes every c_j, and the cost is quadratic in each of them. That is the entire case for AMD in one line.
• S = U − WV⁻¹W^T: partition the BA Hessian first. Put the C camera poses first and the P points second. Then U is the 6C × 6C camera block, V is the 3P × 3P point block — and V is block-diagonal, because no measurement ever touches two points at once, so no point pair shares a factor. W is the 6C × 3P coupling block, with a nonzero 6 × 3 sub-block exactly where camera i observed point j. Apply the marginalisation formula with a = points and b = cameras and you get S = U − WV⁻¹W^T. The subtracted term is precisely the fill-in the points leave behind on the cameras, and its sparsity pattern is the co-visibility graph: cameras i and k become coupled if and only if some point was seen by both. The eliminate-points-first ordering is cheap because V is block-diagonal, so V⁻¹ is P independent 3 × 3 inverses and never a 60,000 × 60,000 one.
• (Λ + λ diagΛ)δ = η. Same Λ and same η as above, plus one scalar λ ≥ 0 on the diagonal. At λ = 0 this is Gauss–Newton; as λ → ∞ the matrix approaches a scaled identity and δ approaches a short step along η, i.e. gradient descent. Scaling by diagΛ rather than by I is what makes it invariant to the units you chose for your states — without it, measuring a translation in millimetres instead of metres changes the step. λ is not tuned; it is driven by the gain ratio ρ = (actual cost decrease)/(decrease the quadratic model predicted): ρ > 0 accept and shrink λ, ρ ≤ 0 reject and grow it.
• Δv_ij = ∑ΔR_ik(a_k − b_a)Δt. The subscript that matters is ik, not wk: the rotation is relative to the body frame at time i, the start of the segment. Written that way the sum contains only IMU samples and biases — no world-frame attitude, no gravity, no initial velocity — so it does not change when the optimiser moves the states at either end. That is why it survives relinearisation, and it is the whole content of the word preintegration: a change of variables, not an approximation.
• dim null(J) = 4 for monocular VIO: count it, do not memorise it. Start from the seven gauge freedoms of monocular structure-from-motion — 3 translations, 3 rotations, 1 scale. Now add an IMU. The accelerometer measures a known-magnitude gravity vector, which pins roll and pitch (2 gone). It also measures specific force in metres per second squared, which pins scale (1 more gone). What is left unobservable is 3 translations plus yaw — rotation about gravity, which no sensor in the suite can see. 7 − 3 = 4. For comparison: 2-D SLAM has 3 (x, y, heading), a 3-D pose graph or stereo BA has 6, and monocular BA with no IMU has 7. If the numerical nullspace of your stacked Jacobian reads one less than the number you just derived, an inconsistent linearisation has fabricated information along that direction — the FEJ bug, and section 4's second row.

2 · System design patterns

Prompt A — "Design the backend for a warehouse AMR fleet: 2-D LiDAR, wheel odometry, IMU, a 200 m × 80 m building, eight-hour shifts."

Prompt B — "We are adding a second robot. Should they share a map?" The framework: (1) is the constraint between the robots observable — do they ever see each other or the same place? (2) What is the bandwidth of the shared object — a full graph is megabytes, a set of keyframe descriptors is kilobytes. (3) Who owns the gauge — two independently-gauge-free graphs need one anchor between them, or the merged problem is rank-deficient by 3 (2-D) or 6 (3-D). (4) What happens on a false inter-robot loop closure — and the honest answer is that it is worse than a false intra-robot one, because it corrupts two maps at once. Name distributed pose-graph optimisation (Choudhary et al., IJRR 2017) and the robust variant (Lajoie et al., DOOR-SLAM, RA-L 2020) as prior art.

Prompt C — "Your backend has a 250 ms budget and occasionally takes 4 seconds. Ship it or fix it?" The framework: separate throughput from latency, and identify who is downstream. If a controller reads the output, a 4 s stall is a safety event and you need a bounded-time path (IMU propagation) that never touches the optimiser. If only the map consumer reads it, you can run the slow path asynchronously and merge. Then name the specific fix: bound the work (fixed lag), or bound the variance of the work (iSAM2's incremental re-elimination), or move the unbounded part off the critical path. "Make it faster" is not an answer; "make it bounded" is.

Prompt D — "How do you know the backend is healthy in the field, with no ground truth?" Four signals, none of which need truth: chi-square of each factor family against its expected distribution; the ratio nnz(L)/nnz(Λ); the fraction of LM steps rejected; and the trace of the marginal covariance on a fixed reference keyframe, which must be non-decreasing between loop closures. A covariance that shrinks with no loop closure is a rank bug (Chapters 1 and 3).

Prompt E — "Design the backend for a handheld AR headset: rolling-shutter mono camera at 30 Hz, IMU at 800 Hz, 512 MB total memory budget." This is Prompt A's mirror image and it is worth working to the same depth, because every constraint pushes the opposite way: the warehouse robot had space and time and one bad sensor; the headset has neither space nor time and a sensor that lies about when.

The one sentence that ties Prompts A and E together, and it is worth saying explicitly at the end of either answer: in both systems the fast loop is bounded by construction and the slow loop is unbounded but off the critical path, and the only object crossing between them is a small, dated correction that is composed, never assigned. Change the sensors, the platform and three orders of magnitude of memory budget, and that invariant does not move. That is the architecture; everything else is parameters.

3 · Coding drills

Drill 1 — "Assemble the normal equations for a pose graph." The ten lines that matter, and they are the ones the Chapter 2 lab makes you write:

python
def add_between(Lam, eta, i, j, meas, w, x):
    r = (x[j] - x[i]) - meas          # J = [-1, +1]
    Lam[i,i] += w; Lam[j,j] += w
    Lam[i,j] -= w; Lam[j,i] -= w      # THE EDGE
    eta[i]   += w*r; eta[j] -= w*r    # eta = -J^T w r

Say while writing: "The off-diagonal is the graph edge — if I only wrote the diagonal, every pose would be independent and no information could propagate. And I am accumulating η = −J^TWr, not J^TWr, because I solve Λδ = η and then add δ."

Then immediately upgrade it, unprompted. The version above is a 1-D chain: x[j] - x[i] is a scalar subtraction, w is a scalar, and Lam[i,i] += w writes one entry. Nobody runs that. Write it, say “that is the scalar warm-up, here is the real one,” and write the SE(2) block form — because the real test is whether you know that w is a 3×3 matrix.

python
# x   : (N, 3) float64   -- [tx, ty, theta] per pose, N poses
# Lam : (3N, 3N) float64 -- information matrix, symmetric
# eta : (3N,)   float64  -- information vector
# Om  : (3, 3)  float64  -- Omega = Sigma^-1 for THIS measurement
def add_between_se2(Lam, eta, i, j, Z_ij, Om, X):
    # r in R^3 -- a manifold residual, NOT a subtraction
    r  = Log(inv(Z_ij) @ inv(X[i]) @ X[j])        # (3,)
    Ji = -Ad(inv(X[j]) @ X[i])                     # (3,3)  d r / d xi_i
    Jj =  np.eye(3)                              # (3,3)  d r / d xi_j
    I, J = slice(3*i, 3*i+3), slice(3*j, 3*j+3)
    Lam[I, I] += Ji.T @ Om @ Ji       # (3,3) into the diagonal block
    Lam[J, J] += Jj.T @ Om @ Jj
    Lam[I, J] += Ji.T @ Om @ Jj       # (3,3) -- THE EDGE, as a block
    Lam[J, I] += Jj.T @ Om @ Ji       # = the transpose of the line above
    eta[I]    -= Ji.T @ Om @ r        # (3,)  eta = -J^T Om r
    eta[J]    -= Jj.T @ Om @ r

Say the shapes out loud as you write them. "x is (N,3) but Λ is (3N,3N), because each pose contributes three columns. The residual is r = Log(Z_ij⁻¹X_i⁻¹X_j) in R³ — a Lie-algebra vector, not a difference of poses, because subtracting two headings across ±π is how you get a 6.28 rad residual and a solver that explodes. J_i is 3×3 and equals minus the adjoint; J_j is the 3×3 identity, which is only true in this right-perturbation convention — and I would state my convention, because the sign of J_i flips in the left one. And the four scalar writes became four 3×3 block writes, with Ω a full 3×3 information matrix, not a scalar weight: the x–y block is large, the θ–θ entry is usually much larger, and the cross terms are what encode a scan matcher that is confident along a wall and uncertain along it."

The scalar version and the block version have identical structure — four writes, two on the diagonal, two off — and that is the point worth landing: the graph does not care about block size. Every count multiplies by 9 for SE(2) or 36 for SE(3), every flop count by roughly 27 or 216, and nothing else changes. That is exactly why block-oriented solvers run the symbolic analysis on the scalar block graph and then call dense 3×3 or 6×6 kernels underneath.

Drill 2 — "Marginalise a variable."

python
X = np.linalg.solve(Laa, Lab)          # NOT inv(Laa) @ Lab
Lam_marg = Lbb - Lba @ X
eta_marg = eta_b - Lba @ np.linalg.solve(Laa, eta_a)

Say while writing: "Solve, never invert — inverting costs three times as much and squares the condition number. And I would store the linearisation point alongside this prior, because it is now frozen: relinearising it later is the FEJ bug."

Drill 3 — "Compute the fill-in of an elimination ordering." A pure graph problem, and a favourite because it has nothing to do with robotics until the last sentence:

python
def fill_in(n, edges, order):
    adj = [set() for _ in range(n)]
    for a, b in edges: adj[a].add(b); adj[b].add(a)
    alive, fill = set(range(n)), 0
    for v in order:
        alive.discard(v)
        nb = sorted(adj[v] & alive)
        for a in range(len(nb)):
            for b in range(a+1, len(nb)):
                if nb[b] not in adj[nb[a]]:
                    adj[nb[a]].add(nb[b]); adj[nb[b]].add(nb[a]); fill += 1
        adj[v].clear()
    return fill
# chain, natural order      -> 0    (perfect elimination ordering)
# chain, middle first       -> 1    (couples the two halves)
# 6-cycle, natural order    -> 3    (a loop closure cannot be free)
# 6-star, hub eliminated 1st-> 15   (= 6*5/2 — the landmark you marginalised too early)

Say while writing: "This is the symbolic phase of a sparse Cholesky, and it runs before any arithmetic. The last line is a landmark seen by six cameras: marginalise it first and you get a six-clique. That is not a bug — it is the Schur complement, and in bundle adjustment it is exactly what you want, because the clique lands in the small camera block."

Drill 4 — "Preintegrate an IMU segment." The order of the four lines is the whole drill:

python
for a, w, dt in imu:
    f = a - ba
    dp += dv*dt + 0.5*dR@f*dt*dt      # uses dv and dR from BEFORE this sample
    dv += dR@f*dt                     # then update dv...
    dR  = dR @ Exp((w - bg)*dt)       # ...and dR last

Say while writing: "Order matters: position integrates the velocity that was already accumulated, so it must be updated first. And I would accumulate the bias Jacobians in the same loop, because re-integrating on every bias update defeats the purpose."

Then put a number on the bug, because this is the one they will push on. Suppose you update dv before dp. The position line now uses dv_new = dv_old + aΔt, so the increment becomes (dv_old + aΔt)Δt + ½aΔt² instead of dv_oldΔt + ½aΔt². Subtract: the per-sample error is aΔt² — not ½aΔt², because the half-term is still there. Exactly twice what a careless answer claims, and being able to say which factor and why is the difference between having read the code and having written it.

Now make it land. At 200 Hz, Δt = 5 ms, so a modest 1 m/s² of specific force leaks 1 × (0.005)² = 2.5×10⁻⁵ m = 25 microns per sample. Over the 50 samples between keyframes that is 50 × 25 µm = 1.25 mm of position error per segment — utterly invisible in one segment and smaller than your feature noise reprojects to. But it is signed with the acceleration, not zero-mean, so it does not average out: over a thousand segments it is a systematic 1.25 m of accumulated inflation on the inertial leg, which the optimiser resolves by quietly shrinking the visual scale to match. That is a scale bias that no chi-square will flag, and it is why this drill exists.

Drill 5 — "Build the Schur complement for a bundle adjustment with 3 cameras and 4 points." Small enough to write out in full at a whiteboard, which is exactly why it gets asked. Start by drawing the partition before writing any code, because the partition is the answer:

• U is 6C × 6C = 18×18, three 6×6 diagonal blocks. In pure BA it is block-diagonal: no factor touches two cameras, so there is nothing off-diagonal to write. (Add an IMU or a relative-pose prior between consecutive cameras and it gains a tridiagonal band — worth mentioning, because it is the difference between BA and VIO.)
• V is 3P × 3P = 12×12, four 3×3 diagonal blocks, and it is block-diagonal always: no measurement ever touches two points. This is the structural fact the whole method rests on.
• W is 6C × 3P = 18×12, with a nonzero 6×3 block at (i, j) exactly when camera i observed point j. W is the visibility matrix, written in blocks.

Give yourself a concrete visibility so the fill is visible: p₁ seen by {c₁, c₂}, p₂ by {c₁, c₂, c₃}, p₃ by {c₂, c₃}, p₄ by {c₁, c₃}. Then eliminate all four points:

python
# U : (6C, 6C)          V : (P, 3, 3)      W : (C, P, 6, 3)
# bc: (6C,)             bp: (P, 3)         obs[j] -> list of cameras
S = U.copy(); b = bc.copy()
for j in range(P):                          # points, in ANY order
    Vinv = np.linalg.inv(V[j])              # (3,3) -- the only inv allowed
    cams = obs[j]
    for i in cams:
        Y = W[i][j] @ Vinv                    # (6,3)
        b[6*i:6*i+6] -= Y @ bp[j]              # (6,)
        for k in cams:                       # <-- THE CLIQUE
            S[6*i:6*i+6, 6*k:6*k+6] -= Y @ W[k][j].T   # (6,6)
dxc = np.linalg.solve(S, b)                  # (18,) -- reduced camera solve
for j in range(P):                          # back-substitute the points
    acc = bp[j].copy()
    for i in obs[j]: acc -= W[i][j].T @ dxc[6*i:6*i+6]
    dxp[j] = np.linalg.solve(V[j], acc)      # (3,)

Say while writing: "The inner double loop over cams is the clique. For point j seen by n_j cameras I write n_j² blocks of 6×6, and the off-diagonal ones are couplings no measurement created — that is the fill-in, and its pattern is the co-visibility graph. With my visibility, c₁–c₂ is coupled through p₁ and p₂, c₂–c₃ through p₂ and p₃, c₁–c₃ through p₂ and p₄ — so all three pairs fill, U was block-diagonal and S is dense 18×18. That is (3 choose 2) = 3 new blocks, the d(d−1)/2 rule from the fill-in row of my cheat sheet. And np.linalg.inv appears exactly once, on a 3×3; everywhere else it is solve."

Then volunteer the honest cost, because at these sizes Schur barely wins. Naive: (18 + 12)³/3 = 30³/3 = 9,000 flops. Schur: four 3×3 inverses at ~27 flops = 108; the pair loop runs ∑n_j² = 2² + 3² + 2² + 2² = 21 block products, each a (6×3)(3×3) then a (6×3)(3×6) at 54 + 108 = 162 flops, so 21 × 162 = 3,402; plus the reduced solve 18³/3 = 1,944. Total ≈ 5,450 flops — a speed-up of 1.65×. Say that number out loud. The toy sizes of 3 and 4 exist so you can write the blocks, not so you can be impressed; the speed-up only becomes 10⁵–10⁶ when P ≫ C, and knowing where the method stops paying is worth more than knowing that it pays.

4 · Debugging scenarios

5 · Classical vs modern, and when to use which

This table is the frontier view, and it is used differently from the others. The two middle columns are trivia — anyone can recite that iSAM2 superseded EKF-SLAM. The fourth column is the whole point. Read the table left to right and you sound current; read it right to left and you sound like someone who has shipped, because the question underneath every “should we use the new thing?” discussion is really “do you reach for the new thing reflexively, or because the constraint demands it?”

How to use it. When a modern method comes up, do three things in one breath: (1) confirm you know it and what it replaced, (2) state the condition under which the classical choice still wins, (3) name the number that decides between them. “Yes, we would use a fixed-lag smoother with FEJ — but on a 4 W platform with no loop closures I would benchmark it against a consistent MSCKF first, because OpenVINS gets comparable ATE there at roughly half the compute, and the crossover is whether you ever revisit.” That answer is unbeatable in both directions: you cannot be caught as out of date, and you cannot be caught as a cargo-culter.

The one row that is a genuine trap is the first. “Is the EKF dead?” invites a confident yes, and the confident yes is wrong — it is dead for mapping, where the dense covariance is fatal past a few hundred landmarks, and alive for bounded-state odometry on power-limited hardware. Split the question before you answer it. The people who get this wrong are not the ones who do not know; they are the ones who heard “the field moved on” and stopped there.