Drift, Loop Closure
& Relocalization

Chapter 1: Sources of Drift, and Observability

Before you can fix drift you have to know which part of it is even fixable. That is not a philosophical question — it is a linear-algebra question with a definite answer, and the answer decides where every constraint in your system has to come from.

Where the error comes from

Drift is the accumulation of small errors in relative measurements. Every odometry source produces a relative motion estimate — "between this frame and the last I moved forward 0.5 m and turned 3°" — and the pose is the running sum of those. Three sources feed it:

Notice a pattern: every one of these is a relative quantity. None of them measures where the robot is in the world — they measure how it changed. And that is exactly why the sum is unconstrained.

Observability: the gauge you can never see

Observability asks: given all your measurements, is the state uniquely determined, or are there directions you could move the entire estimate without changing a single measurement? Those invisible directions are the unobservable subspace — the null space of the information matrix Λ = A^TWA — and error accumulates freely along exactly those directions, because nothing pushes back.

That definition is worth restating carefully, because it is the whole game. The measurements are the residual function r(x); the Jacobian A = ∂r/∂x is how the residuals respond to a nudge in the state. If there is a state direction d with Ad = 0, then to first order you can slide the estimate by any amount along d and every residual stays put. No cost function built from those residuals can prefer one point along d over another. That direction is unobservable, and Λd = A^TW(Ad) = A^TW·0 = 0, so d is exactly a null vector of Λ. Observability and the null space of Λ are the same statement told two ways.

Warm-up: two poses, one factor

Before the four-pose chain, do the smallest non-trivial case entirely by hand — two poses x₀, x₁ on a line, one odometry factor between them. The residual is r = x₁ − x₀ − u. Differentiate: ∂r/∂x₀ = −1, ∂r/∂x₁ = +1. So A is a single row:

With unit weight, Λ = A^TA is the outer product of that row with itself — a 2×2 matrix. Compute all four entries directly, one at a time:

Multiply by the all-ones vector 1 = (1, 1), one row at a time:

So Λ·1 = (0, 0). The gauge is already here in the two-pose case: you can slide both poses by the same amount and the one measurement, which only sees x₁ − x₀, never notices. Make it concrete — suppose the true poses are x₀ = 0, x₁ = 2 and the odometry says u = 2. The residual is r = 2 − 0 − 2 = 0. Now shift both by +5, to x₀ = 5, x₁ = 7: the residual is r = 7 − 5 − 2 = 0, unchanged. The estimate moved a full 5 units and the cost did not flinch. That is unobservability you can feel in three arithmetic steps.

The two eigenvalues fall out of the trace-and-determinant pair of a 2×2:

So λ ∈ {0, 2}. One zero → a one-dimensional null space → exactly one unobservable direction, and it is the all-ones gauge. The nonzero eigenvalue, 2, is the observable direction — the difference x₁ − x₀, which the single factor measures directly and stiffly. Every larger pose graph is this same story with more poses: a null direction that is the gauge, and a spread of positive eigenvalues that are the parts the measurements actually pin down.

The four-pose chain, every entry shown

Now scale up. Four poses on a line, three odometry factors, no prior. The residual of the factor between poses k and k+1 is r = x_k+1 − x_k − u, so its Jacobian row is [… −1 … +1 …] — a −1 in column k and a +1 in column k+1. Stack the three rows (unit weight):

Form Λ = A^TA by hand. Entry (i, j) of A^TA is column i of A dotted with column j of A. Write the four columns down — each is a length-3 vector reading down the stacked rows:

Now every one of the sixteen entries, computed as a dot product (the matrix is symmetric, so the ten on-and-above-diagonal entries fix the rest):

Reflecting the off-diagonals (Λ₁₀=Λ₀₁, and so on) assembles the full matrix:

Read what each entry means: the diagonal Λ_ii counts how many factors touch pose i (the interior poses 1 and 2 sit in two factors each → 2; the endpoints 0 and 3 sit in one → 1), and the off-diagonal Λ_ij = −1 exactly when poses i and j are directly linked by a factor. That is the graph Laplacian of a 4-node path, degree on the diagonal, −(adjacency) off it.

Why the null space is the all-ones vector — not a coincidence

You can see the zero eigenvalue coming without computing anything, and the argument generalises to any pose graph, not just this chain. A graph Laplacian has a structural property: every row sums to zero. Look at any row — say row 1, (−1, 2, −1, 0): the diagonal is the node's degree, and the off-diagonals are exactly one −1 per neighbour, so the negatives cancel the degree: −1 + 2 + (−1) + 0 = 0. This holds for every row because degree = number of neighbours by construction.

“Every row sums to zero” is precisely the statement Λ·1 = 0, because the i-th entry of Λ·1 is the sum of row i. So the all-ones vector is guaranteed to be a null vector for structural reasons — it is not a numerical accident of this particular chain. Verify it term by term for our matrix:

Λ annihilates the all-ones vector. That means 1 is in the null space: you can add the same constant to every pose and every odometry residual is unchanged. Physically, the whole trajectory slides bodily along the axis and no relative measurement can tell. The eigenvalues confirm it — {0, 0.586, 2, 3.414}, where the interior values are 2 − √2 ≈ 0.586 and 2 + √2 ≈ 3.414 — exactly one is zero, and its eigenvector is (proportional to) the all-ones direction. (These are not arbitrary numbers: a path-graph Laplacian on n nodes has eigenvalues 2 − 2 cos(kπ/n) for k = 0…n−1, and k = 0 gives the zero. Here n = 4: k = 1 gives 2 − 2 cos45° = 2 − √2, k = 2 gives 2 − 2 cos90° = 2, k = 3 gives 2 − 2 cos135° = 2 + √2.)

The three nonzero eigenvectors are worth naming, because they are the trajectory shapes the graph actually constrains — the observable subspace, told one mode at a time:

The eigenvalue is the stiffness of that shape: the ramp (0.586) is the direction a solver pins down most weakly, so it is where residual noise leaks into the trajectory shape first — long-wavelength bending is exactly the drift you see in a real map. The zig-zag (3.414) is nailed down hard because adjacent poses disagree the most about it and every factor fights it. The zero mode sits below all of them: no stiffness at all, the free gauge. So the spread of the nonzero eigenvalues (a factor of ~5.8 between softest and stiffest) is what a solver feels as ill-conditioning even inside the observable subspace, and the zero is the part no amount of relative data will ever condition.

The punchline: a loop closure does NOT fix the gauge

Here is the mistake almost everyone makes. "A loop closure will fix everything." No. A loop closure is also a relative measurement — it says pose i and pose j are a certain relative pose apart. So it is one more row in A of exactly the same shape as an odometry row. Rather than assert the result, let us build it and watch the gauge survive.

The loop closes 3→0: its residual is r = x₀ − x₃ − u_loop, so ∂r/∂x₀ = +1, ∂r/∂x₃ = −1 — the Jacobian row is [1, 0, 0, −1]. Stack it under the three odometry rows to get a 4×4 Jacobian:

Now form Λ′ = A′^TA′. The key shortcut: A′^TA′ = (sum over rows of the outer product of each row with itself). The first three rows are the odometry rows we already summed — they give back the original Laplacian Λ. So we only have to add the outer product of the new row v = (1, 0, 0, −1) with itself. That outer product v v^T is nonzero only where v is nonzero — in components 0 and 3:

So the loop adds +1 to the two endpoint diagonals (0,0) and (3,3) — because poses 0 and 3 now each touch one extra factor — and writes −1 into (0,3) and (3,0) — because 0 and 3 are now directly linked. Add that, entry by entry, to the chain Laplacian Λ:

every other entry unchanged from Λ. The result:

This is the Laplacian of the 4-node ring — every pose now has degree 2, and the two −1 off-diagonals per row mark its two ring neighbours. And a ring Laplacian is still a Laplacian, so every row still sums to zero — the structural argument from above did not care whether the graph was a chain or a loop. Check Λ′·1 term by term:

The all-ones vector is annihilated again: the loop closure did not touch the gauge. Only now, having watched it survive, do we read the eigenvalues — {0, 2, 2, 4}. Still exactly one zero. The rank went from 3 to 3 (the new row was linearly dependent on the null-space complement, adding no new observable direction that pins the gauge); the smallest nonzero eigenvalue jumped from 0.586 to 2, which is the loop stiffening the observable subspace — that is the drift getting fixed — while the zero sits untouched. Fixing drift and fixing the gauge are visibly different operations on the spectrum: one lifts the small nonzero eigenvalues, the other would have to lift the zero, and no relative measurement ever can.

So the error splits cleanly into two kinds:

CODE — measure observability directly

You never have to guess whether a direction is observable — you can read it off the information matrix. The unobservable subspace is the null space of Λ, and its dimension is n − rank(Λ). Build the pose-graph Λ from the Jacobians and inspect its smallest eigenvalue:

python
import numpy as np

# 4-pose chain, 3 odometry factors, r = x[k+1] - x[k] - u  ->  Jacobian row [-1, +1]
A = np.array([[-1, 1, 0, 0],
              [0, -1, 1, 0],
              [0, 0, -1, 1]], float)
Lam = A.T @ A                          # the information matrix (graph Laplacian)
w, v = np.linalg.eigh(Lam)
print("eigenvalues", np.round(w, 4))       # [0. 0.586 2. 3.414] — ONE zero
print("null vector", np.round(v[:, 0], 3)) # [-.5 -.5 -.5 -.5] — the global gauge

# add a loop closure 3 -> 0 (Jacobian row [1, 0, 0, -1]) and recheck
A2 = np.vstack([A, [1, 0, 0, -1]])
Lam2 = A2.T @ A2                       # the ring Laplacian: [[2,-1,0,-1],[-1,2,-1,0],[0,-1,2,-1],[-1,0,-1,2]]
print("Lam2 @ ones", Lam2 @ np.ones(4))    # [0. 0. 0. 0.] — gauge annihilated AGAIN
w2, v2 = np.linalg.eigh(Lam2)
print("with loop", np.round(w2, 4))         # [0. 2. 2. 4.] — STILL one zero: gauge survives
print("null vector", np.round(v2[:, 0], 3)) # [-.5 -.5 -.5 -.5] — same all-ones gauge

The library form is a rank test: np.linalg.matrix_rank(Lam) returns 3, so the 4-dimensional state has a 1-dimensional unobservable subspace — and it stays 1-dimensional after the loop closure. This one-liner is how you catch an under-constrained graph before you hand it to a solver that will silently return garbage along the null direction.

DESIGN — where does each constraint enter, at what rate?

In a real stack these live in different threads at different rates. A concrete visual-inertial system:

The loop closure runs slow and off the critical path on purpose: it is expensive (a database query plus a geometric check), it is rare, and a 200 ms latency on a correction that happens once a minute is invisible. The design consequence: place recognition lives in its own thread, and the backend only gets a loop-closure factor when that thread is confident.

DEBUG — the symptom of an unobservable you forgot

FRONTIER

The observability of VIO was pinned down rigorously by Hesch, Kottas, Bowman & Roumeliotis (2014), "Consistency Analysis and Improvement of Vision-aided Inertial Navigation," which showed that a naive EKF-VIO gains spurious information along the unobservable yaw direction because of inconsistent linearisation — making the filter overconfident about something it cannot see. Their fix, the First-Estimates Jacobian (and later OC-EKF, observability-constrained), forces the estimator's null space to match the true one. This is the modern reason the field is careful about where it linearises — the same theme the SLAM Backend lesson develops for smoothers.

Chapter 2: Place Recognition

To close a loop the robot first has to answer a deceptively hard question: have I been here before? It is standing in aisle 7 looking at a wall of identical shelves, holding an image, and it has ten thousand past images in memory. Comparing the new image pixel-by-pixel against all ten thousand is both too slow and too brittle — the lighting changed, the robot is at a different angle, a forklift is parked in the way.

The trick that makes this tractable is to throw away almost everything and keep a summary: a fixed-length vector that captures what the place is made of, robust to viewpoint and lighting, cheap to compare. Two summaries that are close mean two images that are probably the same place. This is place recognition, and for fifteen years the dominant summary was the bag of visual words.

Bag of visual words, from zero

Detect features in the image (ORB, SIFT). Each feature is a high-dimensional descriptor — a little fingerprint of a corner or blob. Now the key idea: quantise every descriptor to its nearest entry in a pre-built vocabulary of, say, 10⁶ "visual words" (learned by k-means over millions of descriptors from a training corpus). The image collapses to a histogram: how many features landed in each word. Order is thrown away — hence "bag."

Not all words are equal — TF-IDF

A raw histogram overweights common words. In a warehouse the word "floor texture" fires in every image, so it tells you nothing about which place you are in. The word "that one cracked pillar with the yellow paint" fires almost nowhere — so when it matches, it is enormously informative.

This is exactly the problem text search solved decades ago, and the fix is the same: TF-IDF weighting. Weight each word by two factors:

Where tf_i (term frequency) is the fraction of this image's features in word i; N is the number of database images; df_i (document frequency) is how many database images contain word i at all. A word in every image has df = N, so idf = log(1) = 0 — it contributes nothing. A word in one image out of a thousand has idf = log(1000) ≈ 6.9 — a huge multiplier.

Hand-worked example

A 4-word vocabulary — {floor, door, shelf, pillar} — and a database of 3 past places, each a count histogram:

Step 1 — document frequency. How many of the 3 places contain each word? "floor" appears in all 3 (df=3). "door", "shelf", "pillar" each appear in 2 (df=2).

"floor" is the "sky is blue" word: it is everywhere, so its weight is exactly zero. Good — the floor should not help you localise.

Step 2 — the query vector. Query counts (4, 2, 4, 1) sum to 11, so tf = (0.364, 0.182, 0.364, 0.091). Multiply by idf:

Step 3 — cosine similarity with each place. Do the same for A, B, C and take the dot products of the unit vectors:

Place C wins clearly. That is the loop-closure candidate: the database image whose weighted histogram is most similar to the query. Chapter 3 is about whether to trust it.

The inverted index — why this is fast

Cosine similarity against a million-image database sounds like a million dot products. It is not, because the histograms are 99.99% zeros. A DBoW-style system keeps an inverted index: for each visual word, a list of the database images that contain it. To score the query you only visit the words the query actually has, and only touch database images that share at least one word. A query with 300 features touches a few thousand images, not a million — sub-millisecond on a laptop.

Learned descriptors — NetVLAD and beyond

Bag-of-words breaks under severe appearance change — the same street in summer vs winter, day vs night, produces different features that quantise to different words. The modern answer is a learned global descriptor: a neural network trained to map an image to a single vector such that same-place images are close and different-place images are far, directly, without a hand-built vocabulary.

NetVLAD (Arandjelović et al., 2016) is the landmark: it makes the VLAD aggregation (residuals to cluster centres) differentiable, so the whole pipeline — feature extraction, cluster assignment, aggregation — trains end-to-end on a weakly-supervised "same place / different place" loss mined from Google Street View. The output is a compact vector (often 4096-D, PCA-reduced to 256-D) compared by dot product.

DEBUG — perceptual aliasing

Chapter 3: Rejecting False Loop Closures

Chapter 2 gave us a candidate: "the query looks a lot like place C." But looks like is not is. In a warehouse full of identical shelving, the place recogniser will happily tell you aisle 3 is aisle 8. And a single false loop closure is catastrophic — far worse than missing a true one.

The appearance check is not enough

The bag-of-words score told us two images have similar content. It said nothing about whether they have consistent geometry. Two aisles of identical shelves have near-identical content and completely different geometry — the corners are in different absolute arrangements. So after the appearance match we demand a second, independent test: can the feature correspondences be explained by a single rigid transform? If yes, it is the same place. If no, the appearance match was an alias.

Geometric verification with RANSAC

We have a set of putative feature correspondences between the query image and the candidate: point p_i in one matches point q_i in the other. If this is a true loop, there exists one rigid transform (R, t) such that q_i ≈ R·p_i + t for most pairs. But many of the putative matches are wrong — the descriptor matcher makes mistakes. Those wrong matches are outliers, and a least-squares fit over all of them would be dragged off by the outliers.

RANSAC (RANdom SAmple Consensus) cuts through this. The recipe:

How many iterations? Derive it.

You do not guess k — you compute it. Let w be the fraction of correspondences that are inliers, and s the minimal sample size. The probability that all s points in one random sample are inliers is w^s. The probability that a single sample is not all-inlier is (1 − w^s). After k independent samples, the probability that none was clean is (1 − w^s)^k. Set the probability of at least one clean sample to a target P (say 0.99) and solve:

Worked example 1 — w = 0.6, s = 2. Suppose 40% of the correspondences are wrong, so the inlier fraction is w = 0.6, with s = 2 for a 2-D rigid transform. Then w^s = 0.6² = 0.36. Plug in, keeping every intermediate:

Eleven samples. That is the whole reason RANSAC is cheap: because the minimal sample is tiny, even at 40% outliers you need barely a dozen tries to hit a clean one with 99% confidence.

Worked example 2 — raise the sample size to s = 4. Keep the same inlier fraction w = 0.6 but suppose the model needs four points to fit (say a homography rather than a rigid transform). Now every intermediate shifts — watch what raising the exponent does:

The count tripled — 11 → 34 — for the same outlier rate, purely because the sample grew from 2 points to 4. That is why you always use the smallest model that defines the transform: the required iterations grow roughly as w^−s, so every extra point you demand in the minimal sample multiplies your work.

Compute it directly. The whole derivation is one line of code — no table lookup, no guessing. This is exactly what you would jot at a whiteboard to justify an iteration budget:

python
import math

def ransac_iters(w, s, P=0.99):
    # w = inlier fraction, s = minimal sample size, P = target confidence
    return math.ceil(math.log(1 - P) / math.log(1 - w**s))

print(ransac_iters(0.6, 2))  # -> 11
print(ransac_iters(0.6, 4))  # -> 34
print(ransac_iters(0.5, 2))  # -> 17  (worse inliers cost more tries)
print(ransac_iters(0.8, 2))  # ->  5  (cleaner matches, far cheaper)

How the inlier fraction moves the budget. Fix s = 2 and sweep w. The table grinds each case out to its final k so you can see the sensitivity — a place recogniser that returns cleaner correspondences (higher w) is worth far more than a faster RANSAC loop:

Halving the outliers (w: 0.5 → 0.8) collapses the budget from 17 tries to 5 — a 3.4× speedup that costs nothing at run time. This is why the frontier of loop closure (SuperGlue, below) invests so heavily in producing clean correspondences before RANSAC ever runs.

Consistency checks beyond a single pair

Geometric verification catches an isolated false match. But sophisticated systems add a second layer of defence — temporal / structural consistency:

• Sequence consistency (SeqSLAM-style): a true loop is not a lone frame — if you really re-entered aisle 3, the next few frames should match the frames after the old aisle-3 visit too. Demand a consistent short sequence, not one image.
• Covisibility consistency (ORB-SLAM): the matched keyframe and its graph neighbours must all support the loop. A candidate whose neighbours disagree is rejected.
• Chi-squared gating: once you have the candidate transform, compute its residual under the estimated covariance. If it is a statistical outlier (Mahalanobis distance beyond a χ² threshold), drop it.

Chi-squared gating, worked out

RANSAC's inlier count is a raw vote; chi-squared gating is the statistical version of the same reject-if-inconsistent idea, and it is what a filtering-based backend uses directly. The candidate loop closure predicts a relative pose; the graph already believes a relative pose from odometry. Their disagreement is a residual r — a small vector, here the x/y position error in metres. But you cannot judge a raw residual: 20 cm of error is huge if your sensor is accurate to a centimetre and negligible if it is accurate to a metre. So you weight the residual by the inverse of its covariance Σ (how much error you expected on each axis) and form the Mahalanobis distance — error measured in units of its own standard deviation:

Worked example. The loop closure leaves a residual r = (0.18, 0.10) metres. The expected error is σ_x = 0.1 m and σ_y = 0.2 m, so the covariance is diagonal, Σ = diag(0.1², 0.2²) = diag(0.01, 0.04). For a diagonal Σ the inverse is just the reciprocal of each entry, and the Mahalanobis distance is a weighted sum of squares:

Now compare against the gate. The residual has 2 degrees of freedom (x and y), so the threshold is the 95th percentile of a χ² distribution with 2 DOF, which is 5.99. Since d² = 3.49 < 5.99, this loop closure is statistically consistent — accept it. Notice the x-axis alone contributes 3.24 of the 3.49: an 18 cm error on a 10 cm-σ axis is 1.8σ, already most of the budget, while the same absolute error on the looser y-axis would barely register.

Contrast a false loop that leaves a larger residual r = (0.30, 0.45): the same arithmetic gives 0.30²/0.01 + 0.45²/0.04 = 9.00 + 5.06 = 14.06, which exceeds 5.99 (and even the 99% gate of 9.21) — so gating rejects it. That single scalar comparison, d² vs a χ² threshold, is the entire acceptance test, and it is why every measurement in a filter carries a covariance: without Σ you have no ruler to decide whether a residual is "too big."

DEBUG — the false-loop signature

FRONTIER

Rejecting false loops after they enter the graph is the domain of Switchable Constraints (Sünderhauf & Protzel, 2012) and Dynamic Covariance Scaling (Agarwal et al., 2013): instead of a hard accept/reject, each loop-closure factor gets a switch variable the optimizer can turn off if the constraint disagrees with everything else — making the backend robust to a fraction of false positives that slipped past the frontend. The frontend defence — learned verification — is advancing too: SuperGlue (Sarlin et al., 2020) replaced nearest-neighbour matching + RANSAC with a graph-neural-network matcher that reasons about all correspondences jointly, producing far cleaner inlier sets before RANSAC even runs.

Chapter 4: Global vs Local Consistency

We have a verified loop closure. We feed it to the backend. Something snaps. But what, exactly, does it fix — and what does it not? Getting this precise is the difference between an engineer who understands SLAM and one who repeats "loop closure makes it accurate."

Two kinds of consistency

Local consistency means each measurement agrees with its neighbours: consecutive poses have plausible relative motion, the map near the robot lines up. Pure odometry is locally consistent everywhere — that is exactly why the first two aisles looked perfect.

Global consistency means the whole map agrees with itself, including places connected only through long chains of measurements. The doubled shelf wall is a global-consistency failure: locally each pass was fine, but the out-pass and the return-pass disagree by a metre because the drift between them was never constrained. A loop closure is precisely a global constraint — it links two poses that local measurements never connected.

Where the correction goes — the residual-spreading law

This is the single most important quantitative fact in the chapter, and it comes straight from least squares. Consider a cycle of edges each measuring a relative pose, one of which is the loop closure with a total inconsistency (the loop error) of e. Minimising the sum of weighted squared residuals, the correction each edge k absorbs is:

Read it plainly: each edge takes a share of the loop error proportional to its variance. A confident (low-σ) edge barely moves; an uncertain (high-σ) edge absorbs most of the correction. With equal weights the error is split evenly — the corrected chain is smooth, not kinked.

Where the formula comes from — the 1-D normal equations

You do not have to take this on faith; it drops straight out of one-dimensional weighted least squares. Line up the loop's edges along a single axis. Let x₀, x₁, … x_n be the pose coordinates, and let each edge k measure a relative step z_k with weight w_k = 1/σ_k². The edge's residual is r_k = (x_k − x_k−1) − z_k, and we minimise J = ∑_k w_k r_k².

Now the trick that makes it a one-liner: a cycle is a closed chain, so the corrections are not free — they must sum to the loop error. Write δ_k for the change we make to edge k's step. The constraint is ∑_k δ_k = e (the whole loop must close), and we want to spend that budget to minimise ∑_k w_k δ_k². Form the Lagrangian L = ∑_k w_k δ_k² − λ(∑_k δ_k − e). Set ∂L/∂δ_k = 0:

So each correction is proportional to 1/w_k = σ_k². Substitute back into ∑_k δ_k = e to pin down λ: λ/2 · ∑_m (1/w_m) = e, hence λ/2 = e / ∑_m σ_m². Putting that back gives exactly the law above — each edge absorbs δ_k = e · σ_k² / ∑_m σ_m². The full multi-dimensional pose-graph version (with Jacobians and the sparse information matrix) is derived in the SLAM Backend lesson; the 1-D case above is the whole idea with none of the bookkeeping.

Worked example — a 6-edge cycle by hand

Take the exact cycle the widget below animates: six edges with standard deviations σ = [1, 1, 1, 3, 1, 1] and a loop error e = 1.0. Edge 3 is the shaky one (three times the noise); the rest are confident. Do the arithmetic in the open.

First square the sigmas to get variances: σ² = [1, 1, 1, 9, 1, 1]. Sum them for the denominator:

Now the share each edge absorbs. The shaky edge 3 takes

while every one of the five confident edges takes only

Sanity check that the corrections spend the whole loop error and no more — they must sum to e:

That is the residual-spreading law made concrete: one edge that was three times noisier soaks up 64% of the correction while the five confident edges together absorb the remaining 36%, split evenly. The widget computes exactly these shares internally — drag its slider to reproduce the σ₃ = 3 case and you will see edge 3's bar hit 0.64 of the total.

The takeaway is what the number means: the loop closure fixes global consistency by trusting each measurement in proportion to its confidence, never overruling a well-trusted edge to satisfy a shaky one.

Second worked example — two edges compete for the budget

One shaky edge is the easy case; the law's real teeth show when two edges are uncertain and must split the correction between themselves. Keep the loop error e = 1.0 but make edge 1 twice as noisy and edge 3 three times as noisy: σ = [1, 2, 1, 3, 1, 1]. Now two edges are competing for the same correction budget, and we want to see which one wins and by how much.

Square the sigmas to variances — and notice the squaring is what makes the competition lopsided: σ² = [1, 4, 1, 9, 1, 1]. Sum for the denominator:

The two noisy edges each take a share proportional to their variance:

while each of the four confident edges takes only δ_k = e · 1 / 17 = 0.059. Sanity-check the budget — the six shares must sum to e:

Here is the sharp lesson. Edge 3 is only 1.5× noisier than edge 1 in standard deviation (3 vs 2), yet it absorbs 2.25× as much correction (0.529 vs 0.235). The ratio is exactly (3/2)² = 9/4 — because the law spreads error by variance, not by sigma, a modest gap in noise becomes a wide gap in who pays. Between them the two shaky edges soak up 13/17 ≈ 76% of the whole correction; the four confident edges together take the remaining 4/17 ≈ 24%. That squaring is why a single badly-modelled edge covariance can quietly dominate a solve: the optimiser dumps the loop error into whatever you told it was least certain, and it does so super-linearly.

What a loop closure does NOT fix

Now the sharp edge. Recall Chapter 1: a loop closure lives in the observable subspace. So:

The monocular-scale row, made concrete — scale is a second gauge freedom

The table gives the monocular-scale case one line; it deserves a number, because it is the most famous way a loop closure lies to you. A single camera recovers geometry only up to scale: every relative-pose measurement z is really a direction times an unknown length. So take our radius-2 ring, whose true adjacent chord is 2R·sin(π/N) = 1.531 m, and imagine the whole run was reconstructed at the wrong scale factor a = 1.5, making every chord 1.5 × 1.531 = 2.296 m.

Now compute the loop-closure residual r = (p_j − p_i) − z at the true scale and at the wrong scale. At a = 1: the poses are the true ring and the loop measurement is the true relative pose, so r = 0. At a = 1.5: every pose coordinate is 1.5× larger, and the loop measurement — also derived from the same monocular frame — is 1.5× larger too. So (p_j − p_i) and z scale together, and r = 0 again:

Read that carefully: the loop residual is identically zero at every scale. The optimiser sees no error to correct because there is none in its objective — a globally consistent map at 1.5× scale is exactly as valid, to the least-squares cost, as one at true scale. Scale is a second gauge freedom, sitting right beside the absolute-position gauge from the anchor row: the loop pins the map's shape but not its size. Only a metric input the cost can actually feel — a stereo baseline, an IMU's gravity-scaled accelerations, a wheel-encoder distance, one known landmark span — enters a residual that is nonzero at the wrong scale and thus removes the freedom. This is why monocular VO/SLAM pipelines are always paired with one metric sensor; the loop closure alone can close a geometrically perfect ring at the wrong size and report a healthy residual the whole time.

Worked example — drive two steps of the ring by hand

Let us make "the gap" a real number before the widget hands us one. The bench walks eight poses around a radius-2 ring and returns near the start. The true relative step between two adjacent ring poses is a chord, and a chord of a circle subtending an angle 2π/N has length 2R·sin(π/N). With R = 2 and N = 8:

The first true step points from pose 0 = (2, 0) to pose 1 = (1.414, 1.414), i.e. the vector (−0.586, 1.414). Now corrupt it the way the odometry does: rotate by the per-edge heading bias b = 0.11 rad and stretch it 2% long. Rotating (−0.586, 1.414) by 0.11 rad and scaling by 1.02 gives the odometry step

One step in, the length error is only 1.561 − 1.531 = 0.03 m — tiny, locally consistent. But the heading bias accumulates: edge k is rotated by k·b, so by the last edge the direction is off by 7 × 0.11 = 0.77 rad ≈ 44°. Summing all seven curled-and-stretched steps, the dead-reckoned chain never returns to the start — the raw end-to-start displacement lands at roughly (−0.001, +0.028) while the loop says it should be loop_meas = (0.586, 1.414). The gap is the length of the difference:

So the seam is open by about 1.5 m before we optimise — that is real arithmetic, not a slogan. Feeding the loop closure to the backend collapses it to gap_after ≈ 0.009 m, a 175× shrink: the ring is now globally consistent. Here is the honest part. The mean distance of each pose from the unknowable truth ring is 0.87 m before closure and 0.90 m after — essentially unchanged. Consistency improved 175-fold; accuracy did not budge. The reason is exactly the residual rotation the loop cannot see: the whole ring is still rotated by roughly b·N/2 = 0.11 × 8 / 2 = 0.44 rad, the constant bias that every edge and the loop measurement all share. No relative constraint can observe a rotation that is baked into every measurement equally; only an external anchor (GPS, a known landmark) removes it.

Why the anchor pins the gauge — a numerical trace of Lam[0,0]

The bench you just ran opens with one line that looks like bookkeeping but is doing all the gauge-fixing work: Lam[0,0] += 1.0 / sig_prior ** 2 with sig_prior = 1e-3. Follow the arithmetic and you see why that single entry pins the absolute-position gauge the loop closure leaves free. A prior of standard deviation 10⁻³ carries weight

Compare that to the edges touching pose 0. The odometry edge (σ = 0.10) contributes w_odo = 1/0.10² = 100, and the loop edge (σ = 0.02) contributes w_loop = 1/0.02² = 2500. Pose 0's diagonal entry is the sum of everything anchored there:

The prior alone is 1 000 000 / 1 002 600 ≈ 99.7% of that number — it out-weighs the odometry edge by 10⁴× and the loop edge by 400×. When the solver forms η₀ = anchor / σ_prior², that giant weight forces pose 0 to sit essentially exactly at the anchor coordinate: any deviation costs w_prior · (deviation)², so a millimetre of slack costs a million times more than a millimetre of odometry residual. That is how a rank-deficient normal-equation system becomes solvable: without the prior, Λ has a null space (translate the whole map and no relative residual changes — exactly the unobservable position gauge from Chapter 1), and np.linalg.solve would hit a singular matrix. The anchor prior adds the one entry that lifts Λ to full rank and nails down the free direction — not by measuring position, but by declaring one pose known. Drop sig_prior toward zero and the pin gets harder; raise it and pose 0 floats, and if you remove the prior entirely the solve fails on a singular Λ. The loop closure fixes shape; this one entry fixes where the shape sits.

DESIGN — when to re-optimise, and how much

A loop closure triggers a re-optimisation of the pose graph, and that is not free. On a 10,000-pose graph a full batch solve is expensive, so production systems are incremental:

DEBUG — the optimiser diverged after a good loop

FRONTIER

The incremental re-optimisation that makes real-time loop closure possible is iSAM2 (Kaess et al., 2012), which represents the factor graph as a Bayes tree and, when a loop closure arrives, re-eliminates only the affected cliques rather than the whole graph — turning a seconds-long batch solve into a millisecond update. It is the algorithm underneath GTSAM's smoother and the reason a robot can close a loop across a 10,000-keyframe map without stalling.

Chapter 5: Map Reuse & Lifelong SLAM

So far the robot built a map once and closed loops within a single run. But the warehouse robot comes back tomorrow, and the day after, for years. That changes everything. Now the map is not a scratchpad — it is a persistent asset that must be stored, loaded, matched against on day two, and kept from growing without bound. This is lifelong SLAM, and it turns loop closure into a superpower: relocalization.

Relocalization = loop closure against a saved map

Relocalization is the same machinery as loop closure, pointed at a different target. Loop closure asks "have I been here during this run?"; relocalization asks "am I somewhere in the previously saved map?" The pipeline is identical:

This is what lets a robot "wake up lost" (the kidnapped-robot problem) and recover: no odometry history, just an image and a saved map. It is also how a delivery robot resumes a route after a reboot, and how AR headsets re-anchor virtual content to a previously scanned room.

Trace the exact data through those four boxes so it is a system, not a slogan. Query: the current 640×480 image goes in, ORB extracts ~1000 keypoints each with a 32-byte descriptor, and the BoW pooling collapses them into one sparse word-count vector over a ~1M-word vocabulary — a few KB out. Retrieve: that vector indexes the inverted database and returns a shortlist of, say, the top-10 candidate keyframe IDs by BoW similarity — ten 8-byte IDs out. Verify: for each candidate, its stored 3-D map points (each a 12-byte XYZ) are matched to the query's 2-D keypoints and PnP+RANSAC runs the 35 iterations we will derive, returning an inlier count and a candidate pose — the decision is the inlier count, an integer. Snap: the winning candidate's map pose composes with the recovered relative pose to yield a single 4×4 (or 3×3 in 2-D) transform — the robot's pose in the map frame. Image in, one matrix out; every arrow carries a concrete payload.

CONCEPT — why one snap fixes the whole session

Here is the claim that makes relocalization feel like magic, and it deserves a derivation rather than a hand-wave: a single successful PnP snap against the saved map fixes ALL of the accumulated drift and gauge freedom of the resumed session at once. Not "reduces it" — removes it entirely, in one step. Why?

The resumed session has been dead-reckoning since boot. It has built a fresh local chain of poses, but that chain lives in an arbitrary frame: the robot picked its start as the origin, so every pose it knows is expressed relative to that private origin, not relative to the saved map. Call the current camera pose, as the session knows it internally, T_kf→current — the rigid transform from a saved keyframe's frame to where the camera is now, which is exactly what PnP+RANSAC recovers when it matches the current image against that keyframe's 3-D points.

The saved map already stores where that keyframe sits in the map frame: T_map→kf. Compose them:

That one matrix product re-expresses the current pose in the map frame. And because every other pose in the resumed session is tied to the current pose by the (drift-corrupted but internally consistent) odometry chain, applying the same left-multiply T_map→kf to the whole chain drops the entire session into the map. The private origin the session invented is gone; the gauge is pinned by the map. This is the same reason Chapter 1's loop closure fixed the gauge — relocalization is that fix pointed at a saved anchor instead of a within-run one.

Work it with real numbers so it is yours. Say the saved keyframe sits at map pose (3.0 m, 1.0 m, heading 30°), and PnP recovers the current camera at (0.4 m, −0.2 m, −10°) relative to that keyframe. Compose the two SE(2) transforms — rotate the keyframe-relative offset by the keyframe's 30°, add the keyframe's translation, and add the headings:

The current camera is at (3.446 m, 1.027 m) heading 20° in the map frame — a full 6-DOF (here 3-DOF in 2-D) pose, recovered from one image and one snap, with no odometry history required. Every pose the session dead-reckoned before this instant snaps into the map by the same left-multiply. One PnP; whole session anchored.

"Same machinery, different target" is the headline, but the operational differences are where the engineering lives — tabulate them so the equivalence does not paper over what actually changes:

The row that matters most is "prior available." Loop closure usually has a decent odometry guess to sanity-check a candidate against; relocalization frequently does not, which is why its acceptance bar (the inlier count we derive next) has to be strict enough to stand on geometry alone.

CONCEPT — how many inliers make a relocalization trustworthy

A snap is only as good as the geometric verification behind it, and the acceptance decision is a count, not a fit — the same lesson as Chapter 3's RANSAC. But relocalization raises the stakes: a false relocalization does not just fold a within-run map, it teleports the robot to the wrong place in a map it will then trust for hours. So we derive two numbers.

First, the iterations. Reuse Chapter 3's formula k = log(1−P) / log(1−w^s), but with the PnP minimal sample s = 3 (three 3-D↔2-D correspondences fix a camera pose; this is P3P). For a coin-flip inlier fraction w = 0.5 and a 99% success target:

Thirty-five PnP hypotheses buy 99% confidence of hitting one clean 3-point sample even when half the matches are wrong — cheap, because the minimal sample is small.

Second, the inlier threshold. How many inliers must the winning hypothesis explain before you believe the snap? Frame it as a false-positive tail: if the candidate keyframe is actually the WRONG place, its matches are essentially random, and a random correspondence lands within the reprojection tolerance τ only by luck — say with probability p₀ ≈ 0.05. With n = 24 correspondences, the chance that a wrong candidate coughs up at least m inliers by pure chance is the binomial tail:

Compute the tail by hand for a few thresholds: m=4 gives 0.030, m=5 gives 0.0060, and m=6 gives 0.00096 — the first threshold that drops the false-accept probability below 1 in 1000. So a rule of "accept only with ≥ 6 geometric inliers" makes an accidental relocalization a sub-0.1% event per candidate. That is the number you would defend at a whiteboard: not a guessed constant, but the m where the binomial tail crosses your error budget.

DESIGN — what a map costs to store

A feature-based map is keyframes, each holding poses, 2-D keypoints, their descriptors, and the 3-D map points they observe. Put real bytes on it:

Work the arithmetic by hand so the numbers are yours. Each ORB feature carries a 32-byte binary descriptor, an 8-byte 2-D keypoint (two floats), and a 12-byte 3-D map point (three floats): 32 + 8 + 12 = 52 bytes. A keyframe with 1000 features plus a 24-byte pose is 1000 × 52 + 24 ≈ 52 KB. A 1 km route at one keyframe every 0.5 m is 2000 keyframes, so 2000 × 52 KB ≈ 104 MB.

Now the lifelong number. An 8-hour shift at 1 m/s covers 8 × 3600 = 28,800 m; keeping one keyframe every 0.5 m is 57,600 keyframes; at 52 KB each that is 57,600 × 52 KB ≈ 3 GB per day. Keep that forever and within a month the map is bigger than RAM, the database query slows to a crawl, and the whole system grinds to a halt. Lifelong SLAM's central problem is not building the map — it is bounding it.

DESIGN — derive the break-even map size

The last paragraph asserted "within a month the query slows to a crawl." Do not assert it — derive the exact map size at which growing the map stops paying for itself. This is a different kind of worked example from the sizing multiplications above: instead of plugging numbers into a formula, we set up the two costs that trade off and solve for where they cross.

Relocalization has a benefit and a cost, and both scale with the map. The benefit is fixed per successful snap: a relocalization saves you the time to rebuild orientation from scratch — call it S, the wall-clock a cold restart would otherwise burn re-exploring until it re-anchors. Put a number on it: S ≈ 0.2 s = 200,000 µs. The cost is the database query itself. A place-recognition query shortlists candidates and geometrically verifies them, and in the regime that bites, that work grows linearly with the number of keyframes M in the map — call it c_q · M, where c_q is the query cost per keyframe. Put a number on it: c_q ≈ 0.5 µs per keyframe.

The map is worth growing only while the query still costs less than the snap saves. The break-even is where they are equal:

Substitute and turn the crank — every intermediate step:

Convert that to bytes at 52 KB/keyframe to feel it: 400,000 × 52 KB ≈ 20.8 GB. Past this size, each relocalization query costs more time than the relocalization saves — the map is now a net drag, and no amount of "keep everything, storage is cheap" reasoning survives it. Bounding is not tidiness; it is the point beyond which the feature stops working. (Change the constants and the number moves, but the method — equate the per-query cost to the per-snap saving and solve for M — is what you reproduce at a whiteboard.)

Why is the query cost linear in M in the first place? Because of what the retrieval database actually is, byte for byte. A bag-of-words database is an inverted index: for each visual word in the vocabulary, a posting list of the keyframes that contain it. To answer a query, you extract the query image's words, walk their posting lists, and accumulate a similarity score for every keyframe that shares a word — then verify the top handful geometrically. The accumulation touches every keyframe that shares any common word, and in a lifelong map where the same corridor is re-observed thousands of times, the popular words' posting lists grow with the number of keyframes. So the score-accumulation step is O(M) in the worst case — exactly the c_q·M cost model the break-even used. (A vocabulary tree makes the word assignment logarithmic, but the posting-list accumulation is still linear in the matched keyframes; that is the term that eventually dominates.) This is why culling is not optional bookkeeping — every redundant keyframe you fail to drop lengthens the posting lists that every future query has to walk.

Keeping the map bounded

• Keyframe culling (ORB-SLAM): drop a keyframe whose map points are ~90% visible in three other keyframes — it is redundant. This keeps map size proportional to space explored, not time spent. A robot sitting still adds no keyframes.
• Map summarisation: keep a diverse subset of keyframes that still covers the space, chosen to maximise localisation utility per byte.
• Marginalisation (fixed-lag): Schur-complement old poses out of the active window, keeping their information as a prior (Chapter 1's fill-in cost applies — see the SLAM Backend).

Before applying the rule, derive the two constants in it — the "90%" and the "3 other keyframes" are not arbitrary. Why ≥ 3 other observers, not 1 or 2? A 3-D map point is only usable for localization if it can be triangulated and remains geometrically constrained after a keyframe is removed. Triangulation needs at least 2 views to fix a point's depth; drop a keyframe that a point depends on, and if only 1 other view remains, that point is now under-constrained (a single ray, infinite depth ambiguity). Requiring the point to survive in ≥ 3 other keyframes guarantees that after culling this one, at least 3 views still see it — comfortably above the 2-view triangulation minimum, with one spare against a future cull. So "3" is "the triangulation minimum, plus margin so one more cull is still safe." And why 90%, not 100%? Because demanding every single point be redundant would refuse to cull a keyframe over one straggler point on the field's edge; 90% tolerates a few boundary points while still guaranteeing the keyframe's bulk geometry is preserved elsewhere. The two constants encode "the map point stays triangulable (≥3), and almost all of them do (90%)."

The culling rule is a bullet in every paper and a mystery in most implementations, so make it concrete. Take six keyframes, each with the set of map-point IDs it observes. The interior of a corridor is heavily co-observed; the frontier is not:

Hand-apply the ORB-SLAM rule to KF2: for each of its points, count how many other keyframes also see it, and it is "covered" if that count is ≥ 3.

• point 3 → also in KF0, KF1, KF3 → 3 others — covered
• point 4 → also in KF0, KF1, KF3, KF4 → 4 others — covered
• point 5 → also in KF0, KF1, KF3, KF4 → 4 others — covered
• point 6 → also in KF1, KF3, KF4 → 3 others — covered

All 4 of KF2's points are covered: redundancy = 4/4 = 1.00 > 0.9, so KF2 is culled. Now do KF5: its points {20…24} appear in no other keyframe, so 0/5 are covered, redundancy = 0.00 — KF5 stays, because it is the only witness to fresh space. One culling pass on this set drops exactly KF2 and keeps {0, 1, 3, 4, 5}: 6 keyframes become 5, and the survivors still cover every map point. That is the whole mechanism — the map shrinks where it is over-witnessed and never where it is thin.

The from-scratch loop above is the version you should be able to write at a whiteboard, but production code precomputes a co-observation count per map point once and reuses it, so a cull check is a table lookup instead of a nested scan. The same logic, one pass, no inner loop:

python
from collections import Counter

# count, ONCE, how many keyframes observe each map point
obs = Counter(p for pts in kfs.values() for p in pts)

def redundancy(k):
    pts = kfs[k]
    # "others" = total observers minus this keyframe itself (1)
    covered = sum(1 for p in pts if obs[p] - 1 >= 3)
    return covered / len(pts)

cull = [k for k in kfs if redundancy(k) >= 0.9]   # -> [2]

Precomputing obs turns the culling sweep from O(K²·P) — every keyframe against every other — into O(K·P) after one O(K·P) counting pass, which is why real systems maintain the co-observation counts incrementally as keyframes are added rather than recomputing them on each cull.

The payoff is the bound. Every culling pass removes only over-witnessed interior keyframes, so the surviving count tracks the volume of space, not the hours logged — a robot circling one aisle all day adds keyframes only where coverage was thin, and the map size saturates instead of climbing. That saturation is exactly what the widget below animates.

The appearance-change problem

Day two is not day one. The lighting is different, boxes have moved, a season has passed. A map built in summer may not relocalize in winter with the same descriptors. Two responses:

Experience maps sound expensive — "store every appearance" — but the whole trick is that they do not. Put real bytes on one experience so the data flow is concrete. When a place fails to relocalize against its existing experiences, the system saves a new one: a global descriptor for retrieval (NetVLAD PCA-256 ≈ 1 KB) plus a compact bag of local features for the geometric snap — keep 200 ORB features at 52 bytes each = 200 × 52 ≈ 10.16 KB. So one experience is 1 + 10.16 ≈ 11.2 KB: two descriptor tiers, one to find the place and one to pin the pose.

Now the accumulation, and this is where the novelty gate earns its keep. A new experience is saved only when today's image fails to match any stored experience for that place — not on every pass. So a place that looks the same every day never grows past one experience; a place that swings between summer/winter and day/night accumulates a handful. Take a 2,000-place route where each place averages 4 distinct experiences over a year:

Eighty-seven megabytes to be robust across a year of appearance change — trivial. Contrast the naive alternative of storing an experience on every daily visit: 2,000 × 365 × 11.2 KB ≈ 7.8 GB, ninety times larger and almost all of it redundant copies of unchanged places. The novelty gate is the difference between a map that gets more robust and barely bigger, and one that drowns in duplicate appearances — the same "keep only what adds information" principle as keyframe culling, applied to appearance instead of geometry.

Put a number on the saturation the widget below plots, because "it plateaus" is a claim you should be able to size. Suppose the route is a loop of length L = 200 m and the culling policy keeps roughly one surviving keyframe every d = 2 m of distinct track once a region is well-covered. Then the steady-state map size is bounded by the geometry, not the clock:

One hundred keyframes, forever — whether the robot patrols the loop for one hour or one year, because every lap after the first re-observes points already witnessed by ≥ 3 keyframes, so the redundancy rule culls the new keyframes as fast as they are proposed. Contrast the raw (no-cull) count over an 8-hour shift at 1 m/s with a keyframe every 0.5 m: 8×3600 / 0.5 = 57,600 keyframes, and climbing. The ratio 57,600 / 100 ≈ 576× is the entire value of culling in one number, and it is why the raw curve blows the RAM ceiling while the culled curve flattens. That flattening is what you are about to watch.

DEBUG — relocalization silently failing

DEBUG — the false relocalization (the dangerous failure)

The failure above is loud: the robot knows it is lost. The opposite failure is silent and far worse — a confident wrong relocalization.

This is why the two DEBUG cases pull in opposite directions and why the threshold is a genuine engineering decision, not a default. The appearance-drift failure tempts you to lower the acceptance bar so week-old keyframes still match; the aliasing failure demands you raise it so look-alike aisles never do. You cannot satisfy both globally — which is exactly why the per-region success-rate log matters: you update the drifted region's appearance (add an experience) rather than weakening the geometric bar that protects the whole map from teleports.

FRONTIER

The systems view of persistent, multi-session mapping was crystallised by Kimera (Rosinol et al., 2020) — a real-time metric-semantic SLAM library with a dedicated loop-closure and pose-graph module built for reuse — and by maplab (Schneider et al., 2018), an open research framework explicitly designed for map merging, re-localization across sessions, and lifelong map maintenance. The learned-relocalization frontier runs from PoseNet (Kendall et al., 2015), which regressed camera pose directly from an image, through modern scene-coordinate-regression methods that predict per-pixel 3-D map coordinates and solve PnP — trading the explicit keyframe database for a network that has memorised the scene.

Chapter 6: The Bench

Everything so far, on one screen. A robot drives a loop through the warehouse. Its odometry drifts — you set how badly. The map shears exactly like the hook: perfect near the start, a metre off and doubled by the far wall. Then you trigger a loop closure and watch the whole trajectory snap back into a consistent ring, with the end-to-start gap collapsing in real time.

Push the drift slider to its maximum and drive: the warm ring peels wildly off the green truth, and the seam gap can exceed two metres — the doubled far wall. Close the loop and the teal ring snaps shut, the gap dropping below a couple of centimetres. Notice what does not happen: at extreme drift the teal ring closes but is still visibly rotated from the green truth — consistency without accuracy, exactly Chapter 4's warning made visible.

The bench runs the same pose-graph relaxation kernel you built in the Chapter 4 Code Lab — and the same one the Studio build session drives. Now open the Studio from the dock and build that kernel yourself: the drifted ring on the stage snaps back the instant your normal equations are right. But the bench is not a black box, and this chapter refuses to defer its depth to earlier ones. Below, we assemble by hand the exact graph the canvas above is solving — the same N = 10 ring, the same weights, the same anchor — and read every number it prints.

CONCEPT — why closing one seam bends the whole chain

The odometry the bench lays down is a chain: node x0 is trusted (the robot started there), and every later node is xi+1 = xi + zi, where zi is the measured step for edge i. Each step carries a heading error, and because heading integrates, a constant per-edge bias β makes the heading at edge k equal to k·β — the drift grows edge by edge, exactly what Chapter 1 called the super-linear seam.

A loop closure adds one more edge the graph did not have: a relative measurement from the last node back to the first, x0 − xN−1 = zloop. On its own that is a single equation. What makes it bend the entire ring is that the graph is solved as one weighted least-squares system, not edge by edge. Write the residual of edge k as rk = (xk+1−xk)−zk and weight it by wk = 1/σk2. The optimiser minimises the total Σk wk rk2. When the loop injects a mismatch e at the seam, the optimum spreads that error backward across every edge in inverse proportion to weight — this is the residual-spreading law from Chapter 4:

A stiff edge (large w, small σ) barely moves; a soft edge absorbs most of the correction. With nine equally-soft odometry edges (wodo = 100) and one very stiff loop edge (wloop = 2500), the correction is shared almost entirely by the odometry chain: each odometry edge takes (1/100) / (9/100 + 1/2500) = 0.01/0.0904 ≈ 11.06% of the closing error, and the loop edge itself takes only (1/2500)/0.0904 ≈ 0.44%. Nine tiny nudges, distributed, unshear the whole ring — that is why the far wall un-doubles even though you only measured one seam.

DESIGN — the rates and weights the bench actually uses

Every weight in the canvas above is a design decision, and the decisions are not arbitrary. Here is the full parameter set, straight out of _c6solve, with the reason each value is what it is.

Those three weights are not just a stiffness policy — they are the exact numbers that land on the diagonal of Λ, one node at a time, and you can build that diagonal by hand without running anything. The rule is mechanical: a node’s diagonal is the sum of the weights of every edge that touches it, because each edge stamps +w onto both endpoints. Walk the ring node by node, count the edges each touches, and add their weights:

Read the diagonal off the last column top to bottom and you have diag(Λ) = [1{,}002{,}600, 200, 200, 200, 200, 200, 200, 200, 200, 2600] — the exact vector Worked example 2 will confirm from the assembler. Three numbers do all the work. Every plain interior node (1 through 8) touches two identical odometry edges, so its diagonal is a flat 200; the drift is invisible here because stiffness lives in the weights, not the measurements. Node 0 is huge because the 106 anchor sits on top of its two edges — that is the gauge-pinning prior refusing to let node 0 move. Node 9 is 2600 because the stiff 2500 loop edge replaces one of its odometry neighbours — that is the seam being held near-rigid so the chain bends to it, not the reverse. The 25× ratio you set in the table above is exactly the difference between node 9’s 2600 and every interior node’s 200.

Worked example 1 — how a 0.11 rad bias becomes a metre of seam

Set the slider to β = 0.11 rad (its default) and drive. Each true step is a chord of the radius-2 ring: with N = 10 nodes the chord length is L = 2·sin(π/10)·2 = 1.236 m. The odometry rotates step i by the accumulated heading error i·β. Integrate the heading edge by edge, no formula, just addition:

In degrees that is 6.3°, 12.6°, 18.9°, 25.2° — already a quarter-turn off by the fourth edge. A step of length L rotated by φ lands its tip 2·L·sin(φ/2) away from where the true step would have put it. Substitute:

• edge 1: 2·1.236·sin(0.055) = 2.472·0.05497 = 0.136 m off
• edge 2: 2·1.236·sin(0.110) = 2.472·0.10978 = 0.271 m off
• edge 3: 2·1.236·sin(0.165) = 2.472·0.16425 = 0.406 m off

Those per-edge displacements do not cancel — they compound in roughly the same direction because the bias is one-signed. Accumulate the actual end-position error and it climbs: 0.14 m after 1 edge, 0.42 m after 3, and by the ninth edge the drifted end sits 2.18 m from the true end. The end-to-start seam the bench reports is gapbefore = 1.802 m — the "metre off, doubled far wall" of the hook, produced entirely by that innocent 0.11 rad per edge.

Worked example 2 — one node’s normal equation, by hand

Now watch the loop pull the seam shut. The bench assembles a symmetric information matrix Λ and vector η where each edge (i, j) of weight w stamps +w onto Λii and Λjj, −w onto Λij and Λji, and pushes ±w·meas into η. Consider one interior node, x5. It touches exactly two odometry edges — (4,5) and (5,6) — so its diagonal accumulates:

Its row of Λ (x-axis) is therefore […, −100, +200, −100, …] — the two off-diagonal −100 entries are the edges to its neighbours. Contrast the seam node x9: it carries one odometry edge and the stiff loop edge, so Λ9,9 = 100 + 2500 = 2600, and the anchored node x0 gets the prior on top of its two edges: Λ0,0 = 1{,}000{,}000 + 100 + 2500 = 1{,}002{,}600. Running the bench’s own assembler confirms the diagonal exactly:

The loop edge’s −2500 off-diagonal linking node 9 back to node 0 is the single entry that ties the two ends of the open chain together. Solve Λx = η and the seam collapses: at β = 0.11 the bench prints gap 1.802 m → 0.008 m — a 1.802 / 0.008 ≈ 226× tightening. Because the loop is 25× stiffer than any odometry edge, the chain does almost all the moving; the loop residual is driven to nearly zero while nine tiny per-edge nudges spread the correction, exactly as the δ_k law predicted.

Worked example 3 — read the whole η column of the N=10 ring, node by node

Worked example 2 built the left side of Λx = η — the diagonal. The right side η is just as mechanical, and reading it end to end shows you at a glance which nodes the solver is being pulled hard, and which barely at all. The rule, straight from edge(), is: for edge (i, j) with measurement meas, subtract w·meas from ηi and add w·meas to ηj. Work the x-axis. Take the nine measured x-steps of one drifted drive — call them z0…z8 — the measured x-closure zloop, and the anchor target x0true. For a concrete drive these are:

Every interior node k (that is, 1 through 8) is the j of the edge coming into it and the i of the edge leaving it, so it collects +wodozk−1 and −wodozk — a single tidy formula ηk = wodo(zk−1−zk). Node 0 additionally carries the anchor push wp·x0true and the loop edge’s +wloopzloop; node 9 carries the loop edge’s −wloopzloop. Walk it down the column, doing each subtraction by hand:

The shape of that column is the physics of the solve. The eight interior entries are all small — between 0 and ±95 — because each is a difference of two nearly-equal odometry steps: wodo(zk−1−zk) is near zero whenever consecutive steps agree, which they mostly do. An interior node is barely being asked to move; it exists to relay the correction along the chain, not to originate it. The two extreme entries are the two constraints shouting. Node 0’s ≈2×106 is the anchor prior wp·2.000 = 2{,}000{,}000 swamping everything else — that is what pins the gauge. Node 9’s 4{,}872.6 is the stiff loop term −2500·(−1.902) = 4755 dominating its lone odometry push of 117.6 — that is the seam yanking node 9 toward the trusted closure. When you divide η through by the diagonal you built in Worked example 2, node 0’s huge η over its huge 106 diagonal gives a modest, pinned position; node 9’s loop-dominated η over its 2600 diagonal snaps it onto the seam; and the near-zero interior entries let those nodes drift into whatever smooth arc the neighbours dictate. Reading the right-hand side alone, before you solve a thing, already tells you the ends will hold and the middle will relax — exactly the bow the ATE table further down will measure.

There is a clean self-check hiding in that column, and it catches the commonest assembly bug. Add up just the eight interior entries: 81.3 + 95.1 + 58.8 + 0 − 58.8 − 95.1 − 81.3 + 0 = 0. They telescope, and they must: Σk=18wodo(zk−1−zk) = wodo(z0−z8), and because this ring closes with equal first and last x-steps (z0 = z8 = 1.176) the interior sum is exactly zero. That is not a coincidence of the numbers — it is the statement that the odometry chain, by itself, applies no net force to the interior; every push into one node is pulled back out of its neighbour. All the net force in the whole system lives in the two boundary entries, node 0 and node 9, which is precisely why those are the only two entries that are large. If you ever assemble η and the interior does not telescope to wodo(z0−zN−2), you have a sign error in one of the edge() pushes — the telescoping identity is a free unit test on your assembler.

Finally, hand this η and the diagonal from Worked example 2 to the solver and watch what the near-zero interior actually does. The dead-reckoning x-positions (cumulative sums of the measured steps) start at 0 and wander with the drift; the anchor insists node 0 belongs at 2.000 instead. Solving Λx = η applies a correction to every node — and the correction column is almost a straight ramp:

The correction climbs almost linearly — roughly +0.130 m per node, from +2.000 at the anchor to +3.171 at the seam. That is the residual-spreading law in its purest visual form on one axis: the near-zero interior η entries mean the interior imposes almost no shape of its own, so the solver is free to interpolate a smooth linear correction between the two things it is told — node 0’s anchored target and node 9’s loop-pinned target. A large interior η would have forced a kink; a near-zero one lets the correction glide. This is exactly why the un-doubled ring comes out smooth rather than creased: the right-hand side is quiet everywhere the geometry does not demand otherwise, and a quiet right-hand side is what a straight-line correction looks like in the arithmetic.

Worked example 4 — solve a reduced Λx = η to numbers, by hand

The ten-node ring is too big to eliminate on paper, but the effect it demonstrates is fully visible in the smallest graph that still has a drifting chain and a loop: three nodes on one axis. This is not a toy analogy — it is a real pose graph with the bench’s exact stamp rule and the bench’s exact 25× stiffness ratio, small enough that every elimination step is arithmetic you can do in the margin.

Put nodes x0, x1, x2 on a line. The robot should end two metres from where it started, but its second odometry step over-reads: it measures z01 = 1.0 and z12 = 1.3 (a 0.3 m drift). Dead reckoning therefore lands node 2 at x2 = 0 + 1.0 + 1.3 = 2.3, and the seam against the trusted loop measurement zloop: x0−x2 = −2.0 is |(0−2.3)−(−2.0)| = 0.300 m — the miniature version of the doubled far wall.

Stamp the three edges with the bench’s rule (wodo = 100, wloop = 2500, anchor wp = 106 on x0). But before we “drop” anything, write the graph out in full — all three unknowns, anchor and all — so the anchor’s job is a number on the page, not a hand-wave. Accumulating each edge’s +w onto the diagonals and −w onto the off-diagonals, adding the anchor prior +106 onto Λ00, and pushing ±w·meas into η (with the anchor pushing +wp·0 = 0 for a target of x0 = 0), the complete 3×3 normal equation is:

Every entry in the anchor row earns its place too. Λ00 = 106 + 100 + 2500 = 1{,}002{,}600 is the anchor prior plus node 0’s two edges (the odometry edge to node 1 and the loop edge back from node 2); the −100 and −2500 off-diagonals are those same two edges reaching to nodes 1 and 2. On the right, η0 = wp·0 − wodoz01 + wloopzloop = 0 − 100(1.0) + 2500(−2.0) = −5100. Now eliminate the anchor row explicitly and watch it collapse the system by hand. Normalise row 0 by its pivot 1{,}002{,}600 to get [1, −100/1{,}002{,}600, −2500/1{,}002{,}600 | −5100/1{,}002{,}600] = [1, −9.974×10−5, −2.494×10−3 | −5.087×10−3]. That last entry already tells you the answer for x0: it is −0.0051 m, pinned within five millimetres of the enforced zero — the anchor did its one job. Now subtract (−100)×row 0 from row 1, and (−2500)×row 0 from row 2, to knock x0 out of both. Row 1’s diagonal becomes 200 − (−100)(−9.974×10−5) = 199.990; its x2 coefficient becomes −100 − (−100)(−2.494×10−3) = −100.249; and row 2 reduces to −100.249 and 2593.766 the same way:

This is the Schur complement of the anchor, and it is almost exactly the tidy 2×2 you would have written if you had simply dropped x0 at the start — the diagonal 199.990 is a whisker below 200, the coupling −100.249 a whisker beyond −100, the right side −30.509 and 5117.283 instead of −30 and 5130. Those tiny differences are the anchor’s residual coupling, each of order (edge weight)2/106 — a fraction of a percent, because the 106 pivot in the denominator crushes them. That is what “the anchor pins x0 = 0 so hard we can drop it” means numerically: eliminating the anchor row leaves the interior system unchanged to three decimals, so the reduced problem below is a legitimate shortcut, not an approximation you have to apologise for. From here we solve the clean reduced form the bench’s interior effectively sees:

The anchor pins x0 = 0 so hard we can drop that unknown and solve for (x1, x2) alone. Accumulating each edge’s +w onto the diagonals and −w onto the off-diagonals, and pushing ±w·meas into η, gives the clean 2×2 reduced normal equation:

Every entry earns its place. The 200 is node 1 touching two odometry edges (100+100); the 2600 is node 2 touching one odometry edge and the stiff loop edge (100+2500); the −100 off-diagonals are the odometry edge between them. On the right, η1 = wodoz01−wodoz12 = 100(1.0)−100(1.3) = −30, and η2 = wodoz12−wloopzloop = 100(1.3)−2500(−2.0) = 5130 — the enormous 5130 is the stiff loop pulling node 2 toward the trusted seam. Now eliminate. The determinant is 200·2600−(−100)(−100) = 520000−10000 = 510000, and Cramer’s rule gives:

If you prefer to eliminate the way the bench’s _solve actually does — Gaussian elimination with normalisation, exactly the code you read in the CODE block — the same two numbers fall out in two steps. Normalise row 1 by its pivot 200 to get [1, −½ | −3/20]. Eliminate x1 from row 2 by subtracting (−100)×row 1 from it: the 2600 becomes 2600−(−100)(−½) = 2550, and the right side becomes 5130−(−100)(−3/20) = 5115. Row 2 is now [0, 2550 | 5115], so x2 = 5115/2550 = 341/170 = 2.0059, and back-substitution gives x1 = −3/20−(−½)(2.0059) = 29/34 = 0.8529. This is not a different method that happens to agree — it is the literal sequence of arithmetic the for loops in _solve execute on this matrix, carried out by hand so you can see the pivoting and elimination the code hides behind two nested loops.

The seam that was 0.300 m open now reads |(x0−x2)−zloop| = |(0−2.0059)−(−2.0)| = 0.0059 m — a 51× collapse from a hand-solved 2×2, the same qualitative event the full bench prints as 1.802 m → 0.008 m. And look at how the correction split. The residual of each edge is (xj−xi)−meas: for edge (0,1) it is (0.8529−0)−1.0 = −0.1471; for edge (1,2) it is (2.0059−0.8529)−1.3 = −0.1471; for the loop edge it is (0−2.0059)−(−2.0) = −0.0059. The two odometry residuals settle at the same value −0.1471 while the loop residual is only −0.0059.

That equality is not luck — it is the δk law in miniature, and you can derive the ratio without solving anything. At the optimum, the weighted residuals of edges meeting at an unconstrained interior node must balance, so the two equal-weight odometry edges are forced to carry equal residual; and the ratio of odometry residual to loop residual is fixed by their weights: rodo/rloop = wloop/wodo = 2500/100 = 25. Check it: 0.1471/0.0059 ≈ 25. The soft edges each moved 25× more than the stiff loop edge — the residual-spreading law of Chapter 4, reproduced exactly by three numbers you can verify without a computer, and predictable from the weights alone before you ever form the matrix.

Lay the three edges side by side and something quietly perfect appears in the last column — the weighted residual w·r is identical on every edge:

That the weighted residuals are all equal is not a fluke of these particular numbers — it is a theorem about any single-cycle pose graph. Around one loop the constraints form a chain of “springs,” and at equilibrium a series of springs carries the same tension throughout; here w·r is exactly that tension. The stiff loop spring stretches only 0.0059 m to hold 14.71 units of tension; each soft odometry spring must stretch 25× as far — 0.1471 m — to hold the same tension, because tension is stiffness times stretch and the stiffness ratio is 25. This is the residual-spreading law read as a physical equilibrium, and it is why you can predict the entire correction from the weights before touching the matrix: equal tension, stretch inversely proportional to stiffness.

One last number ties the whole toy together and connects it back to why we solved a least-squares system at all: the total weighted cost. Before the loop, dead reckoning left the two odometry residuals at zero (it trusts its own steps perfectly) and dumped the entire mismatch into the loop edge, whose residual was the full 0.300 m. The cost is χ2 = Σw·r2 = 2500·(0.300)2 = 225.0 — all of it in one edge, a sharp visible kink at the seam. After the optimise, the cost redistributes: 100·(0.1471)2 + 100·(0.1471)2 + 2500·(0.0059)2 = 2.163 + 2.163 + 0.087 = 4.41. The solver found the arrangement that drops total cost from 225.0 to 4.41 — a 51× reduction, the very same factor as the seam collapse, and no accident: minimising χ2 is what “spread the residual” means. And notice the check that proves you are at the true optimum without re-solving: the weighted residuals at the interior node balance exactly, wodor01 = 100·(−0.1471) = −14.71 and wodor12 = −14.71 — equal and opposite forces pulling node 1 in either direction, which is the calculus condition ∂χ2/∂x1 = 0 written as a force balance. When the weighted pushes on an interior node cancel, that node is exactly where least-squares wants it, and you have solved the graph.

That per-edge split is not special to the toy: the full ring does the same thing, nine ways. Take the β = 0.11 seam the bench actually closes — a closing error of e = 1.802 m. The residual-spreading law hands each of the nine odometry edges the identical share (1/wodo)/Σ(1/w) = 0.01/0.0904 = 11.06%, i.e. δ = 0.1106·1.802 = 0.1993 m of correction per edge, and leaves the stiff loop edge only 0.0080 m. Watch the running sum climb to the whole seam:

Nine equal nudges of 0.1993 m sum to 1.794 m; the loop edge contributes the last 0.008 m; together they account for the entire 1.802 m seam. This is the un-doubling of the far wall written out node by node: no single edge is asked to swallow the whole error, so no single edge visibly kinks — the ring comes out smooth precisely because the correction was spread, not dumped.

Why is every odometry share identical at 11.06%, rather than tapering off toward the anchor the way you might expect drift to? Because the bench gives all nine odometry edges the same weight wodo = 100. The δk law weights each edge’s share by 1/wk, so equal weights force equal shares — the running sum climbs in a straight line, 0.1993 m per step. This is a modelling choice, not a law of nature: if you believed a particular stretch of the drive was rougher (say the robot crossed a threshold strip where the wheels slipped), you would raise that edge’s σ, lower its w, and the residual-spreading law would automatically route more of the correction into it — the running sum would jump at that edge instead of stepping uniformly. The uniform staircase you see here is the signature of a homogeneous-odometry assumption, and the moment you break that assumption the correction profile reshapes itself to match where you said the error actually lived.

CODE — the from-scratch assembler the bench runs

This is the actual kernel behind the canvas above, per axis. No library call does the assembly — the edge() helper stamps the four Λ entries and two η entries that are a pose-graph edge; the anchor is one line; the solve is a dense linear solve. Solve x and y independently because the linearised ring decouples per axis.

python
import numpy as np

def close_loop_axis(rel, loop_meas, anchor, N,
                    sig_odo=0.10, sig_loop=0.02, sig_prior=1e-3):
    Lam = np.zeros((N, N)); eta = np.zeros(N)
    # anchor: pin node 0 to remove the unobservable gauge (Ch 2)
    wp = 1 / sig_prior**2          # = 1_000_000
    Lam[0, 0] += wp; eta[0] += anchor * wp
    def edge(i, j, meas, sig):    # ONE pose-graph edge = 4 Lam + 2 eta stamps
        w = 1 / sig**2
        Lam[i, i] += w; Lam[j, j] += w        # diagonals gain +w
        Lam[i, j] -= w; Lam[j, i] -= w        # off-diagonals gain -w
        eta[i]  -= w * meas; eta[j] += w * meas
    for i in range(N - 1):        # the odometry chain (soft, w=100)
        edge(i, i + 1, rel[i], sig_odo)
    edge(N - 1, 0, loop_meas, sig_loop)  # the ONE loop edge (stiff, w=2500)
    return np.linalg.solve(Lam, eta)      # dense solve; bench uses Gaussian elim

The whole loop-closure effect is the last edge() call. Comment it out and you solve a pure open chain: the anchor holds node 0, but nothing ties node 9 back, so the seam never closes. Add it back and the −w at Λ[N−1, 0] couples the two ends into one relaxable system.

Trace the assembler by hand — two full Λ rows from the edge() stamps

The code above is short enough to run in your head for the first and last odometry edges and watch two complete rows of Λ assemble. Nothing here is hidden inside a library; the four Lam[…] += lines in edge() are the entire mechanism. Start with Λ all zeros, then apply the anchor line and the two odometry edges that write into rows 0, 1, 8 and 9.

Edge (0,1), w = 100. The helper runs Lam[0,0] += 100; Lam[1,1] += 100; Lam[0,1] −= 100; Lam[1,0] −= 100. That is one edge’s complete footprint: two positive diagonal stamps and two negative off-diagonal stamps, all of magnitude w. Row 1 has now received its first contribution — a +100 on its own diagonal and a −100 in column 0.

Edge (1,2), w = 100. The next loop iteration stamps Lam[1,1] += 100 (row 1’s diagonal is now 100+100 = 200) and Lam[1,2] −= 100. Node 1 is a plain interior node touching exactly two odometry edges, so its finished row of Λ is:

The two −100 entries sit in columns 0 and 2 — its neighbours — and the diagonal is the sum of the two edge weights it participates in. This is the canonical interior-node stencil of a chain: […,−w, +2w, −w,…]. Every one of nodes 1 through 8 has exactly this shape, shifted along the diagonal.

Edge (8,9), w = 100. Now the seam. This last odometry edge stamps Lam[8,8] += 100; Lam[9,9] += 100; Lam[8,9] −= 100; Lam[9,8] −= 100. But node 9 is not interior — it also receives the loop edge edge(9,0,…,sig_loop), whose w = 2500 stamps Lam[9,9] += 2500, Lam[9,0] −= 2500, and Lam[0,9] −= 2500. So node 9’s diagonal finishes at 100 + 2500 = 2600, and its row carries a second, far-away off-diagonal:

That single −2500 in column 0 is the loop closure made concrete: it is the only entry in the entire matrix that reaches across the open chain to couple the last node to the first. Delete the final edge() call and this entry vanishes — row 9 collapses back to the interior stencil […,−100, +100], node 9 is tied only to node 8, and the ring can no longer close. Running the bench’s own assembler and reading its rows back confirms both by-hand rows to the digit, and the full diagonal is exactly the diag(Λ) = [1{,}002{,}600, 200, 200, 200, 200, 200, 200, 200, 200, 2600] printed in Worked example 2. Two edges, four stamps each, and you have reconstructed the two most interesting rows of the matrix the canvas is solving — no black box remains.

The right-hand side η assembles from the same two edges, using the other two lines of edge(): eta[i] −= w·meas and eta[j] += w·meas. Trace node 9’s entry the way we traced its Λ row. Edge (8,9) contributes +wodo·z89 to eta[9] (node 9 is the j of that edge), and the loop edge (9,0) contributes −wloop·zloop to eta[9] (node 9 is the i of that edge). So eta9 = wodoz89 − wloopzloop — and because wloop = 25·wodo, the loop term dominates the sum, which is precisely why node 9 is pulled hard toward the trusted seam rather than toward its drifted odometry neighbour. Node 1, being purely interior, gets eta1 = wodoz01 − wodoz12 — a small difference of two nearly equal odometry steps, near zero, which is why interior nodes barely move except to relay the correction along the chain. The structure you assembled by hand — a stiff, loop-dominated right side at the seam and a near-zero right side in the interior — is exactly the structure that makes the ring bend to meet the closure instead of dragging the closure toward the drift.

DEBUG — consistency without accuracy

That divergence is the whole point, so it is worth seeing rather than reading. The companion widget below plots the two metrics the bench computes at three drift levels — the seam gap after closure (what the readout shows you) against the absolute trajectory error against ground truth (what the readout cannot show you). Drag the drift and watch the seam-gap bar stay pinned at the floor while the ATE bar climbs unbounded. The two bars separating is the failure made visible: a loop closure buys you the left bar, never the right.

Read the three anchor points the widget interpolates between. At gentle drift β = 0.03 the closed seam is 0.002 m and the ATE is already 0.33 m — a ratio near 165×: even here the gap under-reports the true error by more than two orders of magnitude. At the default β = 0.11 the seam holds at 0.008 m while ATE more than triples to 1.16 m. At the maximum β = 0.22 the seam is still a tidy 0.012 m but ATE reaches 2.03 m — the ring is nearly its own diameter away from the truth, yet the readout you would ship to a dashboard reads “closed, ~1 cm.” The seam metric is flat across the whole slider; the ATE metric is monotone in drift. A monitor watching only the first would report a healthy map at every drift level — the exact blind spot the DEBUG callout above names, now plotted so you can watch the trusted number lie in real time.

The 0.33 m ATE at β = 0.03 is not a headline number the widget hands you — it is the mean of ten per-node distances, and it is worth computing by hand so you see where the map is wrong even when the seam says it is fine. Solve the ring at β = 0.03, then for each node measure the straight-line distance ‖xkopt−xktruth‖ between the optimised pose and where the robot actually was. Two nodes are pinned: node 0 sits on the near-rigid anchor, and node 9 is dragged onto the seam by the stiff loop edge — both are essentially exact. Everything in between is free to bow, and the bow is worst on the far side of the ring, exactly opposite the two pinned ends:

Sum the ten distances — 3.352 m — and divide by 10: ATE = 0.335 m, the 0.33 m the DEBUG callout quotes, now unpacked. Notice the shape: 0 at the anchor, climbing to a maximum of 0.632 m at node 6 — the far wall, half a ring from both pinned ends — then falling back toward 0 at the loop-pinned seam. This is a bow, not a step: the two constraints hold the ends and the middle sags between them like a loaded beam. And here is the trap laid bare — at this same β = 0.03 the seam gap the readout shows is 0.002 m, so the ratio of true error to reported error is 0.335 / 0.002 ≈ 165×. The one node the readout can see (the seam) is the one node that is correct; the six nodes it cannot see carry more than half a metre of error apiece. That is precisely why a seam-gap dashboard is blind to a mis-gauged map.

The bow is worth one more look, because its shape is diagnostic, not just its height. The peak error 0.632 m at node 6 is 1.89× the mean — a beam pinned at both ends and loaded uniformly deflects most in the middle, and the ATE profile does exactly that, cresting between the two pins and decaying toward each. That single fact tells you where to look for the worst pose without computing anything: it is always the graph node furthest in edges from every hard constraint. Add a second loop closure — say node 4 also sees a landmark it saw earlier — and you introduce a third pin; the single long bow splits into two shorter, shallower bows, and the peak ATE roughly halves because no node is now more than a quarter-ring from a constraint. This is the design lever hiding behind the failure: the mis-gauge does not come from the loop being weak, it comes from there being too few absolute-or-verified constraints spread around the graph, so the unconstrained span between them is long enough to sag. The fix a real system reaches for is not a stiffer loop — the loop is already 25× the odometry — but more pins: additional verified place matches, a GPS fix, a known landmark, each one shortening the longest unconstrained span and flattening the bow it would otherwise carry.

FRONTIER

The pose-graph relaxation the bench runs is the classical backbone, and the reference synthesis is Grisetti, Kümmerle, Stachniss & Burgard, “A Tutorial on Graph-Based SLAM” (2010), which lays out exactly this Λ/η assembly and the intuition that a loop closure is one extra edge whose error spreads over the graph. The general-purpose solver that made it practical at scale is Kümmerle et al., “g2o: A General Framework for Graph Optimization” (2011) — the same sparse Gauss-Newton machinery, generalised over arbitrary variable and edge types, and still a default backend today. The modern move is from batch relaxation to incremental re-optimisation (iSAM2 / the Bayes tree), which re-eliminates only the cliques a new loop actually touches instead of re-solving the whole ring — the difference between the bench’s dense solve and what runs online on a robot mapping for hours.

Chapter 7: Field Guide

Your robot's map sheared a metre by the far wall and doubled the loading bay; you have thirty seconds before standup to name the cause and the metric that proves it. This chapter is that lookup — everything from this lesson compressed into tables you can scan on the job, with the load-bearing numbers worked out by hand so you can reproduce them at a whiteboard rather than recall them. Scan the tables to find the fact; drop into the worked derivations below when you need to show the fact, not just state it.

Cheat sheet — the load-bearing facts

The numbers, worked by hand

A cheat-sheet formula you can only recite is a liability at the whiteboard. Here are the load-bearing formulas from the table above, each substituted with real values and carried to a number — every intermediate line visible, so you can reproduce them cold. The first three isolate one idea each; the fourth solves an entire loop closure end-to-end so you can see them work together.

From scratch: why null(Λ) is the all-ones gauge

The cheat sheet asserts that the global gauge is unobservable. Here is the three-pose derivation that proves it — small enough to do on a napkin. Take three poses on a line, x₀, x₁, x₂, and two relative (odometry) measurements, each residual r = x_k+1 − x_k − u. The Jacobian rows are the derivatives of those residuals w.r.t. the three poses:

With unit weights, the information matrix is Λ = A^TA. Compute it entry by entry (column i of A dotted with column j):

That is the path graph Laplacian. Now multiply it by the all-ones vector 1 = [1, 1, 1]^T — each row sums to zero:

The eigenvalues are 0, 1, 3 — exactly one zero, so rank(Λ) = 2 = n − 1. The zero-eigenvalue direction is the all-ones vector: slide every pose by the same amount and no relative measurement changes, so no relative measurement (odometry or loop closure) can ever pin it down. That unobservable direction is the global position gauge; only an absolute constraint — a GPS fix, a prior, or fixing pose 0 — adds the missing rank and removes it. This is the algebraic fingerprint behind the whole "consistency ≠ accuracy" story.

System-design patterns

Coding drills

Do these from a blank editor, numpy only, no peeking. Each maps to a Code Lab in this lesson.

• TF-IDF place score. Given a word-count database and a query, return the best-matching place by idf-weighted cosine. Verify the ubiquitous word contributes zero.
• RANSAC inlier count. Given putative correspondences, fit a 2-D rigid transform from a minimal sample and count inliers under a threshold. Confirm a true loop's count dominates a random one's.
• Pose-graph loop closure. Assemble Λ and η for a drifted pose chain with one loop closure, solve, and show the end-to-start gap shrinks by an order of magnitude. The off-diagonal −w is the edge.
• Observability check. Build Λ for a relative-only chain and show matrix_rank(Λ) = n − 1; the null vector is the all-ones gauge.
• Iteration budget. Given an outlier ratio and sample size, compute the RANSAC iterations for 99% confidence from the closed form.

Drill prompts are not enough — here is the first one written out in full, the TF-IDF place-recognition kernel from a blank editor. Read it and you should be able to reproduce it; the whole thing is six load-bearing lines plus the idf that silences the ubiquitous word.

python
import numpy as np

def best_place(db, query):
    # db: (N_places, V) integer word counts; query: (V,) word counts.
    N = db.shape[0]
    df = np.count_nonzero(db, axis=0)          # places containing each word
    idf = np.log(N / np.maximum(df, 1))       # ubiquitous word (df==N) -> log(1)=0
    Q = query * idf                             # idf-weight, then
    Q = Q / (np.linalg.norm(Q) + 1e-9)          # L2-normalise the query
    best_i, best_s = -1, -1.0
    for i in range(N):
        P = db[i] * idf
        P = P / (np.linalg.norm(P) + 1e-9)
        s = float(Q @ P)                        # cosine similarity
        if s > best_s: best_i, best_s = i, s
    return best_i, best_s

# A word in EVERY place has df==N, so its idf is log(N/N)=0 and it
# drops out of both P and Q — it can never move the ranking. QED.

What to say while writing it: "The count_nonzero along axis=0 is the document frequency; the log(N/df) is what makes a corridor-wide word contribute zero. I normalise after weighting so the cosine compares directions in idf space, not raw counts — a place I merely saw more features in should not win on volume alone." State the invariant (here, idf = 0 for the ubiquitous word) and let it fall out of the arithmetic.

Here is the second drill written out the same way — the RANSAC inlier-count kernel, the geometric-verification step that turns a place-recognition candidate into a trusted loop-closure factor. Fit a 2-D rigid transform from a minimal sample of two correspondences (rotation is a single angle, translation follows from one anchor point), map all the points, and count how many agree. A true loop lands a large count; a random pairing scatters.

python
import numpy as np

def count_inliers(src, dst, thresh):
    # src, dst: (M,2) putative correspondences between two views.
    # Fit a 2-D rigid transform from a MINIMAL sample (s=2), count agreers.
    M = src.shape[0]
    i, j = np.random.choice(M, 2, replace=False)   # the minimal sample
    ds = src[j] - src[i]                        # baseline in view A
    dd = dst[j] - dst[i]                        # baseline in view B
    ang = np.arctan2(dd[1], dd[0]) - np.arctan2(ds[1], ds[0])
    c, s = np.cos(ang), np.sin(ang)
    R = np.array([[c, -s], [s, c]])              # rotation from the angle diff
    t = dst[i] - R @ src[i]                     # translation from one anchor
    pred = src @ R.T + t                        # map ALL src points
    err = np.linalg.norm(pred - dst, axis=1)     # per-point disagreement
    return int((err < thresh).sum())            # the inlier count

# Wrap in a RANSAC loop: keep the best count over k samples (k from drill 5),
# accept the loop only if best > a threshold. A true 30-deg loop returns ~20/40;
# a shuffled (false) pairing returns ~2/40 — the count is the discriminator.

What to say while writing it: "Two points fix a rigid transform because rotation is one angle and translation is one vector — that is the whole reason s = 2 here, and why the iteration budget from drill 5 stays tiny. I fit from the minimal sample and score against everyone: the geometry either explains the other matches or it doesn't, and the count is a hard, cheap yes/no that no appearance descriptor can fake." That count is precisely the “inliers > threshold” test on the cheat sheet, and its precision-first bias is why a false loop — the expensive failure — almost never survives it.

And the observability drill, written out — the code that demonstrates the null-space derivation from earlier in the panel. Build Λ for a relative-only chain, and show its rank is n − 1 and its one null direction is the all-ones gauge. This is the check you run when a map is self-consistent but drifting off an external reference:

python
import numpy as np

def observability_gauge(edges, n):
    # edges: list of (a, b) RELATIVE measurements over n poses.
    A = np.zeros((len(edges), n))
    for r, (a, b) in enumerate(edges):
        A[r, b] = 1.0; A[r, a] = -1.0       # d(residual)/d(pose)
    Lam = A.T @ A                               # information matrix
    rank = np.linalg.matrix_rank(Lam)         # -> n-1 for a relative-only chain
    w, V = np.linalg.eigh(Lam)               # eigendecomposition
    null = V[:, np.argmin(np.abs(w))]        # eigenvector of the ~0 eigenvalue
    return Lam, rank, null / null[0]         # normalise -> [1,1,...,1]

# A 4-pose relative chain: rank is 3 (= n-1) and the null vector is
# [1,1,1,1] — slide every pose equally and no measurement changes. The
# gauge is unobservable until an ABSOLUTE constraint adds the missing rank.

What to say while writing it: "The rank deficiency is not a bug in my data — it is the physics. Relative measurements see differences of poses, so a global shift lives in the null space by construction. I don’t fight it with more loop closures (they’re relative too); I add one absolute factor — a prior, a GPS fix, or pinning pose 0 — and the rank fills in." This is the coded twin of the by-hand derivation two sections up, and the reason a perfectly consistent map can still sit rotated off the truth.

Now the fourth drill — the pose-graph loop closure itself, the coded twin of worked example 4. Assemble Λ and η for a drifted pose chain with one loop-closure edge, pin the gauge, solve, and watch the end-to-start gap shrink by an order of magnitude. This is the whole back-end in twelve lines; the off-diagonal −w is the edge, and the anchor is what stops the null-space gauge from floating away.

python
import numpy as np

def close_loop(n, odom, loop, w_odom=1.0, w_loop=3.0):
    # n poses on a line; odom: list of (a,b,measurement) unit steps;
    # loop: (a, b, measurement) verified long-range constraint.
    Lam = np.zeros((n, n)); eta = np.zeros(n)
    def add(a, b, u, wt):
        Lam[a,a]+=wt; Lam[b,b]+=wt; Lam[a,b]-=wt; Lam[b,a]-=wt   # -wt off-diag = the edge
        eta[a]-=wt*u; eta[b]+=wt*u
    for a,b,u in odom: add(a, b, u, w_odom)
    add(*loop, w_loop)                                 # trust the loop 3x the odometry
    # pin the gauge: fix pose 0 = 0, drop its row/column
    L = Lam[1:, 1:]; e = eta[1:] - Lam[1:, 0]*0.0
    x = np.linalg.solve(L, e)                        # the corrected poses x1..x_{n-1}
    return np.concatenate([[0.0], x])

# 4 poses, 3 unit-step odom edges, one loop saying end-to-start is really 2.7:
# x = close_loop(4, [(0,1,1),(1,2,1),(2,3,1)], (0,3,2.7))
# Lambda over x1,x2,x3 = [[2,-1,0],[-1,2,-1],[0,-1,4]], eta = [0,0,9.1];
# solves to [0, 0.91, 1.82, 2.73] -> end-to-start gap 0.30 shrinks to 0.03.

What to say while writing it: "The add helper is the entire grammar of a pose graph — each edge deposits +w on two diagonal entries and -w on the pair that connects them, and that -w off-diagonal is literally the drawn edge. Odometry gets w = 1; the verified loop gets w = 3, so the bottom-right entry becomes 1 + 3 = 4 — the loop landing on the far pose. Then I pin pose 0 before solving, because a relative-only Λ is rank-deficient (drill 4’s twin above) and would leave the gauge floating." The seam collapsing from 0.30 to 0.03 — one order of magnitude — is the residual-spreading law of example 3 doing its work across every edge at once.

And the fifth drill — the RANSAC iteration budget, the closed form from example 1 turned into the two lines you’d actually write. Given an outlier ratio and a minimal sample size, return how many random samples you need for 99% confidence, and confirm the lever is the sample size s, not the outlier rate.

python
import numpy as np

def ransac_iters(outlier_ratio, s, P=0.99):
    # outlier_ratio: fraction of bad matches; s: minimal sample size;
    # P: probability we want at least one all-inlier sample.
    w = 1.0 - outlier_ratio                          # inlier ratio
    p_allgood = w ** s                              # chance one sample is all inliers
    if p_allgood >= 1.0: return 1                     # guard: no outliers at all
    k = np.log(1.0 - P) / np.log(1.0 - p_allgood)  # the closed form
    return int(np.ceil(k))                          # always round UP

# A 2-D rigid fit (s=2) at a 50/50 in/outlier split needs ceil(16.0) = 17.
# Push outliers to 70% (w=0.3, s=2): 49. But raise s to 3 at the same 50/50: 35 --
# doubling the minimal set roughly doubles the budget. Fit the SMALLEST model.

What to say while writing it: "This is the number an interviewer pokes at — ‘matches are 70% outliers, how many iterations?’ The closed form is k = log(1−P)/log(1−w^s), and the whole trick is that s sits in the exponent: at 50/50 the 2-point fit needs 17 and the 3-point fit needs 35, and the gap only widens as the data dirties. So I always fit the smallest model the geometry allows — two correspondences for a rigid transform (drill 2’s s = 2) — and I round up, because a fractional iteration still has to run." That is the same 17 that the calculator widget at the foot of this panel plots as its teal curve.

Drills 2 and 5 lock together into one number, so here is that join worked on real data — the RANSAC loop: run drill 5’s budget of 17 iterations, each calling drill 2’s count_inliers, keep the best count, and gate on it. Take 40 putative correspondences and a candidate 30° rigid loop:

• A true loop — 25 of the 40 matches obey one 30° rotation + translation; the other 15 are noise. Over 17 samples the best minimal fit recovers the transform and scores 25 / 40 inliers.
• A false loop — the same 40 matches shuffled, so no single transform explains more than a couple by luck. Over 17 samples the best count is 2 / 40 inliers.
• The gate. Accept only if best-inliers > 12: the true loop (25) passes, the false loop (2) is rejected. That 25-vs-2 chasm is the whole point — geometry gives a hard, cheap yes/no that no appearance descriptor can fake, and it is what keeps the expensive failure (a false loop, which folds the map) out of the back-end.

Micro-example: iSAM2 vs batch — how few variables a loop actually re-touches

The system-design table above says “iSAM2 / Bayes tree incremental re-optimisation, not batch” — but how much does incremental actually save? That is a countable number, and countable numbers beat adjectives at a whiteboard. When you add a loop-closure factor between poses i and j, a batch solver (g2o, Ceres) re-linearises and re-solves every variable in the graph. iSAM2 re-eliminates only the variables the new factor marks — those on the Bayes-tree path from i and j up to their common ancestor, plus any whose linearisation point moved past the relinearisation threshold.

Debugging scenarios — symptom → cause → the metric that reveals it

The second debug row — “loops close but everything is 30% too small” — is the one candidates most often misdiagnose, so here it is worked out. The give-away is a constant ratio, and constant ratios are worth carrying to numbers.

Classical vs modern

Recommended reading

• Cummins & Newman (2008), "FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance" — the probabilistic bag-of-words place recogniser that handles perceptual aliasing head-on.
• Gálvez-López & Tardós (2012), "Bags of Binary Words for Fast Place Recognition" (DBoW2) — the vocabulary tree + inverted index that ships in ORB-SLAM.
• Arandjelović et al. (2016), "NetVLAD" — end-to-end learned place recognition robust to appearance change.
• Mur-Artal & Tardós (2017), "ORB-SLAM2" — loop closing, relocalization, and keyframe culling in a complete real-time system.
• Kaess et al. (2012), "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree" — the incremental re-optimisation behind real-time loop closure.
• Sünderhauf & Protzel (2012), "Switchable Constraints for Robust Pose Graph SLAM" — surviving false loop closures.
• Hesch et al. (2014), "Consistency Analysis and Improvement of Vision-aided Inertial Navigation" — the observability story and First-Estimates Jacobians.
• Churchill & Newman (2013), "Experience-based Navigation for Long-term Localisation" — lifelong operation across changing appearance.

Iteration-budget calculator

The RANSAC row on the cheat sheet is the one number an interviewer loves to poke: "matches are 60% outliers — roughly how many iterations?" Answer it before you reach for the slider: w = 0.4, so w² = 0.16, log(0.01)/log(0.84) = −4.6052/−0.17435 = 26.4 → 27 iterations for the 2-point fit — still cheap. The 3-point fit at the same 60% is 69.6 → 70, already 2.6× more, and it only diverges further as the data dirties. Drag the outlier ratio and watch the budget k = log(1−P) / log(1−w^s) move. Two curves are drawn: the minimal 2-point rigid fit (s = 2, teal) versus a 3-point fit (s = 3, warm) at 99% confidence. The lesson of the whole panel is in the gap between them — the smaller model stays cheap far longer.

Where this sits in the track

This lesson closes the loop — literally — on the SLAM sub-track. It builds directly on the pose-graph machinery of SLAM Backend, uses the place-recognition foundations from Place Recognition (BoW), and completes the picture begun in Classical SLAM and Modern SLAM. The factor-graph view underlying all of it lives in SLAM: Factor Graphs.

"The map is not the territory — but a consistent map is the best territory you will ever get from a drifting robot."