The SLAM
Frontend

Chapter 1: Detect & Describe

Take a single frame from a warehouse aisle: concrete floor, steel uprights, cardboard boxes, a strip light. Now try this: circle ten places you would put a keypoint — and then say why those ten and not the ten next to them.

That question is not about Harris. It is about what a keypoint is for. And a keypoint is for exactly one thing: to be found again, in the next frame, from a different viewpoint, under different light, by an algorithm that has no idea what a shelf is.

CONCEPT — the four properties, then the test you can do by hand

Before any algorithm, state the requirements. Holding these four in order is the difference between having thought about the problem and having memorised a detector name.

A corner satisfies all four and an edge satisfies only the first two — which is the entire content of the Harris eigenvalue argument, compressed. Along an edge you can slide the window without changing what you see, so the position along the edge is unrecoverable. That is the aperture problem, and it will come back in Chapter 4 wearing a different costume.

Now the practical part. Nobody runs Harris in a 30 Hz frontend; Harris needs image derivatives, a smoothing convolution and a 2×2 determinant per pixel. What runs is FAST (Features from Accelerated Segment Test, Rosten & Drummond 2006), and it is a decision you can execute by hand.

Worked example 1 — run FAST-9 by hand

Centre pixel I_p = 100, threshold t = 20. So "brighter" means > 120 and "darker" means < 80. Here are the sixteen ring pixels, in order around the circle:

Step 1 — the high-speed pre-test. Before doing any of this, FAST checks only four pixels: p₁, p₅, p₉, p₁₃ (the compass points). For an arc of 9 to exist, at least three of those four must be all-bright or all-dark — because any 9 contiguous pixels out of 16 must contain at least three compass points. Here: p₁=64 (dark), p₅=139 (bright), p₉=144 (bright), p₁₃=133 (bright). Three bright. Pre-test passes, so we continue. Roughly 90% of pixels in a real image are discarded on those four comparisons alone, and that is where FAST's speed comes from.

Step 2 — classify all sixteen. Brighter than 120: p₄ through p₁₃. Darker than 80: p₁₄, p₁₅, p₁₆, p₁, p₂, p₃.

Step 3 — longest contiguous arcs. The bright run is p₄…p₁₃, which is 13 − 4 + 1 = 10 pixels. The dark run wraps around the end of the array: p₁₄, p₁₅, p₁₆, p₁, p₂, p₃ = 6 pixels.

Step 4 — the verdict. 10 ≥ 9, so this is a corner under FAST-9.

Step 5 — the score, for non-maximum suppression. Two adjacent pixels will both pass the test, and you want only one keypoint. Rosten's score V is the sum of absolute differences over the winning arc, less the threshold:

Adding those up: 28 + 39 + 47 + 55 + 51 + 44 + 36 + 41 + 49 + 33 = 423, so V = 423 − 20 = 403. The neighbour with the higher V survives NMS.

Step 6 — now raise the threshold to t = 40 and redo it. "Brighter" now means > 140. Only p₆=147, p₇=155, p₈=151, p₉=144 qualify contiguously (p₁₀=136 breaks it), then p₁₁=141, p₁₂=149 form a run of 2. Longest bright arc = 4. Darker than 60: only p₂=58. Longest dark arc = 1. Neither reaches 9, so at t = 40 this pixel is not a corner.

Why 9, and not 8 or 12

Here is a derivation that takes twenty seconds and impresses people, because almost nobody has done it.

Model a perfect straight edge through the centre pixel: exactly half the ring is on the bright side and half on the dark side. Sixteen pixels, so the longest bright arc is 8 and the longest dark arc is 8.

Set N = 8 and every edge pixel in the image fires. Set N = 9 and edges are rejected by exactly one pixel, while a 90° corner — where the dark wedge subtends 90° of the 360° circle, taking 16 × 90/360 = 4 ring pixels — leaves a bright arc of 16 − 4 = 12, comfortably above 9.

N = 9 is the smallest integer that rejects a straight edge. That is the whole reason. FAST-12 (the original) is stricter and faster because the four-point pre-test becomes exact, but it misses shallow corners; FAST-9 with a learned decision tree is what OpenCV ships by default.

Two things to find with those sliders. First, set the contrast to 40 and walk the threshold up: the test dies the moment t exceeds c/2, because the ring pixels sit at I_p ± c/2 and the test needs a strict margin of t. That inequality, c > 2t, is the contrast budget of your detector and it is worth knowing cold. Second, hold contrast high and open the wedge: 90° gives a bright arc of 12, 180° gives exactly 8, and 8 < 9.

CONCEPT — orientation and the descriptor

FAST gives you where. It gives you no orientation and no scale, so a raw FAST keypoint cannot survive the camera rotating. ORB (Rublee et al. 2011) bolts both on cheaply, and the orientation trick is the second thing you should be able to compute by hand.

The intensity centroid assumes the patch's brightness is not symmetric about the corner — which is true, because a corner has a bright side. Compute the first moments of the patch, take the vector from the centre to the centroid, and call its angle the keypoint's orientation.

Worked example 2 — the orientation of a 3×3 patch, and one Hamming distance

Take this patch, with the centre pixel at (0,0), x increasing right and y increasing down:

m₀₀, the total mass: 10 + 20 + 90 + 15 + 60 + 120 + 20 + 70 + 130 = 535.

m₁₀ = Σ x · I. The x = −1 column contributes −(10 + 15 + 20) = −45; the x = 0 column contributes 0; the x = +1 column contributes +(90 + 120 + 130) = +340. So m₁₀ = −45 + 340 = 295.

m₀₁ = Σ y · I. The y = −1 row contributes −(10 + 20 + 90) = −120; the y = +1 row contributes +(20 + 70 + 130) = +220. So m₀₁ = −120 + 220 = 100.

The centroid sits at (295/535, 100/535) = (0.5514, 0.1869) pixels from the centre — up and to the right, which matches the visibly bright right-hand column.

The angle is θ = atan2(100, 295) = atan(0.3390) = 18.73°. Every BRIEF sampling pair is then rotated by that angle before the comparison, which is what makes ORB rotation-invariant.

And the descriptor distance. BRIEF is 256 binary tests: "is the pixel at offset a brighter than the pixel at offset b?" Distance between two descriptors is the Hamming distance — XOR then popcount. On eight bits, to keep it hand-sized:

On 256 bits, a single 64-bit CPU can do this in four XOR + four POPCNT instructions — about 2 nanoseconds. That number is the entire reason binary descriptors survived the deep-learning era on embedded hardware, and it is the number to quote when someone proposes swapping in a float descriptor.

DESIGN — scale, distribution, and the bandwidth you just signed up for

Scale. A corner at 2 m is a blob at 10 m. ORB handles this with an 8-level pyramid at scale factor 1.2 and detects independently on each level. The feature budget must be split across levels, and the standard split is proportional to level area:

The shares are 0.6944ⁱ divided by their sum 3.096, which is the geometric series from Chapter 0. Note the consequence: the top three levels together get 113 features, and those are the only features that will still be matchable when the robot approaches a shelf and everything grows.

Distribution. This is the part textbooks skip. A raw "take the top 1000 by score" will put 800 of them on the one high-contrast object in the frame, because that object genuinely has the strongest corners. Production detectors force a spatial spread instead:

• Grid bucketing: divide the image into cells (ORB-SLAM uses roughly 30×30 px, so 1280×720 gives 42 × 24 = 1008 cells) and cap the features per cell.
• Quadtree / octree redistribution: ORB-SLAM2's DistributeOctTree repeatedly splits nodes containing more than one keypoint until the node count reaches the target, then keeps the highest-response keypoint in each node. Result: exactly N features, spread as evenly as the image content allows.
• Adaptive non-maximum suppression (ANMS): keep keypoint i only if no stronger keypoint exists within radius r_i, then choose the radius that yields N keypoints. Better spread than bucketing, more expensive.

Bandwidth. Your descriptor choice is a bandwidth and memory decision before it is an accuracy decision. At 1000 keypoints per frame and 30 Hz:

Where it sits in the system. Detection and description live on the tracking thread, before anything geometric. They consume an undistorted image — calibration comes first, because a detector run on a fisheye image produces keypoints whose neighbourhood is warped differently at the edge than at the centre, which quietly destroys descriptor repeatability across the frame. Output shapes: keypoints (N, 2) float32, octave (N,) uint8, angle (N,) float32, response (N,) float32, descriptors (N, 32) uint8.

CODE — the segment test from scratch, then the library

Write it from scratch once, narrating the pre-test to yourself as you go — the narration is where the understanding lives.

python — from scratch
import numpy as np

# Bresenham circle of radius 3, in ring order (dy, dx)
RING = [(-3, 0), (-3, 1), (-2, 2), (-1, 3), (0, 3), (1, 3), (2, 2), (3, 1),
        (3, 0), (3, -1), (2, -2), (1, -3), (0, -3), (-1, -3), (-2, -2), (-3, -1)]

def longest_run(mask):
    """longest CIRCULAR run of True in a length-16 boolean list"""
    if all(mask): return len(mask)
    best = run = 0
    for v in mask + mask:              # doubling handles the wrap-around
        run = run + 1 if v else 0
        best = max(best, min(run, len(mask)))
    return best

def fast_corners(img, t=20, N=9):
    H, W = img.shape
    img = img.astype(np.int16)
    kps = []
    for y in range(3, H - 3):
        for x in range(3, W - 3):
            Ip = img[y, x]
            # --- high-speed pre-test: compass points 0, 4, 8, 12 ---
            comp = [img[y + RING[k][0], x + RING[k][1]] for k in (0, 4, 8, 12)]
            if sum(c > Ip + t for c in comp) < 3 and \
               sum(c < Ip - t for c in comp) < 3:
                continue                      # ~90% of pixels die here
            ring = [img[y + dy, x + dx] for dy, dx in RING]
            b = [v > Ip + t for v in ring]
            d = [v < Ip - t for v in ring]
            if longest_run(b) >= N or longest_run(d) >= N:
                arc = [abs(v - Ip) for v, f in zip(ring, b if longest_run(b) >= N else d) if f]
                kps.append((x, y, sum(arc) - t))   # (x, y, score V)
    return kps

def orientation(patch):
    """ORB intensity centroid: theta = atan2(m01, m10)"""
    h, w = patch.shape
    ys, xs = np.mgrid[-(h // 2):h // 2 + 1, -(w // 2):w // 2 + 1]
    m10 = (xs * patch).sum()
    m01 = (ys * patch).sum()
    return np.arctan2(m01, m10)

Then the production form, which you should reach for immediately after showing you can write the above:

python — production
import cv2
orb = cv2.ORB_create(nfeatures=1000, scaleFactor=1.2, nlevels=8,
                     fastThreshold=20, edgeThreshold=31,
                     scoreType=cv2.ORB_HARRIS_SCORE)     # Harris re-ranks the FAST candidates
kp, des = orb.detectAndCompute(gray, None)
# kp : list of cv2.KeyPoint  (pt, angle, octave, response, size)
# des: (N, 32) uint8  — 256 bits each

The detail worth mentioning unprompted: scoreType=ORB_HARRIS_SCORE means OpenCV detects with FAST (cheap) and then ranks with Harris (accurate). You get FAST's speed and Harris's ordering. That two-stage pattern — cheap generate, expensive rank — recurs everywhere in this lesson.

DEBUG — the failure that looks perfectly healthy

Every primary metric is green. The bug is feature clustering: the aisle is featureless concrete and blank shelf backs, except for one bank of printed labels at eye height on the left. All 400 features are in a 200 × 120 pixel box on the left third of the image.

Why that produces a heading bias and not just noise. Pose is estimated by minimising reprojection error over the observed features. If all features occupy a narrow cone, rotating the camera slightly about the vertical axis moves them almost the same way as translating the camera sideways. The two motions become nearly indistinguishable — the information matrix has a small eigenvalue in the yaw–sideslip direction — and the estimator splits the difference. Consistently. In the same direction, because the geometry is the same every time.

The metric that reveals it. Not inlier ratio — that is computed among the features you have and is blind to where they are. Use keypoint spatial entropy: bin the keypoints into an 8×6 grid of cells, normalise the counts to a distribution p, and compute

Worked, so the numbers mean something:

Ship the alarm at H/H_max < 0.6. The confirming second signal is the ratio of largest to smallest eigenvalue of the pose information matrix J^TΩJ: healthy frames sit around 20–80, the clustered frames in this aisle sit above 3,000.

The fix, and the wrong fix. The wrong fix is to detect more features — you will get 2000 features in the same box. The right fix is grid bucketing with a per-cell cap plus an adaptive threshold, so that empty cells re-run FAST at t = 7 and contribute weak features that are nevertheless in the right place. Weak features in the right place beat strong features in the wrong place, and being able to say that sentence is the point of this section.

FRONTIER — learned keypoints, briefly

This is a landscape section, not a depth section — a map of who is who and why.

The defensible position: learned detectors win clearly on repeatability under viewpoint and illumination change, and that advantage is largest exactly where classical detectors fail — low texture, motion blur, day/night. They lose on determinism, bandwidth, and the fact that a failure mode you have never seen cannot be characterised. XFeat (2024) is the paper that makes the tradeoff genuinely live on a CPU-only robot, and it is the one to name.

Chapter 2: Match & Associate

Start with two numbers: 1000 and 1000. Descriptors from frame A, descriptors from frame B. How many comparisons? And of the matches you hand the estimator, how many are wrong?

The first number is easy. The second is the job.

CONCEPT — two independent questions, two independent tests

A putative match (a, b) can be wrong in two structurally different ways, and each has its own test.

They are independent because they read different slices of the distance matrix: the ratio test reads one row, mutual consistency reads a row and a column. That independence is why applying both is worth more than tightening either one.

Deriving the ratio test — it is a likelihood ratio in disguise

Why compare to the second-nearest neighbour rather than thresholding the absolute distance? Here is the argument. It takes ninety seconds and is worth having ready.

Step 1 — absolute thresholds do not transfer. Descriptor distance depends on texture richness, blur, contrast and the descriptor's own scaling. A distance of 60 bits out of 256 is an excellent match on a crisp label and a hopeless one on a blurred one. There is no single number that works across an image, let alone across a fleet.

Step 2 — the second-nearest neighbour is a free noise estimate. If a's true partner is present, the nearest neighbour d₁ is drawn from the "correct match" distribution while the second-nearest d₂ is drawn from the "unrelated descriptor" distribution. If a's true partner is absent, both d₁ and d₂ come from the unrelated distribution — so they will be close together.

Step 3 — therefore r = d₁/d₂ is a local, self-calibrating statistic. The denominator absorbs exactly the local conditions the numerator suffers from. Blur inflates both; the ratio does not move. This is the same trick as a control group, and saying it that way lands well.

Step 4 — where 0.8 comes from. Lowe measured the two distributions on 40,000 keypoints and chose the threshold that maximised useful yield. At τ = 0.8, roughly 90% of false matches are eliminated while fewer than 5% of correct matches are discarded. That asymmetry — throw away 5% of the good to remove 90% of the bad — is the entire justification, and the next widget lets you watch it move.

Worked example 1 — four candidate matches, decided by hand

Binary descriptors, Hamming distance out of 256 bits, τ = 0.8.

Rows B and D are the ones to point at. B has a large absolute distance and is accepted; D has a tiny absolute distance and is rejected. Any absolute threshold gets both of them backwards. That single observation is the derivation, compressed into a sentence you can say while drawing the table.

Three things to do with it. One: park τ at 0.8 with overlap 0.6 and confirm the numbers — recall 0.95, false-acceptance 0.10, and F1 at its maximum. Lowe's threshold is not folklore; it is the argmax. Two: drag the overlap down to 0.2, which is what a fast turn looks like, and watch precision at τ = 0.8 collapse while recall does not move at all — precision depends on the prior, recall does not. Three: at overlap 0.2, find the new F1-optimal τ. It is well below 0.8. The correct threshold is a function of your scene, and shipping one constant is a choice you should be able to defend.

CONCEPT — mutual consistency, and why it is not redundant

The ratio test reads one row of the distance matrix. It is blind to a pathology that lives in the columns: many-to-one collapse. A single descriptor in image B — a specular highlight, a low-contrast strut, a compression artefact — can be the nearest neighbour of a dozen different A descriptors, each with a comfortable ratio, because none of those A descriptors has a real partner and this one attractor happens to sit closest to all of them.

The fix costs one extra argmin along the other axis:

Twelve A descriptors all pointing at one b produce at most one surviving match, because b points back at only one of them. Eleven false matches deleted by an operation over a matrix you already computed.

Worked example 2 — the combinatorics, and why gating is not optional

Now the discrete side. Suppose you have M measurements in this frame and N landmarks in the local map. Each measurement can be assigned to any landmark or to "none of them" — it might be a new landmark, or spurious. So the number of complete association hypotheses is

Step 1 — a modest case. M = 5 measurements, N = 20 landmarks. Hypotheses = 21⁵. Building it up: 21² = 441, 21³ = 9,261, 21⁴ = 194,481, 21⁵ = 4,084,101. Four million hypotheses from five measurements.

Step 2 — a realistic case. M = 10, N = 50. 51¹⁰ = 1.19 × 10¹⁷. At one hypothesis per nanosecond that is 3.8 years per frame. Exhaustive association is not slow; it is impossible, and it becomes impossible at scales that sound small.

Step 3 — gating collapses it. Project each landmark into the image with the predicted pose and its covariance, and keep only the landmarks whose 95% Mahalanobis gate contains the measurement — the chi-square test from Uncertainty & Robust Costs. In a well-predicted frame that shrinks the candidate set from 20 landmarks to about 2, so the hypothesis count becomes 3⁵ = 243.

Step 4 — the reduction factor. 4,084,101 / 243 = 16,807×, and it came entirely from using the motion model you already had. Prediction is the cheapest form of data association. That sentence is the answer to "how does your system scale."

Step 5 — the residual problem gating does not solve. Individual gating tests each measurement against each landmark alone. A set of associations can be individually compatible and jointly absurd: measurement 1 to landmark A and measurement 2 to landmark B might each pass their own gate while the pair implies the robot is in two places at once. JCBB (Joint Compatibility Branch and Bound, Neira & Tardós 2001) searches over sets with a joint Mahalanobis distance and a chi-square threshold on the full stacked dimension. It is the classic answer to this problem, and worth knowing by name.

DESIGN — nobody brute-forces anything

Start from the cost of the naive thing, because that is what motivates every optimisation after it.

Where 2.13 comes from, because the number looks invented until you derive it. Project a map point into the image with the predicted pose, and search a circular window of radius r = 25 px. The expected number of the frame's 1000 keypoints falling in that window, if they were spread uniformly over 1280 × 720, is

So guided matching turns a million comparisons into two thousand — a 470× reduction — and it costs nothing but the motion prediction you were already computing. This is the highest-leverage single decision in the whole matching stage.

Choosing r honestly. Not by tuning. Propagate the pose covariance through the projection to get the predicted pixel standard deviation σ_u, and set r = 3σ_u. When tracking is healthy σ_u sits at 3–8 px; after a dropped frame it doubles, the window grows, the candidate count quadruples, and the ratio test starts rejecting more. That chain is worth narrating, because it shows you understand that the matcher's difficulty is set by the tracker's health — the feedback loop from Chapter 0, seen from the other end.

Bag-of-words, in one paragraph. Quantise every descriptor against a vocabulary tree (DBoW2: branching factor 10, depth 6, so 10⁶ leaves) and restrict matching to descriptors that land in the same node at some intermediate level. It buys 10–30× on unguided matching, and — more importantly — the same index answers "which past keyframe looks like this one," which is the input to relocalisation and loop closure in Lesson 8.

The asymmetry worth drawing out: the 30 Hz tracking path is guided and costs microseconds of comparison; the expensive unguided path (relocalisation, loop-closure candidates) runs only at keyframe rate, roughly 3–5 Hz. Two matchers, two budgets, two accuracy requirements. A frontend diagram with a single matcher box is the sign of one that has never run.

CODE — the matcher, from scratch and then not

python — from scratch
import numpy as np

def hamming_matrix(dA, dB):
    """dA (Na,32) uint8, dB (Nb,32) uint8  ->  (Na,Nb) int distances"""
    x = np.bitwise_xor(dA[:, None, :], dB[None, :, :])      # (Na,Nb,32)
    return np.unpackbits(x, axis=-1).sum(-1)                     # popcount

def match(dA, dB, tau=0.8, mutual=True):
    D = hamming_matrix(dA, dB).astype(np.float64)
    order = np.argsort(D, axis=1)
    j1, j2 = order[:, 0], order[:, 1]
    rows = np.arange(len(dA))
    d1, d2 = D[rows, j1], D[rows, j2]

    keep = d1 < tau * np.maximum(d2, 1e-9)                   # ratio test — ONE row
    if mutual:
        back = D.argmin(axis=0)                                # each B's best A — the COLUMN
        keep &= (back[j1] == rows)                                # symmetry
    return np.stack([rows[keep], j1[keep]], axis=1)           # (M,2) index pairs

Two things to narrate while writing it. The argsort is wasteful — you only need the two smallest, so np.argpartition(D, 2, axis=1) is the right call at scale — the kind of detail you only learn by profiling. And the mutual check is one extra argmin over a matrix you already have: near-zero cost, large precision gain.

python — production
import cv2
bf = cv2.BFMatcher(cv2.NORM_HAMMING)              # NORM_L2 for SIFT / SuperPoint
knn = bf.knnMatch(desA, desB, k=2)
good = [m for m, n in knn if m.distance < 0.8 * n.distance]

# crossCheck=True gives mutual consistency but is INCOMPATIBLE with knnMatch(k=2),
# so one OpenCV call cannot give you both. Do the ratio test with knnMatch and
# filter mutually yourself — or run knnMatch both ways and intersect.

That comment is a genuine API trap and worth knowing: cv2.BFMatcher(crossCheck=True) silently requires match(), not knnMatch(k=2). Code that claims to use both usually uses only one.

DEBUG — the aisle where matching dies

This is repetitive structure, and warehouses are built out of it: identical uprights every 1.2 m, identical bin labels, identical pallet corners. The ratio test is working exactly as designed — it sees two equally good candidates and refuses to guess. The refusal is correct. The problem is that appearance alone genuinely cannot resolve the ambiguity.

The metric that reveals it: the ratio-test rejection rate — the fraction of query features whose best match was discarded because r ≥ τ. Log it per frame; it costs nothing, since you already computed r.

The pair of numbers is the diagnosis. Feature count and rejection rate together separate three failures that all present as "match count dropped". Holding that as a table rather than a guess is the difference between a senior diagnosis and a staff one.

The wrong fix is to raise τ to 0.95 so the matches come back. Drag the widget above there: the count recovers and precision falls off a cliff, so you have converted a visible failure into an invisible one — and Chapter 0 already showed what a confident wrong constraint does to a map.

The right fix is to stop asking appearance to do geometry's job. Guided matching with a 25 px projected window means the fourth upright, 300 px away, is never a candidate at all — so the second-nearest neighbour becomes a genuinely unrelated descriptor again and the ratio test recovers its discriminating power. The ambiguity did not go away; you removed it from the question. Secondary measures: reduce τ slightly rather than raise it, because in this scene a marginal match is more likely to be wrong; and let the keyframe policy fire more aggressively so baselines stay short and the search window stays tight.

FRONTIER — the matcher stopped being a nearest-neighbour search

This is the sub-field where learning has most clearly won, and the reason is structural: matching is a joint assignment problem, and nearest-neighbour search solves it one row at a time.

What actually changed conceptually: the dustbin. SuperGlue's most-copied idea is not attention, it is that the assignment matrix carries an explicit extra row and column meaning "this point matches nothing." That is the ability to say I do not know — the same thing the ratio test and mutual consistency were hand-built to approximate — now learned end to end and enforced globally rather than per row.

The defensible position for a 30 Hz embedded frontend: keep the classical matcher on the tracking path, where guided matching has already reduced the problem to two candidates and a learned matcher has almost nothing to add. Put the learned matcher on the relocalisation and loop-closure path, which is unguided, wide-baseline and low-rate — exactly where classical matching is weakest. Same lesson as Chapter 1: put learning where the geometry is hardest and the rate is lowest.

Chapter 3: RANSAC & Geometric Verification

Four hundred putative matches. About forty percent of them are wrong. You are going to fit an essential matrix. How many RANSAC iterations do you need — and what does that number not buy you?

The first half of that question is standard. The second half is where the engineering lives.

CONCEPT — the derivation, in five lines and four assumptions

Fix the notation before you start, because a derivation is only as trustworthy as its symbols:

• w — the inlier ratio: the probability that a randomly drawn correspondence is correct.
• s — the minimal sample size: how many correspondences the model needs. 2 for a 2D line or a 2D rigid transform, 3 for P3P, 4 for a homography, 5 for a calibrated essential matrix, 7 or 8 for the fundamental matrix.
• p — the confidence you want that at least one sample was clean. Usually 0.99, sometimes 0.999.
• N — the number of iterations. The unknown.

Line 1. The probability that one drawn point is an inlier is w.

Line 2. The probability that all s points in one minimal sample are inliers is w^s — because the draws are independent.

Line 3. So the probability that a given sample is contaminated (contains at least one outlier) is 1 − w^s.

Line 4. The probability that all N samples are contaminated is (1 − w^s)^N. That is the event we want to avoid, so we require it to be at most 1 − p.

Line 5. Take logs of both sides. Because 1 − w^s < 1, its logarithm is negative, so the inequality flips:

Both logarithms are negative, so N is positive. If you cannot say why the inequality flipped, the derivation was recited.

Worked example 1 — a 2D line at 60% outliers

s = 2 (two points define a line), w = 0.4, p = 0.99.

Step 1. w^s = 0.4² = 0.4 × 0.4 = 0.16. Only sixteen samples in a hundred are clean.

Step 2. 1 − w^s = 1 − 0.16 = 0.84.

Step 3. ln(1 − p) = ln(0.01) = −4.60517.

Step 4. ln(0.84) = −0.174353.

Step 5. N = −4.60517 / −0.174353 = 26.41, so take N = 27.

Twenty-seven iterations to survive sixty percent garbage. That number surprises people, and it is the reason RANSAC is used at all.

Worked example 2 — the essential matrix at 50% outliers, 99.9% confidence

s = 5 (Nistér's five-point algorithm), w = 0.5, p = 0.999.

Step 1. w^s = 0.5⁵ = 1/32 = 0.03125. Three clean samples in a hundred.

Step 2. 1 − 0.03125 = 0.96875.

Step 3. ln(0.001) = −6.907755.

Step 4. ln(0.96875) = −0.031749.

Step 5. N = −6.907755 / −0.031749 = 217.58, so N = 218.

Drop the confidence to p = 0.99 and it becomes 4.60517/0.031749 = 145.05, so 146 — the last factor of ten in confidence costs you 50% more iterations, which is cheap. Confidence is not where the cost lives.

Where the cost lives is s and w, and the table below is the one worth memorising to one significant figure.

CONCEPT — the four assumptions, and where each one breaks

The formula is five lines; the assumptions are the engineering.

Deriving the threshold instead of tuning it

The inlier threshold t is a knob everyone tunes and almost nobody derives, which makes deriving it a cheap way to stand out.

Model the localisation noise on a correct correspondence as zero-mean Gaussian with standard deviation σ per pixel coordinate — σ ≈ 1 px for a corner detector without subpixel refinement, ≈ 0.3 px with it. Then the squared residual divided by σ² follows a chi-square distribution whose degrees of freedom equal the dimension of the residual:

Now feel the cost of getting it wrong. Suppose the true σ is 1 px and the true inlier ratio at the correct threshold is w = 0.5. Set t = 1.0 px instead of 1.96, and you keep only the residuals within one standard deviation — about 68% of the genuine inliers — so the apparent inlier ratio drops to 0.5 × 0.68 = 0.34. Feed that back into the formula with s = 5:

146 iterations became 1,011 — a 7× runtime penalty — purely from a threshold set by feel. And it is a silent penalty: the system still works, it just eats your frame budget. That is a genuinely good thing to have found on a real system.

Adaptive N — the four lines that make RANSAC practical

Assumption 2 says you do not know w. The standard resolution is to start pessimistic and update as evidence arrives:

The effect is dramatic in the good case. If the data is actually 80% inliers, the first decent sample sets w = 0.8 and N drops to 12, so the loop exits after roughly fifteen iterations rather than the 146 you would have budgeted for w = 0.5. Adaptive RANSAC is fast when the data is easy and only slow when it has to be, which is exactly the property you want inside a fixed frame budget.

Cap it anyway. An adaptive loop on genuinely hopeless data (w = 0.15, s = 5) computes N ≈ 60,000 and blows the frame. Ship max_iters and treat hitting it as a signal: log it, and let the tracker declare the frame untrusted rather than pretending.

DESIGN — two milliseconds, spent deliberately

The cost of RANSAC is iterations times the per-iteration cost, and the per-iteration cost has two parts:

For the warehouse AMR: N = 218, the Nistér 5-point solver takes about 10 µs (it builds a 10th-degree polynomial and extracts its real roots), M = 400 correspondences, and a Sampson residual is about 5 ns.

Note the split: 83% of the cost is the minimal solver, not the scoring. The instinct is to assume the opposite and optimise the scoring loop. On this workload that would recover at most 0.4 ms. The way to make RANSAC faster is to run fewer iterations, and there are three named ways to do that:

Where geometric verification sits. Immediately after matching and before any pose is trusted. Its inputs are (M, 2) correspondence indices; its outputs are an (M,) boolean inlier mask and a model. The mask is the product that matters — the model from RANSAC is a by-product you usually throw away and re-estimate from all the inliers, because a model fitted to a minimal sample is the noisiest possible estimate.

CODE — adaptive RANSAC from scratch, then the library

python — from scratch
import numpy as np, math

def ransac(data, fit, residual, s, t, p=0.99, max_iters=2000):
    """fit(sample)->model ; residual(model, data)->(n,) distances"""
    n = len(data)
    rng = np.random.default_rng(0)
    best_mask = np.zeros(n, bool)
    N, k = float('inf'), 0

    while k < N and k < max_iters:
        idx = rng.choice(n, s, replace=False)
        model = fit(data[idx])
        k += 1
        if model is None:                          # degenerate minimal sample
            continue
        mask = residual(model, data) < t
        if mask.sum() > best_mask.sum():
            best_mask = mask
            w = max(best_mask.sum() / n, 1e-6)      # the adaptive step
            denom = max(1 - w ** s, 1e-12)
            N = math.log(1 - p) / math.log(denom)

    # RANSAC returns a PARTITION. Re-fit on the whole inlier set.
    return fit(data[best_mask]), best_mask, k

Then the production call, and the flags that matter:

python — production
import cv2
E, mask = cv2.findEssentialMat(pts1, pts2, K,
                                  method=cv2.USAC_MAGSAC,   # not RANSAC; MAGSAC++ since 4.5.4
                                  prob=0.999,               # this is p
                                  threshold=1.0)            # pixels — derive it, do not guess
n_in, R, t, _ = cv2.recoverPose(E, pts1, pts2, K, mask=mask)

# The degeneracy check that recoverPose will NOT do for you:
H, maskH = cv2.findHomography(pts1, pts2, cv2.USAC_MAGSAC, 2.45)
if maskH.sum() > 0.9 * mask.sum():
    # the inliers are (near) coplanar — E is not identifiable from them
    pass

DEBUG — the failure RANSAC cannot see

Every RANSAC health metric is excellent, and that is the tell. This is assumption 3 failing: a clean sample does not guarantee a correct model when the inliers are degenerate.

The geometry. If all the matched points lie on a plane, the two views are related exactly by a homography H. Any essential matrix consistent with that homography also explains the data perfectly — and there is a one-parameter family of them. RANSAC finds one member of that family, essentially at random, and each frame it picks a different member. Hence the flipping. The inliers are real inliers; the model is unidentifiable from them.

The metric that reveals it. Fit both models to the same correspondences and compare how well each explains the data:

where S_H and S_E are the inlier scores of the homography and the essential matrix. ORB-SLAM's initialisation uses exactly this and chooses the homography when R_H > 0.45. The simpler form to remember is the ratio of inlier counts:

Why the decoys do not fit. Rolling shutter would produce a skew that scales with angular rate, not a direction flip at constant speed. A sync error would be speed-dependent. Too few features would be noisy and jumpy, not smooth with a great inlier ratio. A clean, high-confidence model that changes its mind every frame is the signature of an unidentifiable parameter, and that phrasing transfers to every estimation problem you will ever debug.

The second degeneracy worth naming while you are there: pure rotation. With zero translation, E = [t]_×R is identically zero and the five-point solver returns noise. Chapter 6 gives the parallax gate that catches it.

FRONTIER — robust estimation after RANSAC

The position to defend: RANSAC is not going away, because it is the only component in the frontend with a provable guarantee — you can state the probability that it found a clean sample. What is going away is uniform sampling and hand-set thresholds. The realistic 2026 stack is a learned matcher supplying confidences into a PROSAC-ordered, locally-optimised, MAGSAC-scored solver, with an explicit degeneracy test bolted on the end because none of the above will catch a plane for you.

Chapter 4: Direct vs Feature-Based

Your detector finds forty corners on a warehouse wall. The wall is not flat white — it has scuffs, a paint seam, a gentle brightness gradient from the strip light. So you just discarded nine hundred thousand pixels, most of which carry some information. Can you defend that?

This is the direct-versus-indirect question, and answering it well means understanding that feature extraction is a lossy compression with an assumption baked in, not a free lunch.

CONCEPT — derive the photometric residual and where it leads

Start from the physical claim. Brightness constancy says that the same surface point, seen from two poses, produces the same intensity:

Reading it right to left: take pixel x in the first image, un-project it with its inverse depth d to a 3D point, move it by the pose T, project it back into the second image, and look up the intensity there. The claim is that it equals the intensity you started with. The photometric residual is the amount by which that claim fails:

Note what is not in that equation: no descriptor, no matching, no discrete correspondence. The correspondence is implied by the pose. That is the whole idea, and it is why direct methods have no data-association problem in the sense of Chapter 2 — they have a much harder optimisation problem instead.

Now linearise, because the residual is nonlinear in T. For a small increment δ to the warp parameters:

Stack all the pixels, call the row vector J = ∇I₂ · ∂w/∂δ, and minimise the sum of squares. You get the Gauss–Newton normal equations, which are the Lucas–Kanade equations:

Worked example 1 — one Lucas–Kanade step, by hand

Reduce to one dimension so the arithmetic fits on a whiteboard. Template T sampled at two pixels; current image I; unknown shift p.

Suppose at those two pixels the image gradients are g₁ = +10 and g₂ = −6 intensity levels per pixel, and the current photometric errors (I − T at the current estimate) are e₁ = −4 and e₂ = +3 intensity levels.

Step 1 — write the linearised residual. r_i(p) = e_i + g_i p.

Step 2 — the cost. C(p) = (e₁ + g₁p)² + (e₂ + g₂p)².

Step 3 — differentiate and set to zero. dC/dp = 2g₁(e₁+g₁p) + 2g₂(e₂+g₂p) = 0, so p (g₁² + g₂²) = −(g₁e₁ + g₂e₂).

Step 4 — the Hessian. Σg² = 10² + (−6)² = 100 + 36 = 136.

Step 5 — the gradient term. Σg·e = (10)(−4) + (−6)(3) = −40 − 18 = −58.

Step 6 — the step. p = −(−58)/136 = 58/136 = 0.4265 pixels.

Apply it, re-sample the image at the new position, recompute e, repeat. Two things to notice, and to say. First, that 136 in the denominator is the information: a low-gradient region contributes a tiny number and the step becomes huge and untrustworthy, which is exactly the aperture problem. Second, the whole method rests on the linearisation being valid, which requires p to be small compared with the width of the image structure — typically 1–3 pixels at the finest pyramid level. That is the convergence basin, and it is the direct method's central weakness.

The convergence basin, in numbers you can quote

Take the warehouse AMR: f = 500 px, 30 Hz.

Rotation is the killer, and the reason is structural: translation-induced pixel motion scales as f·v/Z and gets small for distant scene points, but rotation-induced pixel motion is f·ω with no depth in it at all. Every pixel moves, near and far alike. This is why direct methods are always paired with (a) a coarse-to-fine pyramid — four levels at scale 2 multiplies the 3 px basin by 2³ = 8, giving about 24 px — and (b) an IMU prior, which predicts the rotation so accurately that the residual motion is back inside the basin. A direct VO with no IMU and no pyramid does not survive a head turn, and being able to say why in one sentence is the point.

CONCEPT — the bargain broken: brightness is not constant

Real cameras violate brightness constancy constantly, and each violation has a name:

Worked example 2 — what one auto-exposure step costs, and how two numbers fix it

The auto-exposure controller raises the gain by 20% between two frames. Nothing moved.

Step 1 — the residual. At a pixel whose true intensity is I₁ = 120, the new intensity is I₂ = 1.2 × 120 = 144, so the photometric residual is r = 144 − 120 = 24 intensity levels. A well-tracked pixel normally sits at r ≈ 3–5. This is a five to eight times inflation of every residual in the image, simultaneously.

Step 2 — what motion would explain it. Where the local gradient is 10 intensity levels per pixel, the optimizer must move by 24 / 10 = 2.4 pixels to make that residual vanish. That is the same order as the entire convergence basin at the finest level, and it appears in one frame with no warning.

Step 3 — whether it becomes a pose error. Be precise here, because the honest answer is better than the dramatic one. The apparent displacement at each pixel points along that pixel's own gradient. If gradient directions are isotropic, much of the effect cancels in the sum and you mostly get inflated residuals rather than a large bias. If the scene has a dominant gradient direction — a corridor, a horizon, a long shelf edge, which is to say most robot environments — it does not cancel and you get a bias along that direction.

Step 4 — what is guaranteed to break regardless. The residual budget. The median photometric residual jumps from about 4 to about 24; the robust kernel, tuned for the 4, down-weights nearly every measurement; the effective number of constraints collapses; and the covariance you report becomes fiction. Even in the lucky isotropic case, your uncertainty is now wrong even if your mean is not.

Step 5 — the fix, and why it is so cheap. Put the violation into the model. Replace the residual with an affine brightness version carrying two extra unknowns per frame:

With a = 1.2 and b = 0, the residual returns to exactly zero at every pixel. Two unknowns cancel a corruption of sixteen thousand measurements, which is one of the best returns on a parameter in all of robotics, and DSO adds exactly these (as e^a and b, per keyframe) for exactly this reason. Beyond that, DSO does offline photometric calibration: it measures the sensor response curve and the vignette once, and reads the exposure time out of the frame metadata, so the affine parameters only have to absorb what calibration missed.

Set the gain to 1.00 and both estimators recover the shift to three decimals: with brightness constancy holding, the two extra parameters cost nothing at all. Now drag the gain to 1.30. The plain estimator's answer walks away from the truth and keeps walking, while the affine estimator sits on the true value and simply reports a ≈ 1.30 — it absorbed the violation into the parameter that models it. The black-level slider does the same thing additively.

Then leave the gain at 1.00 and walk the true shift outward until both estimators break together. That point is the convergence basin, it is set by the spatial frequency of the image content and nothing else, and no brightness model rescues you from it. This demo profile is deliberately smooth, so its basin is wide (around 18–20 px); a real image at pyramid level 0 carries energy right down to the pixel scale and its basin is the 1–3 px quoted above. Same phenomenon, different content.

Two independent failure modes, two independent fixes: an affine model for the photometric one, a pyramid or an IMU prior for the geometric one. Conflating them is the classic way to get stuck.

DESIGN — where each method actually sits

The last row is the one most people miss. A direct method has no RANSAC, because RANSAC needs a discrete hypothesis to test. Its only defence against a moving forklift filling a third of the frame is a robust kernel, and a robust kernel — as Chapter 0 established — down-weights what is currently large, which is not the same thing as rejecting what is wrong.

The third option nobody mentions: semi-direct. SVO (Forster et al. 2014) uses a sparse direct image alignment step to get the pose to within a pixel or two, then switches to feature-based refinement and reprojection error. It gets the direct method's speed and precision for the fine alignment and the feature-based method's outlier machinery and relocalisation for everything else. Whenever the question is framed as a binary, "semi-direct" is the answer — the field settled the argument a decade ago.

Numbers for the frame budget. A sparse direct alignment over 2000 points with an 8-pixel pattern and 4 pyramid levels costs roughly 16,000 × 4 = 64,000 residual evaluations, each a bilinear sample plus a subtract, at about 15 ns = ~1 ms per Gauss–Newton iteration, and you want 5–10 of them. So 5–10 ms, against the 14 ms that detection, description and matching cost. Direct is not obviously cheaper; it moves the cost from the frontend into the optimizer.

CODE — inverse-compositional alignment from scratch

The trick worth showing is the inverse-compositional formulation (Baker & Matthews 2004): use the template's gradient rather than the current image's, so the Hessian is constant across iterations and can be inverted once. This is the difference between a research implementation and one that runs at 30 Hz.

python — from scratch
import numpy as np

def sample(img, x):
    """1-D linear interpolation, clamped at the edges"""
    x = np.clip(x, 0, len(img) - 1.001)
    i = np.floor(x).astype(int); f = x - i
    return img[i] * (1 - f) + img[i + 1] * f

def align_ic(T, I, iters=30, affine=True):
    """shift of I relative to T. affine=True adds gain a and offset b."""
    xs = np.arange(len(T), dtype=float)
    gT = np.gradient(T)                       # template gradient — computed ONCE
    p, a, b = 0.0, 1.0, 0.0
    for _ in range(iters):
        Iw = sample(I, xs + p)
        r = Iw - (a * T + b)                    # photometric residual
        if affine:
            J = np.stack([gT, -T, -np.ones_like(T)], axis=1)   # (n,3)
        else:
            J = gT[:, None]                                    # (n,1)
        Hm = J.T @ J
        d = np.linalg.solve(Hm + 1e-9 * np.eye(len(Hm)), -J.T @ r)
        p += d[0]
        if affine:
            a -= d[1]; b -= d[2]
        if abs(d[0]) < 1e-7:
            break
    return p, a, b

python — production
import cv2
# Sparse pyramidal LK — the workhorse for direct/semi-direct tracking
p1, st, err = cv2.calcOpticalFlowPyrLK(
        prev_gray, gray, p0, None,
        winSize=(21, 21),                 # the aperture — bigger = more stable, less local
        maxLevel=3,                        # 4 levels: basin ~3px -> ~24px
        criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01))
good = st.ravel() == 1              # LK reports its OWN failures — use this
# For a full direct SLAM system: DSO (github.com/JakobEngel/dso) — note that it
# requires photometric calibration files, not just K. That requirement IS the method.

DEBUG — the pose spike with a timestamp you can look up

The location clue is the whole diagnosis: those are the places where the scene's overall brightness changes fast enough that the auto-exposure controller steps the gain.

The metric that reveals it, in three parts.

1. Log the frame mean intensity per frame — one number, essentially free.
2. Log the median photometric residual per frame — you compute it anyway.
3. Correlate the pose-increment norm against |Δ mean intensity| across the run.

On a healthy run the correlation is near zero. On this failure it is strongly positive, and the spike frames sit at Δmean > 8 intensity levels with the median residual jumping from about 4 to about 20–25 — exactly the arithmetic of worked example 2.

The decoy this separates from. Every symptom above is also consistent with motion blur from a fast turn. Two things distinguish them without ambiguity: motion blur correlates with gyro rate, not with intensity change; and motion blur degrades the residual gradually over several frames, while an exposure step is one frame wide with clean frames on both sides. Plot the pose spike against gyro rate and against Δmean intensity, and whichever one lights up is the answer.

The fix ladder, in order of how much you should like it:

1. Add the affine brightness parameters (a, b) per frame. Two unknowns. This should have been there from the start.
2. Do offline photometric calibration — response curve, vignette — and read the exposure time from the frame metadata so the model has less to absorb.
3. Lock the exposure where the product allows it. Effective, and often refused by the perception team who need auto-exposure for detection.
4. Switch that segment to feature-based tracking. Descriptors are contrast-normalised and largely survive a gain step. This is the semi-direct argument again, arriving from the failure side.

FRONTIER — the frontend is being absorbed

What the trend actually is, stated so it survives the next paper: the direct/indirect distinction is being dissolved from underneath. Both are hand-designed answers to "how do I get correspondence," and learned models are becoming a third answer that is better than either on the hard cases. What is not being dissolved is the geometry behind them — DROID-SLAM's core is still a bundle adjustment, MASt3R-SLAM still does a pose graph. Learning is replacing the correspondence stage; it is not replacing the estimator. That framing is the safe, defensible answer to any "will learning replace SLAM" question, and it has stayed true for eight years.

Chapter 5: Point Cloud Registration

Now switch sensors. Forget cameras: two LiDAR scans, twenty thousand points each, taken a tenth of a second apart. There are no descriptors, no keypoints, and no correspondences at all — just two bags of coordinates. Find the transform between them.

This is the problem that separates people who have used ICP from people who have debugged it, and the fastest way to get it wrong is to say "run ICP" without saying what ICP is.

CONCEPT — the closed form, derived

Given N putative correspondences (p_i, q_i), find the rotation R and translation t minimising

This has an exact solution in closed form, which surprises people who expect to iterate. Four steps.

Step 1 — the translation decouples. Differentiate with respect to t and set to zero:

The optimal translation always maps the source centroid onto the target centroid, whatever R turns out to be. This is the single most useful fact in the derivation, and it is what makes the rest tractable.

Step 2 — substitute it back. Write a_i = p_i − p̄ and b_i = q_i − q̄ for the centred clouds. Then E = Σ ‖R a_i − b_i‖². Expanding the square:

(using R^TR = I so ‖Ra‖ = ‖a‖). The first bracket does not contain R at all, so minimising E is the same as maximising Σ b_i^T R a_i.

Step 3 — turn it into a trace. A scalar equals its own trace, and the trace is cyclic, so b^TRa = tr(b^TRa) = tr(R a b^T). Summing:

H is the cross-covariance matrix — 2×2 in the plane, 3×3 in space. It is the only thing about your point clouds that the rotation depends on.

Step 4 — maximise the trace with an SVD. Take H = UΣV^T. Then tr(RH) = tr(R U Σ V^T) = tr(V^TR U Σ). Call M = V^TRU, which is orthogonal because everything in it is. Then tr(MΣ) = Σ_k m_kkσ_k, and since every entry of an orthogonal matrix satisfies |m_kk| ≤ 1 while every σ_k ≥ 0, the maximum is reached when M = I. Therefore V^TRU = I, giving

Worked example 1 — three points, by hand, exactly

Source P = (0,0), (2,0), (1,3). Target Q is P rotated by an angle whose cosine is 0.8 and sine is 0.6 (that is 36.87°, the 3–4–5 triangle), then translated by (2, −1). Recover the transform.

Step 1 — centroids. p̄ = ((0+2+1)/3, (0+0+3)/3) = (3/3, 3/3) = (1, 1). Applying the true transform to each point gives Q = (2, −1), (3.6, 0.2), (1.0, 1.4), whose centroid is q̄ = ((2+3.6+1.0)/3, (−1+0.2+1.4)/3) = (6.6/3, 0.6/3) = (2.2, 0.2).

Step 2 — centre both clouds.

Step 3 — the shortcut that exists only in 2D. You do not need an SVD in the plane. The optimal angle is

The numerator sums the cross products, the denominator sums the dot products. Compute them term by term.

Cross products:
a₁×b₁ = (−1)(−1.4) − (−1)(−0.2) = 1.4 − 0.2 = 1.2
a₂×b₂ = (1)(−0.2) − (−1)(1.4) = −0.2 + 1.4 = 1.2
a₃×b₃ = (0)(1.6) − (2)(−1.2) = 0 + 2.4 = 2.4
Sum = 1.2 + 1.2 + 2.4 = 4.8

Dot products:
a₁·b₁ = (−1)(−0.2) + (−1)(−1.4) = 0.2 + 1.4 = 1.6
a₂·b₂ = (1)(1.4) + (−1)(−0.2) = 1.4 + 0.2 = 1.6
a₃·b₃ = (0)(−1.2) + (2)(1.6) = 0 + 3.2 = 3.2
Sum = 1.6 + 1.6 + 3.2 = 6.4

Step 4 — the angle. θ = atan2(4.8, 6.4) = arctan(0.75) = 36.87°. Exact, with no numerical method anywhere. And 4.8/6.4 = 3/4 is the 3–4–5 triangle we started from, which is a satisfying check to point out.

Step 5 — the translation. R = [[0.8, −0.6], [0.6, 0.8]], so R p̄ = (0.8−0.6, 0.6+0.8) = (0.2, 1.4), and t = q̄ − R p̄ = (2.2 − 0.2, 0.2 − 1.4) = (2, −1). Exactly the truth.

For the record, the same numbers through the SVD route: H = Σ a b^T = [[1.6, 1.2], [−3.6, 4.8]], and V U^T comes out as the same rotation with determinant +1. The 2D shortcut and the general SVD agree, as they must — but knowing the shortcut lets you do a 2D registration on a whiteboard in ninety seconds.

CONCEPT — point-to-plane, and why it converges five times faster

Point-to-point treats every correspondence as a full vector constraint. But a LiDAR point on a wall does not know where on the wall it is — the beam happened to land there. Constraining its lateral position is inventing information.

Point-to-plane (Chen & Medioni 1991) projects the residual onto the target's surface normal, so only the component perpendicular to the surface counts:

Sliding along the surface is free, which is the correct model of what a plane actually tells you.

Worked example 2 — the same wall, both cost functions

A wall along the x-axis, sampled every metre at x = 0, 1, 2, 3, 4, all with y = 0, normal n = (0, 1). The source scan is the same wall displaced by (0.70, 0.10) — so the truth is a correction of (−0.70, −0.10), and note that 0.70 is more than half the 1 m point spacing.

Step 1 — nearest-neighbour correspondences. The source point (0.70, 0.10) is at distance √(0.30² + 0.10²) = √0.10 = 0.3162 from the target (1, 0), and at √(0.70² + 0.10²) = √0.50 = 0.7071 from (0, 0). So it snaps to (1, 0) — the wrong physical point, one metre along the wall. Same for (1.70, 0.10)→(2,0), (2.70,0.10)→(3,0), (3.70,0.10)→(4,0). The last source point (4.70, 0.10) has no target beyond it, so it snaps back to (4, 0) at distance 0.7071.

Step 2 — the point-to-point residuals (target minus source):

Step 3 — the point-to-point step is the mean of those (translation only, since the geometry is not rotating):

The y correction is right. The x correction is +0.10, in the wrong direction — the truth is −0.70. The discretisation of the wall has manufactured a lateral force out of nothing, and on the next iteration the source sits at 0.80 with the same correspondences, so it keeps going the wrong way until the single edge point balances the other four. That is the classic ICP sliding artefact, and it is why point-to-point on a flat surface takes thirty to fifty iterations and lands somewhere arbitrary.

Step 4 — the point-to-plane residuals are n^T applied to the same vectors, which simply reads off the second component:

All five identical. The spurious x components, including the edge point's −0.70, are projected away exactly. The step is a pure y translation of −0.10, which is correct and complete in one iteration, and the x direction is left correctly unconstrained because a flat wall genuinely contains no information about sliding along it.

CONCEPT — NDT, the version with no correspondences at all

NDT (Normal Distributions Transform, Biber & Straßer 2003) removes the nearest-neighbour search entirely. Voxelise the target once; fit a Gaussian to the points in each voxel; then score a transform by how likely the transformed source points are under those Gaussians.

Take one voxel containing five points: (0.1, 0.2), (0.3, 0.1), (0.2, 0.4), (0.5, 0.3), (0.4, 0.5).

Mean: x: (0.1+0.3+0.2+0.5+0.4)/5 = 1.5/5 = 0.30. y: (0.2+0.1+0.4+0.3+0.5)/5 = 1.5/5 = 0.30. So μ = (0.30, 0.30).

Centred points: (−0.2,−0.1), (0,−0.2), (−0.1,0.1), (0.2,0), (0.1,0.2).

Scatter: Σx² = 0.04+0+0.01+0.04+0.01 = 0.10. Σy² = 0.01+0.04+0.01+0+0.04 = 0.10. Σxy = 0.02+0−0.01+0+0.02 = 0.03. Dividing by n−1 = 4:

Inverse: det = 0.0250×0.0250 − 0.0075² = 0.000625 − 0.00005625 = 0.00056875, so Σ⁻¹ = [[43.956, −13.187], [−13.187, 43.956]].

Now the payoff. Take two displacements of equal Euclidean length 0.1414: d = (0.1, 0.1) along the voxel's long axis, and d = (0.1, −0.1) across it.

Same distance, different score. The voxel has learned the local surface orientation from its own points and penalises movement across the surface more than movement along it — which is the point-to-plane idea, obtained for free from a covariance instead of a normal estimate. This is why NDT is anisotropic without any explicit normal computation, and it is the answer to "what does NDT buy you over ICP" that shows you have read past the abstract.

CONCEPT — correspondence rejection, the step nobody writes down

Every ICP iteration produces a correspondence for every source point, including points that have no business having one: a person walking through the scan, a pallet that moved, the parts of the scan that fell outside the previous view. Four standard rejectors, in rough order of value:

The median-scaled distance rejector deserves the emphasis, because it is adaptive. A fixed threshold like "reject beyond 1 m" is right at the start of a badly-initialised registration and hopelessly loose at convergence. Scaling by the current median tightens automatically as the alignment improves — and KISS-ICP (2023) built its whole reputation on doing exactly this well.

DESIGN — the numbers that make or break a LiDAR frontend

A 128-beam spinning LiDAR at 10 Hz produces 128 × 1800 = 230,400 points per scan, which is 2.3 million points per second. You have 100 ms per scan, and you are not getting all of it.

Now do the budget. Point-to-point at 40 iterations: 3 + 4 + 5 + 40 × 4.5 = 192 ms. That misses the 100 ms deadline by a factor of two. Point-to-plane at 8 iterations: 3 + 4 + 5 + 8 × 4.5 = 48 ms. Comfortable. The choice of cost function is a real-time decision before it is an accuracy decision, and stating it that way is the point of this section.

Three architectural decisions that follow from those numbers:

• Scan-to-map, not scan-to-scan. Registering consecutive scans compounds error every frame, exactly like frame-to-frame visual odometry. Registering the new scan against a local map (a voxel-hashed accumulation of recent scans) costs the same per iteration and drifts far less, because the map is denser and self-consistent. Keeping the tree over a rolling local map also amortises the build.
• Initialise from something else. ICP's basin is narrow; the constant-velocity model or the IMU gives you an initial guess good to a few centimetres, which puts you inside it. A registration frontend without a motion prior is a registration frontend that fails on the first fast turn.
• Deskew before you register. Skipping it is a 15 cm systematic error at walking pace, which is larger than the accuracy you are trying to achieve, and it looks exactly like a calibration problem — see The Sensor Suite for the mechanism.

CODE — ICP from scratch, then the library

python — from scratch
import numpy as np

def best_rigid(A, B):
    """closed-form R, t minimising ||R a + t - b||^2   (Arun / Umeyama / Kabsch)"""
    ca, cb = A.mean(0), B.mean(0)
    H = (A - ca).T @ (B - cb)                       # cross-covariance
    U, S, Vt = np.linalg.svd(H)
    d = np.sign(np.linalg.det(Vt.T @ U.T))         # the REFLECTION GUARD
    D = np.diag([1.0] * (len(S) - 1) + [d])
    R = Vt.T @ D @ U.T
    return R, cb - R @ ca

def icp(src, dst, T0=None, iters=40, tau=2.5):
    R = np.eye(src.shape[1]) if T0 is None else T0[0]
    t = np.zeros(src.shape[1]) if T0 is None else T0[1]
    for _ in range(iters):
        S = (R @ src.T).T + t
        d2 = ((S[:, None, :] - dst[None, :, :]) ** 2).sum(-1)   # use a KD-tree for real n
        j = d2.argmin(1)
        d = np.sqrt(d2[np.arange(len(S)), j])
        keep = d < tau * np.median(d)                # ADAPTIVE rejection
        if keep.sum() < src.shape[1] + 1:
            break
        dR, dt = best_rigid(S[keep], dst[j][keep])
        R, t = dR @ R, dR @ t + dt                   # compose, do not replace
    return R, t

The two lines to narrate: the reflection guard, and R, t = dR @ R, dR @ t + dt. That second one is a composition, not an assignment — the step you just solved is expressed in the current frame, so it multiplies on the left. Getting that wrong produces an ICP that oscillates rather than converges, and it is the single most common bug in a from-scratch implementation. Frames & Transforms owns why.

python — production
import open3d as o3d
reg = o3d.pipelines.registration.registration_icp(
        source, target,
        max_correspondence_distance=0.5,      # the rejector — the most important argument
        init=T_init,                             # from the IMU or constant velocity. NEVER identity
        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(reg.transformation, reg.fitness, reg.inlier_rmse)
# fitness = fraction of source points with a correspondence inside the threshold.
# HIGH FITNESS DOES NOT MEAN CORRECT — see the DEBUG section below.

Three experiments, and each one settles an argument.

1. Start at 35°, τ = 2.5, Run 30. The angle error goes to 0.000° and the RMS to zero, and the readout shows 124 of 132 pairs kept — the eight rejected ones are exactly the source points whose wall the boxes occluded. The rejector found them without being told they existed.
2. Same start, τ = 9 (rejection off). Those eight orphans now snap onto the blob and stay in the solve, and the final angle is 2.2° wrong with a non-zero RMS. Eight bad pairs out of 132 — six percent — cost two degrees of heading. That is the number to quote when someone asks whether correspondence rejection is worth the code.
3. Start at 110°, τ = 2.5, Run 30. It converges just as confidently, the residual falls just as smoothly, and the answer is sixty-five degrees wrong. Walk the initial angle down and find the edge: somewhere between 35° and 60° the basin gives out. A converged ICP tells you nothing about whether it converged to the right thing, which is the entire subject of the next section.

DEBUG — the corridor where ICP is confidently lost

The registration is not failing. It is succeeding at a problem that does not have a unique answer. In a corridor, translating along the corridor axis maps the wall onto itself, so the cost function is flat in that direction. Every position along the dock is an equally good optimum, and ICP returns whichever one its initialisation was nearest — which means it returns whatever the motion prior said, with no correction at all. The dead-reckoning error accumulates unopposed while every quality metric reports excellence.

Why fitness and RMSE are structurally blind to this. Both measure residuals. A degenerate direction has zero residual by definition — that is what degenerate means. You cannot detect an unobservable direction by looking at how well you fit the data; you have to look at the curvature of the cost.

The metric that reveals it. Form the registration information matrix J^TJ (6×6 in 3D, 3×3 in the plane) at the solution and take its eigen-decomposition. Zhang, Kaess & Singh's "On Degeneracy of Optimization-based State Estimation Problems" (ICRA 2016) defines the degeneracy factor as the smallest eigenvalue, and the practical readout is:

That last row is the whole diagnosis. The eigenvector does not merely tell you that something is wrong; it tells you which degree of freedom is unconstrained, in metres and radians you can read. Ship the eigen-decomposition alongside fitness and this failure becomes a one-line log message instead of a two-day investigation.

What to do about it, in ascending order of correctness:

1. Solution remapping (Zhang 2016): project the ICP update onto the well-conditioned eigenvectors only, and take the degenerate directions from the prior instead. The registration contributes what it knows and stays silent about what it does not.
2. Report an honest covariance. Set the information along the degenerate eigenvector to near zero so the backend weights it correctly — a pose graph handles a constraint that says "I know my lateral position and nothing about my longitudinal position" perfectly well, if you tell it that.
3. Fuse another sensor. Wheel odometry constrains exactly the along-corridor direction the LiDAR cannot see. This is the complementary-failure argument from The Sensor Suite, arriving with a concrete number attached.

FRONTIER — registration since 2020

The position to hold: KISS-ICP is the most useful paper on this list, because its message is that the preprocessing was always the hard part. Deskewing, adaptive downsampling and adaptive rejection contributed more than a decade of cost-function refinement. That is an unfashionable claim, it is well supported, and it maps directly onto how you should prioritise work on a real robot.

Chapter 6: Motion Estimation

You have four hundred verified correspondences. Now recover the motion. And before you write anything — there are three different ways to do that, and they need three different things. Which one are you about to use, and why?

Most people reach for the essential matrix. On a robot that already has a map, that is the wrong answer, and knowing why is worth more than knowing the five-point algorithm.

CONCEPT — three estimators, one decision

CONCEPT — P3P, from the law of cosines

P3P asks: given three known 3D points and the three image rays they project onto, where is the camera? The elegant part is that you do not solve for the pose directly. You solve for the three unknown distances from the camera centre to the points, and once you have those you have three 3D points in camera coordinates, and Chapter 5's closed form finishes the job.

Let s₁, s₂, s₃ be those distances, let θ_ij be the angle between rays i and j (known exactly from the calibrated image coordinates), and let d_ij be the distance between world points i and j (known from the map). The law of cosines on each of the three triangles gives:

Three quadratics in three unknowns. Eliminating two of them yields a quartic in the remaining one, so there are up to 4 geometrically valid solutions (the degree-8 polynomial you sometimes see also counts the mirrored ones behind the camera). This is why P3P always needs a fourth point: not to constrain the pose, but to disambiguate among four candidate poses that all explain the first three perfectly.

Worked example 1 — one P3P constraint, with numbers

Take the first equation with cosθ₁₂ = 0.9 and d₁₂ = 2.0 m.

Step 1 — what the angle actually is. θ₁₂ = arccos(0.9) = 25.84°. Two map points 2 m apart, subtending 26° at the camera.

Step 2 — write the constraint. s₁² + s₂² − 2(0.9) s₁s₂ = 4, that is s₁² + s₂² − 1.8 s₁s₂ = 4. This is a conic in the (s₁, s₂) plane — an entire curve of solutions, which is exactly why one pair is not enough.

Step 3 — pick the symmetric point on that curve to get a number you can check. Set s₁ = s₂ = s:

Step 4 — verify geometrically. Two points at 4.472 m separated by 25.84° form an isosceles triangle whose base is 2 × 4.472 × sin(25.84°/2) = 2 × 4.472 × sin(12.92°) = 2 × 4.472 × 0.22361 = 2.000 m. Which is d₁₂. The constraint is satisfied exactly, and you have just checked a P3P equation with a pocket calculator.

Step 5 — the step that completes it. That is one point on a conic. Two more equations cut it down to a quartic, giving up to four poses, and a fourth correspondence picks the right one. So inside RANSAC a P3P sample is three points plus one for disambiguation, and you score all four candidate poses against the rest of the data. That last detail is the one people miss.

Worked example 2 — one Gauss–Newton PnP step, entirely by hand

P3P gives a coarse pose from three points. Then you refine against all the inliers by minimising reprojection error. Here is one step of that, small enough for a whiteboard.

Setup. f = 500 px, principal point c_x = 320. A map point sits at (X, Y, Z) = (0.5, 0, 5.0) in the current camera frame. You observe it at u_obs = 374.0. Solve for the camera's sideways translation t_x.

Step 1 — predict. u_pred = f·X/Z + c_x = 500 × 0.5/5.0 + 320 = 50 + 320 = 370.0 px.

Step 2 — residual. r = u_obs − u_pred = 374.0 − 370.0 = 4.0 px.

Step 3 — the Jacobian. Moving the point sideways by δX changes u by f·δX/Z, so

Step 4 — the normal equation. With one measurement and one unknown, J^TJ = 100² = 10,000 and J^Tr = 100 × 4.0 = 400, so

Four centimetres. Sanity-check it the other way: 4 px at 100 px per metre is 0.04 m. The Gauss–Newton machinery, on one measurement, reduces to dividing by the gain — which tells you what the Hessian is.

Step 5 — the other Jacobians, and the number that matters. The same point gives:

Now put the last two rows together. 8.81 px of image motion could be 1° of yaw, or it could be 8.81/100 = 8.8 cm of sideways translation. From a single point at a single depth, those two hypotheses are indistinguishable. What separates them is having points at different depths: the yaw Jacobian barely changes with Z while the translation Jacobian scales as 1/Z, so a scene with depth variety breaks the ambiguity and a scene without it does not.

CONCEPT — the parallax gate, and why a stationary robot is dangerous

The second degeneracy from Chapter 3, done properly. E = [t]_×R. If the camera rotates without translating, t = 0, so [t]_× = 0 and E is identically zero. The five-point solver, handed a matrix that should be zero, returns whatever the noise says. No error, no warning, and a plausible-looking translation direction that is pure noise.

The measure of how close you are to that catastrophe is the parallax angle: the angle subtended at a scene point by the two camera centres. For baseline B and depth Z it is α ≈ arctan(B/Z). Its effect on triangulation is the number to have memorised.

Derive the depth uncertainty. Depth from a two-view pair is Z = fB/d, where d is the disparity in pixels. Differentiate:

Depth uncertainty grows with the square of depth and falls with the baseline. Now put a robot's numbers in: f = 500 px, σ_d = 1 px, a landmark at Z = 10 m.

Read the first row. Triangulating a 10 m landmark from consecutive frames gives a depth estimate with forty percent uncertainty. That landmark is not a measurement; it is a rumour. Put it in the map and it corrupts everything that later matches against it.

Read the third row. A ten-times-longer baseline gives ten-times-better depth. That is the entire argument for keyframes — not memory, not compute, but the fact that a wide baseline is the only way to make triangulation mean anything. Whenever someone asks "why keyframes?", this table is the answer.

The gate itself. ORB-SLAM refuses to triangulate a new landmark unless the parallax exceeds about 1°. From the table, 1° corresponds to B/Z = tan(1°) = 0.01746 — a baseline of at least 1.75% of the depth, which at Z = 10 m means 17.5 cm of travel. Nearby points clear the gate almost immediately; distant points may never clear it, and are correctly used to constrain rotation only. That last observation — distant points constrain rotation, near points constrain translation — is worth committing to memory.

Slide B down to 0.05 m at Z = 10 m and the readout says 40% — the same number as the table, drawn. Then hold B and slide Z out: the uncertainty grows as Z², so doubling the depth quadruples the error. Notice that the baseline at which the 1° gate opens moves with Z, and that is the point: a fixed baseline threshold in metres is wrong; the gate has to be an angle.

DESIGN — the decision tree a real frontend runs

Two numbers to carry away from that diagram. RANSAC for P3P at 60% inliers needs 19 iterations, not the 57 the essential matrix would need — a 3× saving that comes purely from the smaller minimal sample. And the refinement is 800 residual evaluations, roughly 0.3 ms, which is why the whole pose stage fits inside the 1 ms budgeted for it back in Chapter 0.

The keyframe policy, since it falls out of the parallax table. Insert a keyframe when any of: more than ~20 frames have passed; tracked map points fall below ~90% of the reference keyframe's count; or the accumulated baseline exceeds a few percent of the median tracked depth. That last criterion is the parallax gate promoted to a scheduling decision, and it is the one that adapts correctly when the robot slows or stops — a stationary robot accumulates no baseline, inserts no keyframes, triangulates nothing, and stays safe.

CODE — the PnP refinement step, then the library

python — from scratch
import numpy as np

def skew(v):
    return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])

def refine_pose(R, t, P_w, uv, K, iters=5, huber=2.45):
    """Gauss-Newton on SE(3). P_w (N,3) world points, uv (N,2) observations."""
    fx, fy = K[0, 0], K[1, 1]
    for _ in range(iters):
        Pc = (R @ P_w.T).T + t                       # into the camera frame
        X, Y, Z = Pc[:, 0], Pc[:, 1], Pc[:, 2]
        proj = np.stack([fx * X / Z + K[0, 2], fy * Y / Z + K[1, 2]], 1)
        r = (uv - proj).reshape(-1)                  # (2N,)

        # d(proj)/d(Pc) — the 2x3 projection Jacobian
        Jp = np.zeros((len(P_w), 2, 3))
        Jp[:, 0, 0] = fx / Z;  Jp[:, 0, 2] = -fx * X / Z ** 2
        Jp[:, 1, 1] = fy / Z;  Jp[:, 1, 2] = -fy * Y / Z ** 2

        # d(Pc)/d(xi) for a left-multiplied SE(3) increment: [ I | -skew(Pc) ]
        J = np.zeros((len(P_w), 2, 6))
        for i in range(len(P_w)):
            dPc = np.hstack([np.eye(3), -skew(Pc[i])])       # (3,6)
            J[i] = -Jp[i] @ dPc
        J = J.reshape(-1, 6)

        # Huber weights: full weight inside the threshold, threshold/|r| outside
        a = np.abs(r)
        w = np.where(a <= huber, 1.0, huber / np.maximum(a, 1e-9))
        Hm = J.T @ (w[:, None] * J)
        g = J.T @ (w * r)
        xi = np.linalg.solve(Hm + 1e-9 * np.eye(6), -g)         # (rho, phi)

        th = np.linalg.norm(xi[3:])                                # exponential map
        dR = np.eye(3) + skew(xi[3:]) if th < 1e-6 else \
             np.eye(3) + np.sin(th)/th * skew(xi[3:]) + \
             (1-np.cos(th))/th**2 * skew(xi[3:]) @ skew(xi[3:])
        R, t = dR @ R, dR @ t + xi[:3]
    return R, t

python — production
import cv2
ok, rvec, tvec, inl = cv2.solvePnPRansac(
        objectPoints=P_w, imagePoints=uv, cameraMatrix=K, distCoeffs=None,
        flags=cv2.SOLVEPNP_AP3P,      # algebraic P3P — the right minimal solver for RANSAC
        reprojectionError=2.45,       # sqrt(chi2 at 2 dof, 95%) with sigma = 1 px — Chapter 3
        confidence=0.999, iterationsCount=200)
# then polish on the inlier set with an iterative solver
rvec, tvec = cv2.solvePnPRefineVVS(P_w[inl.ravel()], uv[inl.ravel()], K, None, rvec, tvec)

# SOLVEPNP_SQPNP (OpenCV >= 4.6) is globally optimal and needs no initial guess;
# it superseded EPNP as the recommended non-minimal solver. Know the name.

DEBUG — the robot that is standing still and thinks it is moving

This is the pure-rotation degeneracy, and it produces the worst kind of corruption: the bad landmarks persist. The robot eventually drives away, and every future frame that matches one of those 400 m phantoms is dragged by it.

The metric that reveals it: median parallax angle across the matched correspondences. Cheap — for each match, the angle between the two bearing vectors after de-rotating by the estimated R.

Both primary metrics get better, which is the tell — and they get better for a comprehensible reason: with no translation a homography explains the data exactly, so the residuals really are smaller. The model is fitting beautifully. It is just not the model you wanted.

How this separates from Chapter 3's planar degeneracy, since both show a high H/E ratio. Parallax. A planar scene under real translation still has healthy parallax (2–3°); a pure rotation has none (< 0.1°).

Two numbers, four quadrants, and the two most common frontend degeneracies are separated. That compact diagnostic beats any anecdote.

The fix. Gate on parallax before doing anything irreversible: estimate a rotation-only model, do not estimate t, do not triangulate, do not create a keyframe. Report a rotation-only pose increment with an enormous covariance on translation, so the backend correctly learns nothing about position from that frame instead of learning something false.

FRONTIER — pose estimation, 2020 onward

The honest reading. The eight-year pattern holds: learning keeps eating the correspondence problem and keeps leaving the estimation problem alone, because geometry has properties learning does not — metric consistency by construction, a residual you can inspect, a covariance that means something, and degradation you can predict. Reloc3r and MASt3R-SLAM are the first systems to attack the estimation side seriously, and the right position for 2026 is: run them shadow-mode to mine hard cases and to bootstrap relocalisation, and keep the geometric estimator in the loop that moves the robot. When someone pushes back, name the switching condition: "I move pose estimation onto a learned model when it produces a calibrated uncertainty I can validate against ground truth on my own domain, and not before."

Chapter 7: The Frontend Bench

Six chapters, six pieces. This chapter wires them into one machine and hands you the controls.

The bench runs a complete frontend on a synthetic scene: a matcher produces putative correspondences, a ratio test filters them, RANSAC verifies the survivors geometrically, and a two-point rigid solver estimates the motion. You drive three knobs — how hard the scene is, where you set the ratio threshold, and how many RANSAC iterations you can afford — and the machine reports both what it did and what the theory said it should do.

How the three knobs are wired together

The controls are not independent, and that coupling is the whole lesson. Here is the chain, in the order the data flows.

Five experiments, and what each one is teaching

1. Verify the derivation. The defaults: ρ = 0.45, τ = 0.80, N = 40. The ratio test keeps 168 of the 300 putative matches at an inlier ratio of w = 0.851; the formula demands four iterations for 99% confidence, and the dots ride the curve (theory 1.000, measured 0.970). That is Chapter 3's five-line derivation surviving contact with an actual implementation, and it is why you can quote N from a formula instead of tuning it.
2. Turn the ratio test off and starve. Push ρ to 0.85 and τ to 1.00 — a matcher that accepts its nearest neighbour unconditionally. All 300 matches survive, w collapses to 0.140, and the readout says the formula demands 233 iterations while your budget is 40: 5.8× too small. Theory puts success at 0.547 and measurement at 0.490, so roughly half your frames are wrong and nothing in the system says so. Now push ρ to 0.90 and it stops being subtle: w = 0.097, the required budget is 491 iterations — 12.3× too small — and the reported rotation is off by 0.26° on a frame where it happened to be lucky. This is where a real frontend should log "hit max_iters" and mark the pose untrusted rather than hand the backend a confident lie.
3. Buy w back with τ. Return to ρ = 0.85 and drag τ down to 0.85. The kept count falls from 300 to 98, w jumps from 0.140 to 0.408, and the required iterations fall from 233 to 26 — comfortably inside the budget you already had. A ninefold reduction in required work, from moving one threshold. This is the most useful reflex in the lesson: when RANSAC is too slow, the fix is almost never a faster RANSAC. It is a better filter in front of it.
4. Overtighten, and meet the opposite failure. Set ρ = 0.90 and walk τ from 0.80 down to 0.40, watching the rotation error rather than the success curve:

   τ	matches kept	w	rotation error
   0.80	64	0.406	0.209°
   0.60	23	0.913	0.164° — the minimum
   0.50	16	0.938	0.218°
   0.40	13	1.000	0.316° — the worst

   At τ = 0.40 the inlier ratio is perfect and the estimate is the worst on the sweep. RANSAC has nothing left to reject; the estimator is now limited by how few constraints it is averaging over, and error falls roughly as one over the square root of the count. Two failure modes, opposite directions, one knob — and the optimum between them, around 0.6 here, is a property of your scene rather than of the algorithm. That is why "we use 0.8 because Lowe said so" is a weak answer.
5. Find where the theory is optimistic. Look at the gaps: 0.547 against 0.490 in experiment 2, and at ρ = 0.90 with τ = 0.40 a theory of 1.000 against a measurement of 0.830. The dots sit consistently below the curve, and that is assumption 3 from Chapter 3 — a sample free of outliers is not guaranteed to yield a correct model. With only thirteen surviving matches, two inliers that happen to lie close together define the rotation badly, so the sample is clean and the model is still wrong. The formula is an upper bound on success, not a prediction of it, and knowing which direction the bound errs in is the sort of detail that matters on a real system. It is also why production RANSAC re-fits on the consensus set rather than shipping the minimal-sample model, which this bench does too.

Reading the bench the way you would read a bag file

The five experiments above are the bench driving you. This section is you driving the bench — the diagnostic order to read the six numbers in, which is the same order you would read a real frontend's telemetry.

Two things this bench teaches about real systems

How to design the experiment. Sweep the matcher threshold against scene difficulty on recorded bags, and plot the inlier ratio and the required RANSAC budget as surfaces over that grid. The threshold is not a constant — it is the argmax of a curve whose shape depends on how much of the previous frame is still visible, and the honest artefact is the surface, not a number. That converts a tuning question into an engineering one.

What to ship. Log w, the required N, and the achieved N on every frame. Three floats, essentially free, and together they are a complete statement of whether verification succeeded on evidence rather than on hope. Then alarm on the ratio of required to achieved, because that is the one that fires before the map folds rather than after. Notice what that does: it takes the derivation from Chapter 3 and turns it into production telemetry. A derivation you can log is worth more than a derivation you can recite.

What the bench simplifies, stated honestly

Every simulation leaves things out, and knowing exactly what is worth more than the simulation itself.

That last row deserves the arithmetic, because it is the sort of number that ends an argument. At 30 frames per second, a 99% per-frame success rate means 0.01 × 30 × 60 = 18 failed frames every minute. A 99.9% rate still means 1.8 per minute. This is why a production frontend does not stop at RANSAC: it adds a degeneracy test, a parallax gate, a consistency check across consecutive frames, and a keyframe policy that refuses to commit anything it is unsure about. Verification is not one step. It is a ladder, and each rung buys a decimal place.

Chapter 8: The Field Guide

Everything above, compressed into the form you reach for in the field. Read section 7 first if you have twenty minutes; read section 1 first if you have five.

1 · The cheat sheet

2 · System-design patterns

Prompt A — "Design the perception frontend for a warehouse AMR: one mono camera at 1280×720 and 30 Hz, a 16-beam LiDAR at 10 Hz, wheel odometry, on a Jetson Orin Nano."

Framework: open with the budget, not the blocks. 33.3 ms per camera frame; state that you will spend 18 ms and keep 15 ms of margin, then justify each line. Then draw the pipeline: undistort → detect with bucketing → describe → guided match using the predicted pose → ratio test → mutual consistency → RANSAC + degeneracy test → PnP against the local map → keyframe decision gated on parallax. Name the two paths explicitly — the 30 Hz guided tracking path and the 3–5 Hz unguided relocalisation path — because a single matcher box is the tell of a design that has never run. Close on the LiDAR: deskew first (15 cm at walking pace), scan-to-map not scan-to-scan, initialised from wheel odometry, and log the smallest eigenvalue of J^TJ because the loading dock is a corridor.

Prompt B — "Your frontend has a 15 ms budget and it is taking 40 ms. Where do you look?"

Framework: profile before theorising, then attack in this order. (1) Detection dominates — 2.85 Mpx across the pyramid at ~3 ns each is 8.6 ms. Cut pyramid levels or resolution before anything clever. (2) Matching is unguided — if you are brute-forcing 1000×1000 you are burning 2.7 ms for nothing; a projected 25 px window makes it 2,130 comparisons. (3) RANSAC is starved of w — 83% of RANSAC's cost is the minimal solver, so the lever is fewer iterations, and fewer iterations come from a higher w, and a higher w comes from the ratio test and PROSAC ordering. State the number: at w = 0.7 versus 0.4, the essential matrix needs 26 versus 448 iterations. (4) Only then consider descriptors: float32 SuperPoint is 1 MB per frame, larger than the image.

Prompt C — "Would you replace ORB with SuperPoint plus LightGlue on this robot?"

Framework: split perception from estimation, then cost it. Learned detection genuinely wins on repeatability under blur and lighting change. It costs a GPU that the planner also wants, 32× the descriptor bandwidth, and a validation burden across a 400-robot fleet whose cameras are not identical. Answer: not on the tracking path, where guided matching has already reduced the problem to two candidates and there is almost nothing to win — yes on the relocalisation and loop-closure path, which is unguided, wide-baseline, low-rate and exactly where classical matching is weakest. Then name the switching condition: "I move it onto the tracking path when it fits the power budget with margin and I can characterise its failure on my own domain." Mention XFeat (CVPR 2024) as the CPU-only option that could change the calculation.

Prompt D — "A colleague wants to drop RANSAC because the backend uses a Huber kernel."

Framework: this is Chapter 0, and it is the highest-value objection in the lesson. A robust kernel down-weights residuals that are large under the current estimate. A false constraint with a high information weight moves the estimate until its own residual is small — run the arithmetic: a loop closure at Ω = 100 against a 54-link odometry chain of effective stiffness 0.0185 absorbs 99.98% of its own error, leaving a residual of 0.6 mm on a 3 m lie, eighty times below any sensible Huber threshold. Robust backends handle wrong values; they do not handle wrong correspondences, because a wrong correspondence rewrites what "large" means. Geometric verification is the only stage that makes a discrete decision, and discrete errors are the unbounded ones.

3 · Coding drills

Drill 1 — "Implement RANSAC with an adaptive iteration count." The key lines:

the 10 lines that matterN, k = float('inf'), 0
while k < N and k < max_iters:
    idx   = rng.choice(n, s, replace=False)
    model = fit(data[idx]); k += 1
    if model is None: continue
    mask = residual(model, data) < t
    if mask.sum() > best.sum():
        best = mask
        w = max(best.sum()/n, 1e-6)
        N = math.log(1-p) / math.log(max(1-w**s, 1e-12))
return fit(data[best]), best        # RE-FIT on the consensus set

What matters while writing it: "N starts at infinity because I do not know w — that is assumption two of the derivation, and the adaptive update is how you discharge it." Then: "I am capping iterations, and hitting the cap is a signal I want logged, not swallowed." Then, on the last line: "RANSAC returns a partition, not a model; the minimal-sample model is the noisiest estimate available, so I re-fit."

Drill 2 — "Implement the Lowe ratio test and mutual consistency."

the 6 lines that matterorder = np.argpartition(D, 2, axis=1)[:, :2]     # not argsort — you need two
j1, j2 = order[:, 0], order[:, 1]
rows   = np.arange(len(D))
keep   = D[rows, j1] < 0.8 * D[rows, j2]         # the ROW test
back   = D.argmin(axis=0)                          # the COLUMN test
keep  &= (back[j1] == rows)

What matters: "argpartition rather than argsort, because I only need the two smallest." And: "The ratio test reads one row, mutual consistency reads a column — that is why they are not redundant. Mutual catches many-to-one collapse onto an attractor descriptor, which no row-wise test can see."

Drill 3 — "Implement one ICP iteration."

the 8 lines that matterS    = (R @ src.T).T + t
d2   = ((S[:, None] - dst[None]) ** 2).sum(-1)      # KD-tree in production
j    = d2.argmin(1); d = np.sqrt(d2[np.arange(len(S)), j])
keep = d < 2.5 * np.median(d)                       # ADAPTIVE rejection
A, B = S[keep], dst[j][keep]
H    = (A - A.mean(0)).T @ (B - B.mean(0))
U, _, Vt = np.linalg.svd(H)
D    = np.diag([1, 1, np.sign(np.linalg.det(Vt.T @ U.T))])   # reflection guard
dR   = Vt.T @ D @ U.T;  R, t = dR @ R, dR @ t + (B.mean(0) - dR @ A.mean(0))

What matters: "The determinant guard, because V U^T is only guaranteed orthogonal and orthogonal includes reflections — collinear corridor scans hit that constantly." And: "The last line composes rather than assigns; the increment is expressed in the current frame, so it multiplies on the left. Getting that backwards makes ICP oscillate instead of converge."

Drill 4 — "Write the Gauss–Newton reprojection step."

the 6 lines that matterPc   = (R @ Pw.T).T + t
proj = np.stack([fx*Pc[:,0]/Pc[:,2] + cx, fy*Pc[:,1]/Pc[:,2] + cy], 1)
r    = (uv - proj).reshape(-1)
J    = -Jproj @ np.hstack([np.eye(3), -skew(Pc)])       # chain rule onto se(3)
xi   = np.linalg.solve(J.T @ (w[:,None]*J), -J.T @ (w*r))
R, t = exp_so3(xi[3:]) @ R, exp_so3(xi[3:]) @ t + xi[:3]

What matters: "∂u/∂t_x is f/Z and ∂u/∂t_z is −fX/Z², so forward motion is ten times weaker than sideways at a typical geometry — that asymmetry is why forward-facing cameras have poor depth-of-travel observability." And: "w is a Huber weight, and one Huber-weighted Gauss–Newton pass is IRLS."

4 · Debugging scenarios

5 · Classical vs modern — and when to use which

6 · Recommended reading

The one book. Multiple View Geometry in Computer Vision, Hartley & Zisserman, 2nd edition. Chapters 4 (estimation and RANSAC), 9–11 (epipolar geometry and the fundamental matrix) and Appendix 6 (iterative estimation) cover most of this lesson's geometry with more rigour than any paper. If you want one robotics book instead, Introduction to Visual SLAM by Gao and Zhang is the practical companion, with working C++ for every chapter here.

Five papers, and why each.

1. Lowe, "Distinctive Image Features from Scale-Invariant Keypoints" (IJCV 2004). Read section 7.1 for the ratio-test justification. It is the clearest example in the field of choosing a statistic by measuring two distributions rather than by argument.
2. Mur-Artal, Montiel & Tardós, "ORB-SLAM" (T-RO 2015). The reference frontend. Read for the automatic initialisation (the H-versus-E model selection), the covisibility graph, and the survival-of-the-fittest keyframe policy — each is a design decision with a stated reason.
3. Barath et al., "MAGSAC++" (CVPR 2020). The modern answer to "how do I pick the inlier threshold" — do not. Also the gateway to the RANSAC-benchmark literature, whose honest conclusion is that the combination beats any single idea.
4. Vizzo et al., "KISS-ICP" (RA-L 2023). The most useful paper on this list, because its message is unfashionable and well supported: deskewing, adaptive downsampling and an adaptive rejection threshold matter more than a decade of cost-function refinement.
5. Lindenberger, Sarlin & Pollefeys, "LightGlue" (ICCV 2023). The state of learned matching, and the clearest exposition of adaptive computation — exit early on easy pairs, prune points you are confident cannot match. Read it alongside SuperGlue (CVPR 2020) for the dustbin idea.

Five repositories, and what to look at in each.

1. UZ-SLAMLab/ORB_SLAM3 — open src/ORBextractor.cc and read DistributeOctTree. It is the spatial-distribution machinery this lesson's Chapter 1 debug section is about, and almost no textbook describes it.
2. opencv/opencv — modules/calib3d/src/usac/. The PROSAC sampler, the local optimisation and the MAGSAC scoring, all in readable C++ with the papers cited inline.
3. PRBonn/kiss-icp — roughly a thousand lines total. Read KissICP.cpp and count how much of it is preprocessing versus registration. That ratio is the paper's argument.
4. cvg/LightGlue — lightglue/lightglue.py, specifically the confidence head and the early-exit logic. It is the cleanest available implementation of "know when to stop."
5. JakobEngel/dso — read the photometric calibration documentation before the code. The fact that a direct method needs a response curve and a vignette map, not just K, is the method's central tradeoff made concrete.

7 · The numbers drill, and the practice protocol

Answer each to one significant figure, out loud, without notes. If any takes more than ten seconds, that is the one to practise.

The practice protocol, spread over a week, for making this material permanent:

1. Day 1 — derivations, on paper, twice each. The RANSAC iteration count, the ICP closed form including the determinant guard, and the photometric linearisation. Twice, because the first time you are recalling and the second time you are rebuilding.
2. Day 2 — the numbers table above, out loud, cold. Anything over ten seconds goes on a card.
3. Day 3 — draw the frontend from memory and say aloud what breaks if each block is deleted. Then draw it again with the millisecond budget written on each block.
4. Day 4 — the failure taxonomy. For each of the seven scenarios in section 4, state symptom, metric and distinguishing value without looking. These are what separate senior from staff, and they are the part everyone skips.
5. Day 5 — run the Studio cold (the button at the top of this page): it is a timed practice session on exactly this material, and its debrief will tell you which chapter to revisit.
6. Day 6 — the tradeoff. Practise stating one in three parts, in under thirty seconds: the assumption it makes, the failure it accepts, and the observation that would change your mind.
7. Day 7 — rest. Re-reading on the last day measures your short-term memory, not your understanding.

"The first principle is that you must not fool yourself —
and you are the easiest person to fool."
— Richard Feynman

Every failure in this lesson was a system fooling itself with a healthy-looking metric.
The whole job is knowing which second number to look at.