Downstream Perception
Maps, detection & the handoff to planning

Chapter 1: Occupancy Grids & Costmaps

Chop the floor into a grid of little squares — say 5 cm on a side. Give each square one number: the probability that it is occupied. That is the whole idea of an occupancy grid: a raster of the world where every cell holds P(cell is occupied), and free space, walls and the unexplored beyond are all just different values of that one number.

The genius is not the grid — it is the update. Every scan gives you weak, noisy evidence about many cells at once. We need a rule that fuses one more scan into the current belief without ever revisiting the old scans. That rule is a recursive Bayes filter, and when you write it in the right variable it collapses to something you can do with a running sum.

Derive the update: Bayes, then a change of variable

Fix one cell. Let m be the binary event “this cell is occupied”, and let z_1:t be all the measurements so far. We want the posterior p(m | z_1:t). Apply Bayes’ rule to the newest measurement z_t, and assume measurements are conditionally independent given the map (the standard occupancy assumption):

Here p(z_t | m) is the forward sensor model and p(m | z_1:t−1) is the belief we already carried. The trouble is the denominator: it needs a marginal over m, so this does not chain cleanly. The trick is to write the same equation for the complementary event ¬m (“cell is free”) and divide the two. The nasty normaliser p(z_t | z_1:t−1) is identical in both and cancels:

That ratio p / (1−p) is the odds of occupancy. So the equation says: new odds = (a likelihood ratio from this measurement) × (old odds). It chains — but it is still a product, and products are awkward. Take the logarithm of both sides. The product becomes a sum, and we define the log-odds:

The update is now a single addition. The measurement contributes a fixed increment — the inverse-sensor-model log-odds, written ℓ_occ when the beam says “hit here” and ℓ_free when the beam passes through. In practice we do not model p(z|m) explicitly; we just pick two increments directly, which is the inverse sensor model.

A hand-worked example: watch a cell converge

Pick the two increments. A common lidar choice: a beam that reports the cell occupied carries p_hit = 0.7, and a beam passing through reports p_miss = 0.4. Convert each to log-odds with natural logs:

Start a cell at the prior p = 0.5, so ℓ₀ = log(1) = 0 (a cell we know nothing about). Now hit it three scans in a row. Each scan adds ℓ_occ = 0.847:

Three consistent hits and the cell is 93% occupied. Notice the belief climbs but with diminishing steps — that is the log-odds saturation doing its job. Now suppose the same cell, having reached 5 hits (ℓ = 4.237, p = 0.986), gets two spurious misses because a person walked through the beam. Each miss subtracts 0.405:

The cell barely flinches. Five real hits built a wall of belief that two stray misses cannot knock down — and that robustness came for free from summing evidence. This is the entire reason occupancy grids work in a world full of transient noise.

DESIGN: real numbers for a real grid

Occupancy is not a toy. Here is what a warehouse robot actually carries. A local costmap of 10 m × 10 m at 5 cm resolution is a 200 × 200 grid — 40,000 cells. Store ℓ as a single byte (a fixed-point log-odds), and that is 40 KB, small enough to live in L2 cache and update in well under a millisecond.

In the ROS nav2 stack this object is costmap_2d, and it is built in layers: a static layer (the pre-built map), an obstacle layer (live lidar via exactly the log-odds update above), and an inflation layer.

From occupancy to cost: the inflation layer

A planner cannot treat the robot as a point; it has width. If you route a 60 cm robot through a 55 cm gap, the map says “free” and the robot scrapes the wall. The fix is to inflate obstacles: every occupied cell radiates a decaying cost outward to a radius, so the cost of a cell is high next to a wall and fades to zero far from it. A common form is an exponential falloff beyond the robot’s inscribed radius r_i:

where d is the distance from the nearest obstacle and α controls how fast the cost decays. Inside r_i the cost is lethal (the robot centre cannot be there). The planner then just plans through low-cost cells, and staying in low cost automatically keeps clearance — the geometry of the robot is baked into the map, so the planner can stay a point.

CODE: the update from scratch, then the library form

The from-scratch version is startlingly short, because the derivation did all the work. Every cell along a beam is free until the endpoint; the endpoint is occupied.

python # from-scratch: integrate one range beam into a log-odds grid
import numpy as np

L_OCC  = np.log(0.7 / 0.3)   # +0.847  a hit
L_FREE = np.log(0.4 / 0.6)   # -0.405  a pass-through
L_MIN, L_MAX = -8.0, 8.0       # clamp: keep beliefs reversible

def integrate_beam(grid, cells_free, hit_cell):
    for (cx, cy) in cells_free:          # cells the ray passed through
        grid[cy, cx] = np.clip(grid[cy, cx] + L_FREE, L_MIN, L_MAX)
    if hit_cell is not None:            # the endpoint the beam struck
        hx, hy = hit_cell
        grid[hy, hx] = np.clip(grid[hy, hx] + L_OCC, L_MIN, L_MAX)

def to_prob(grid):
    return 1 - 1 / (1 + np.exp(grid))  # log-odds -> probability

The library form (ROS nav2) hides the loop but is doing exactly this: the obstacle layer raytraces each beam, marks free cells, marks the hit, and clamps — the same three lines, in C++, over a rolling window.

yaml # nav2 costmap_2d obstacle layer config (the log-odds is inside)
obstacle_layer:
  plugin: "nav2_costmap_2d::ObstacleLayer"
  observation_sources: scan
  scan:
    topic: /scan
    max_obstacle_height: 2.0
    clearing: true          # raytrace free space (the L_FREE pass)
    marking: true           # mark the hit cell (the L_OCC pass)
    obstacle_max_range: 2.5

DEBUG: the map fills with phantom walls

Symptom. The robot maps a clean hallway, but a diagonal streak of “occupied” cells appears in the middle of open floor, and the planner refuses to cross it. Nothing is physically there.

Cause. The classic culprit is a missing clearing pass combined with a moving object: a person walked through, the lidar hit them, cells got the ℓ_occ increment — and then the person left, but nothing ever subtracts it back. Without ray-clearing (marking every cell along the beam as free), occupancy is monotone: it only ever goes up. Transient hits become permanent phantom walls.

The metric that reveals it. Log the ratio of marked cells to cleared cells per scan. A healthy scan clears far more than it marks (a 10 m beam clears ~200 cells and marks 1). If your clear:mark ratio is near 1, clearing is broken, and phantoms are inevitable. The fix is to ensure every beam runs the free pass (the clearing: true flag above) — and to clamp ℓ so even a legitimate wall can be un-learned when the beam later passes through where it used to be.

FRONTIER: the grid is old, and still winning

The log-odds occupancy grid dates to Elfes and Moravec’s work in the mid-1980s (Elfes, “Occupancy Grids: A Probabilistic Framework for Robot Perception and Navigation”, 1989). It is one of the oldest ideas in robotics still in daily production use, which is remarkable. But the frontier is alive: OctoMap (Hornung et al., Autonomous Robots, 2013) stores the same log-odds values in an octree, collapsing large uniform regions into single nodes so a whole-building 3-D map fits in megabytes. And the current wave replaces the fixed inverse-sensor model with a learned one: networks that predict occupancy (and even completion of unseen space) from raw sensor data — e.g. occupancy-prediction heads in autonomous-driving stacks (the “occupancy networks” line, Tesla’s occupancy grid at CVPR 2022; and academic work such as OccNet, 2023). The representation survives; the model that feeds it keeps getting smarter.

Chapter 2: Voxels & the TSDF

A ground robot can live on a flat grid. A drone cannot — it needs to know there is a beam over its head and clear air below. A manipulator reaching into a bin needs the exact surface of the parts, not just “there is stuff here.” So we go up a dimension.

The obvious move is to make the grid 3-D: a voxel grid, each cell a little cube holding a log-odds occupancy. The update from Chapter 1 carries over unchanged — a beam clears the cubes it passes through and marks the cube it hits. And it works. But it has a flaw that gets worse as you demand more precision.

Why a hard voxel grid is not enough

Occupancy is binary in spirit: a cube is occupied or free. But a surface is not aligned to your voxel lattice. When a wall runs diagonally through a 5 cm cube, the cube is “half occupied,” and the grid rounds it to fully occupied — so a smooth wall becomes a jagged staircase of cubes, accurate only to the voxel size. To halve the error you must halve the voxel, which octuples the memory. Precision and memory fight, and precision loses.

Deriving the TSDF from a depth measurement

A TSDF — Truncated Signed Distance Function — stores in each voxel a value D(v):

• Signed: D > 0 in front of the surface (empty space the sensor saw through), D < 0 behind it (occluded space), and D = 0 on the surface.
• Distance: the magnitude is the distance to the nearest surface along the viewing ray.
• Truncated: we only care about voxels near a surface, so we clamp D to ±μ (the truncation distance) and ignore anything farther. That is what keeps it cheap: a TSDF stores a thin shell around surfaces, not the whole volume.

Here is how one depth pixel updates it. The camera sits at a known pose and a pixel measures depth d along a ray. For a voxel v at range r from the camera along that same ray, the signed distance to the surface the pixel found is simply:

Read it: if the voxel is closer than the measured depth (r < d), it is in front of the surface, sdf > 0, free space. If it is farther (r > d), it is behind the surface, sdf < 0, occluded. If r = d, the voxel is exactly on the surface, sdf = 0. Then truncate and normalise to [−1, 1]:

Fusion: many noisy depths, one clean surface

One depth image gives one noisy estimate of D at each voxel. The magic is the fusion rule — a running weighted average (Curless & Levoy, 1996):

where D, W are the stored value and weight and d, w are the new observation and its weight. This is just an incremental mean, and its power is worth stating plainly: the zero-crossing of the averaged field lands on the true surface even when no single depth image put it there. Two cameras that each mis-measure the depth — one short, one long — average out to the truth.

A hand-worked example: fusion finds the surface

Work in 1-D so the arithmetic is visible. A true surface sits at x = 5.0 m. Truncation μ = 4.0. Camera A measures depth d_A = 4.5 (0.5 m short); camera B measures d_B = 5.5 (0.5 m long). Both look along +x from the origin, so a voxel at x has range r = x. Compute D for a few voxels near the surface, equal weights so fusion is a plain average:

Camera A alone crosses zero at x = 4.5 (its wrong depth). Camera B alone crosses at x = 5.5. But the fused field crosses zero at exactly x = 5.0 — the truth — and it did so with a single average, no iteration. Extract the surface by finding where the field changes sign and interpolating: between two voxels with fused values +0.25 and −0.25 the crossing is exactly halfway. That linear interpolation is what buys the sub-voxel precision a hard grid could never reach.

DESIGN: what a TSDF costs, and where it runs

The truncation is the whole budget story. A dense TSDF of a 4 m × 4 m × 3 m room at 2 cm voxels is 200 × 200 × 150 = 6 million voxels — but only a thin shell of ±μ around actual surfaces ever holds a non-trivial value. So real systems store it sparsely, in voxel hash tables or octrees, and only the ~1–5% of voxels near a surface exist.

To turn the field into a triangle mesh you run Marching Cubes (Lorensen & Cline, 1987): march through the voxels, and wherever the eight corners of a cube straddle zero, emit triangles interpolated to the crossings. The mesh is the surface at sub-voxel resolution, ready for rendering or collision-checking.

DEBUG: the reconstructed wall is doubled or blurred

Symptom. You scan a flat wall and the mesh comes out as two parallel surfaces a few centimetres apart, or as a thick fuzzy slab instead of a clean plane.

Cause. Almost always a pose error, not a TSDF error. If the camera-to-world pose drifts by a few centimetres between two frames, the two depth images write their surfaces at slightly different world positions, and the fused zero-crossing sits between them — a ghost. The TSDF faithfully averaged two inconsistent inputs. Garbage poses in, blurred surface out. (A too-small truncation μ can also cause it: if μ is smaller than the pose jitter, the two shells do not overlap and you literally get two surfaces.)

The metric that reveals it. Track the average voxel weight at the zero-crossing versus the gradient magnitude |∇D| there. A healthy TSDF has |∇D| ≈ 1/μ at the surface (the field passes cleanly from +1 to −1 over 2μ). A blurred or doubled surface shows |∇D| well below 1/μ — the field is mushy because inconsistent observations flattened it. If the gradient is fine but you see two crossings, it is pose drift; tighten the front-end (Chapter 4) or reduce the frames fused per surface.

FRONTIER: KinectFusion and its descendants

The TSDF went from a graphics technique to a robotics workhorse with KinectFusion (Newcombe et al., ISMAR 2011): a depth camera, a GPU, real-time dense fusion and tracking against the very surface being built. Its descendants define modern dense mapping. Voxblox (Oleynikova et al., IROS 2017) added incremental Euclidean signed distance extraction for planning (so a planner can read clearance directly off the field) and voxel hashing for scale. nvblox (NVIDIA, 2022–) is the GPU-native, real-time descendant used on robots today. And the neural wave — iSDF (Ortiz et al., RSS 2022) and the NeRF/neural-SDF line — replaces the explicit voxel field with a small network that is the signed-distance function, trained online. The signed-distance idea proved so good that even the deep-learning era kept it — it just moved the field from a voxel array into a network’s weights.

Chapter 3: Detect, Segment, Track

The occupancy grid tells the planner a cell is occupied. It does not tell it whether that cell is a wall it should route around forever, or a forklift that will drive off in ten seconds and should be predicted, not inflated. Both are “occupied.” The difference is semantics, and semantics come from three tasks that a robotics engineer must reason about even when a vendor library does the heavy lifting.

This chapter is deliberately “enough to reason about them.” We are not training a detector; we are learning the three quantities you must be able to define and defend when a perception result feeds a plan: the box overlap metric (IoU), the filter that removes duplicate detections (NMS), and the association rule that keeps a track alive across frames (a gate).

Detection: boxes, and the metric that scores them

Object detection outputs, per frame, a set of boxes each with a class and a confidence: “forklift, 0.92, at pixels (120, 80)–(260, 300).” Every downstream decision — is this the same forklift as last frame? is it close enough to yield to? — rests on one geometric primitive: how much do two boxes overlap? That is Intersection over Union (IoU):

IoU is 0 for disjoint boxes and 1 for identical ones. Work an example by hand. Box A spans pixels (10,10) to (60,60); box B spans (30,30) to (80,80). The intersection is the overlap rectangle, from the max of the corners to the min:

Two boxes overlapping by 22% — below the usual 0.5 threshold, so a detector would call these different objects. That single number decides duplicate-vs-distinct, match-vs-new-track, and correct-vs-false in the evaluation. Everything hinges on it.

Why union? Deriving the denominator from scratch

The formula has a subtlety worth pausing on: why divide by the union and not, say, by the smaller box, or by the average area? Because the union is the only denominator that makes IoU a true, symmetric fraction of the combined footprint — it counts every pixel either box touches exactly once, and asks what share of that combined region both boxes agree on. Divide by the smaller box instead and a tiny box fully inside a huge one scores 1.0 (“perfect match”) even though the boxes describe wildly different objects; the union punishes that size mismatch because the big box inflates the denominator. Union is what makes IoU scale-honest.

And the denominator is not a second thing to measure — it is derived from the two areas and the one intersection you already have, by inclusion–exclusion:

Read it as counting pixels: add up A’s pixels and B’s pixels, and every pixel in the overlap got counted twice — once in A, once in B — so subtract one copy of the intersection to count it once. That is the whole identity. Let us verify it numerically on a second, small pair where we can literally count every cell. Put box A over grid cells (columns 0–3) × (rows 0–2) — a 4×3 block, 12 cells — and box B over (columns 2–5) × (rows 1–3), also 12 cells:

Now check it the honest way — by directly counting the distinct cells the two boxes cover. A alone has 12 cells; B alone has 12; but 4 of them are shared, so the number of distinct cells is 12 + 12 − 4 = 20 — the same 20. The identity is not an approximation; it is exact cell-counting. So IoU(A,B) = 4/20 = 0.200. Two independent pairs, two hand computations — the metric is now something you can produce cold at a whiteboard, denominator and all.

NMS: killing the duplicates

A detector fires many overlapping boxes on the same object — a cluster of slightly different guesses. Non-Maximum Suppression (NMS) prunes them: sort by confidence, keep the highest, delete every remaining box whose IoU with it exceeds a threshold (say 0.5), repeat. What survives is one box per object.

python # greedy NMS from scratch
def iou(a, b):
    x0, y0 = max(a[0], b[0]), max(a[1], b[1])
    x1, y1 = min(a[2], b[2]), min(a[3], b[3])
    inter = max(0, x1 - x0) * max(0, y1 - y0)
    ua = (a[2]-a[0])*(a[3]-a[1]) + (b[2]-b[0])*(b[3]-b[1]) - inter
    return inter / ua if ua > 0 else 0.0

def nms(boxes, scores, thr=0.5):
    order = sorted(range(len(boxes)), key=lambda i: -scores[i])
    keep = []
    while order:
        i = order.pop(0)                 # highest-confidence survivor
        keep.append(i)
        order = [j for j in order if iou(boxes[i], boxes[j]) < thr]
    return keep

Set thr too low and NMS deletes real neighbours (two people standing close become one); too high and duplicates survive. It is a threshold with real failure modes on both sides.

NMS by hand: four boxes, two passes, watch it prune

Code is not understanding. Trace the algorithm on paper and the “keep the best, kill its duplicates, repeat” loop stops being three words and becomes something you can execute cold. Here are four detections, each a 100×100 box (area 10 000), across two real objects — a forklift on the left, a person on the right — each object fired a strong box and a slightly-shifted duplicate:

First compute the IoU of every pair against the box we are testing, by hand. Every box is 100×100, so each pairwise union is 10 000 + 10 000 − intersection = 20 000 − inter. The overlap of two boxes that share full height (100) and differ only in x-offset is (100 − offset) × 100:

Now run the greedy loop with threshold 0.5. Sort by confidence: the order is A (0.95), B (0.90), C (0.75), D (0.70).

Notice what the two passes bought us. In pass 1 the loop could not have deleted D by comparing it to A — their IoU is only 0.111, far below threshold, because A and D are on different objects that barely touch. D was only killable by C, and C only became the “current best” once A had finished suppressing its own cluster. That is why NMS repeats: suppression is always relative to the current highest-confidence survivor, and each object’s cluster is pruned by that object’s own winner, not by an unrelated neighbour.

Segmentation: from a box to a mask

Semantic segmentation labels every pixel with a class — road, sidewalk, person — instead of drawing a box. Instance segmentation goes further: it separates the individual objects (this person vs that person) with per-object masks. For a robot, the payoff is precision at the object boundary: a bounding box around an L-shaped pallet claims a big rectangle of free floor as occupied; a mask claims only the pallet. That extra clearance can be the difference between a passable and a blocked aisle.

The box lies about free floor: an L-pallet worked by hand

That last claim — “a box claims free floor, a mask does not” — sounds like a slogan until you count the cells. So count them. Put an L-shaped pallet on a 4×4 grid of floor cells (16 cells total). The pallet physically occupies an L: the whole bottom row plus the left column above it. In grid coordinates (x right, y up, origin bottom-left), the occupied cells are:

Now ask a detector for a box. A bounding box is by definition the tightest axis-aligned rectangle that contains every occupied cell. The occupied cells span x from 0 to 3 and y from 0 to 3, so the box is the entire 4×4 grid:

The mask claims 7 cells; the box claims 16. The difference is the lie:

Read that ratio. The box marks 16 cells as impassable; only 7 are. 9 of the 16 — 56% of the box — is drivable floor the box has just walled off. The mask marks exactly the 7 real cells and leaves the other 9 open. In a warehouse aisle exactly one pallet wide, those 9 cells are the difference between the robot squeezing past on the free side of the L and declaring the aisle blocked. The concave notch of an L, a U, or any non-convex object is precisely where a bounding box — which can only ever be a rectangle — is forced to lie, and the mask is the only representation that tells the truth cell-by-cell.

Scoring a mask: IoU still, but over pixels

How do you tell a good mask from a bad one? The same IoU you already know, but the sets are now the pixels labelled foreground, not box corners. Mask IoU = |predicted ∩ ground‑truth| / |predicted ∪ ground‑truth|, counted per pixel. Because a mask can be any shape, this rewards getting the boundary right, which a box-IoU cannot even represent — two very different masks can share the identical bounding box and thus identical box-IoU. Mask IoU (the mIoU averaged over classes is the standard segmentation benchmark number) is the metric that finally distinguishes “roughly here” from “exactly this silhouette.”

And there is a second reason a robot prefers the mask: the box-vs-mask gap we just counted is lost clearance, so the mask’s free-floor recovery can be scored directly — the fraction of the box that the mask reclaims as drivable. For the L-pallet that fraction is 9/16 = 0.56. Let us implement exactly that computation from scratch.

DEBUG: the mask has ragged, flickering edges

Symptom. The segmentation of a static pallet is stable in the middle but its boundary shimmers frame to frame — a fringe of pixels flips between pallet and floor — and the costmap built from it grows and shrinks a cell-wide halo around the object every frame, so the planner keeps nudging its path.

Cause. Boundary uncertainty. A segmentation net outputs a per-pixel probability, and the mask is that probability thresholded (usually at 0.5). Pixels right on the object edge sit near 0.5, so tiny per-frame noise flips them across the threshold. It is the segmentation twin of the ID-switch problem: the interior is confident and stable, the boundary is a coin-toss. The box does not have this failure — it has the opposite one (it is stable but wrong by 9 cells).

The metric that reveals it. Track boundary IoU (mask IoU restricted to a thin band around the contour) separately from full-mask IoU. If full-mask IoU is high and steady but boundary IoU is low and noisy frame-to-frame, the interior is fine and only the edge is unstable — point the fix at the boundary: hysteresis (two thresholds, one to turn a pixel on, a lower one to keep it on) or temporal smoothing of the probability map before thresholding, exactly as a Schmitt trigger de-bounces a noisy signal.

Tracking: identity through time, and the association gate

A detector is memoryless — it re-detects from scratch every frame and hands you an unordered bag of boxes. Multi-object tracking adds the missing thing: identity. It answers “the forklift I saw last frame — which of this frame’s boxes is it?” so you can estimate its velocity and predict where it is going.

The heart of tracking is data association: matching this frame’s detections to existing tracks. The clean, classical rule uses a motion filter (a constant-velocity Kalman filter per track — see the uncertainty lesson for the covariance machinery). Each track predicts where its object should appear this frame, with a covariance P describing the uncertainty of that prediction. A detection z is a candidate match only if it falls inside the prediction’s validation gate — measured by the squared Mahalanobis distance:

where ẑ is the predicted position and γ is a chi-squared threshold. For a 2-D position at 99% confidence, γ = χ²₂(0.99) = 9.21. Work it: a track predicts a detection at (5, 5) with P = diag(1, 1). Two detections arrive:

d1 is well inside the gate (0.29 « 9.21) — it is the forklift. d2 is far outside (32 > 9.21) — a different object, which will start its own track. The gate is what stops the tracker from teleporting a track onto an unrelated detection across the frame. Among all detections that pass a track’s gate, the assignment is resolved globally (the Hungarian algorithm minimises total Mahalanobis cost), but the gate is the primitive.

DESIGN: the perception stack’s real budget

Note the asymmetry: the detector is the expensive, GPU-hungry stage; the tracker is nearly free. That is why production stacks run a heavy detector at a modest rate and a light tracker fast, using the tracker’s motion model to interpolate object positions between detections.

DEBUG: the track ID keeps flipping

Symptom. A single pedestrian walks across the frame and the tracker assigns them ID 4, then 4 vanishes and ID 9 appears on the same person, then 12. Downstream, the velocity estimate resets every time, and the planner keeps re-reacting to a “new” obstacle.

Cause. An ID switch, and the usual driver is a detection gap. If the detector misses the person for a few frames (occlusion, motion blur), the track’s prediction coasts on its motion model, its covariance P inflates each missed frame, and when a detection reappears it may fall outside the (still finite) gate — so the old track is killed and a new one is born. The gate that protects you from wrong matches also, if the object is lost too long, prevents the right re-match.

The metric that reveals it. The standard MOT metric is exactly this: ID switches (IDS), and the composite MOTA = 1 − (FN + FP + IDS)/GT. A rising IDS count with stable FN/FP points straight at association, not detection. The fix is a longer track “coast” tolerance (keep a track alive through N missed frames) plus an appearance descriptor so re-identification does not rely on position alone — which is precisely the leap the frontier made.

FRONTIER: from motion-only to motion-plus-appearance

Classical tracking-by-detection is SORT (Bewley et al., ICIP 2016): a Kalman filter per track and the Hungarian algorithm on IoU — fast, and exactly the machinery above. Its weakness is ID switches through occlusion, which DeepSORT (Wojke et al., ICIP 2017) fixed by adding a learned appearance embedding to the association cost, so a re-appearing object is matched by looks, not just position. ByteTrack (Zhang et al., ECCV 2022) squeezed more out of the pipeline by associating even low-confidence detections in a second pass, recovering objects the first pass would have dropped. And detection itself moved to transformers — DETR (Carion et al., ECCV 2020) reframed detection as set prediction and made NMS optional, since the model learns not to emit duplicates. The metric IoU, though, still scores all of them.

Chapter 4: The Handoff to Planning

We have a pose (from localization), a costmap (Chapter 1), maybe a 3-D map (Chapter 2), and a set of tracked dynamic objects (Chapter 3). Now the question that gets glossed over in every block diagram: where, exactly, do these become the input a planner consumes? Draw the seam wrong and the robot plans against a map that is a beat behind the world, or in the wrong coordinate frame — and it drives into a wall while every module reports healthy.

The handoff is a coordinate-frame contract

The single most important fact about the handoff: the costmap and the plan must live in a consistent, well-defined frame, and the pose is what places the robot inside that frame. The standard robotics frame tree (REP-105) is worth memorising because the handoff is this tree:

Why two frames and not one? Because a global pose and a smooth pose are irreconcilable. The map→base_link transform is accurate but jumps when a loop closure corrects drift — you cannot run a wheel controller against something that teleports 20 cm in one tick. The odom→base_link transform is smooth and continuous but slowly drifts. Localization publishes the map→odom correction so that composing the two gives the global pose. Control consumes odom (smooth); planning consumes map (accurate). That split is the handoff.

Deriving the composition: map ← odom ← base_link, by hand

“Composing the two gives the global pose” is a sentence that hides the whole mechanism. Let us do it explicitly, in 2-D, because a ground robot on a floor is exactly an SE(2) problem — a planar position (x, y) and a heading θ. The trick that makes composition a single multiply is the homogeneous transform: pack a rotation and a translation into one 3×3 matrix so that chaining frames becomes chaining matrices.

A pose (x, y, θ) of frame B expressed in frame A becomes the matrix ^AT_B:

The top-left 2×2 block is the rotation R(θ); the top-right column is the translation t = (x, y); the bottom row (0, 0, 1) is what lets a translation ride along inside a matrix multiply. A point p = (p_x, p_y) in frame B is lifted to (p_x, p_y, 1) and transformed by a single matrix–vector product: ^Ap = ^AT_B · [p_x, p_y, 1]^⊤, which unpacks to the familiar R·p + t. The bottom-row 1 carries the translation; that is the entire point of homogeneous coordinates.

Now the handoff. The frame tree gives us two links, and the global pose is their product:

Read right-to-left, the way transforms chain: first place the robot body inside the smooth odom frame (that is odometry’s fast, drifting estimate), then apply the localization correction that places odom inside the global map frame. The correction is ^mapT_odom — usually small, because good odometry keeps the two frames nearly aligned, and it only grows when localization has drift to undo.

“Grows with drift” is quantifiable, and the numbers tell you when a correction is healthy versus alarming. A decent wheeled odometry drifts on the order of 2% of distance travelled in translation and about 0.1° per metre in heading (integrated wheel slip and yaw-rate bias). Between loop closures that drift accumulates, and the correction ^mapT_odom is exactly what localization must apply to cancel it:

Now look back at the correction we are about to compose — a (2, 1) offset and 10°. Its translational magnitude is √(2² + 1²) = 2.24 m, and its heading is 10°. Read against the table, that is consistent with roughly 100 m of travel since the last loop closure — a large but not pathological correction. This is why the composed pose earlier moved so much when the 10° correction rotated the odometry offset: a 10° correction is not a rounding term, it is a hundred metres of un-cancelled drift, and the R·p+t coupling makes its effect grow with how far ahead the obstacle sits.

The practical upshot for a monitoring dashboard: plot the correction magnitude over time and it should sawtooth — ramping up as drift accrues, then snapping toward zero at each loop closure. A correction that ramps and never resets means loop closures are not firing (the robot is lost but does not know it); a correction that is always near zero on a long traverse means either the odometry is unusually good or, more often, the localizer is silently rejecting fixes and the “drift-free” global pose is a fiction. The single number — magnitude of ^mapT_odom — is one of the cheapest health signals a navigation stack exposes.

Let us grind through concrete numbers so nothing is hidden. Say odometry reports the body at (3, 0) with heading 30° in the odom frame, and localization has published a correction that offsets odom by (2, 1) and rotates it 10° to undo accumulated drift. Write both as matrices (cos 30° = 0.8660, sin 30° = 0.5000; cos 10° = 0.9848, sin 10° = 0.1736):

Multiply them. The rotation part composes by adding the angles — 10° + 30° = 40° — so the top-left block is R(40°) with cos 40° = 0.7660, sin 40° = 0.6428. The translation is the interesting part: it is not just (2, 1) + (3, 0), because the correction rotates the odom translation before adding its own. The rule is t_map,base = R(10°) · t_odom,base + t_map,odom:

So the composed global pose of the robot is:

The robot is at (4.95, 1.52) in the map frame, heading 40°. Notice what the 10° correction did: it did not merely shift the pose, it rotated the odometry offset, nudging x from a naive 3 + 2 = 5 down to 4.95 and lifting y from 1 to 1.52. That coupling — rotation acting on the downstream translation — is exactly why you cannot add poses component-wise and must compose matrices. Get it wrong and the robot’s reported position is off by centimetres that grow with the correction angle.

Proving the rotation block is exact: R^⊤R = I, by hand

Before we push a single point through this pose, pause on a claim the rest of the chapter leans on but never shows: that the rotation block R(40°) is orthonormal — its transpose is its inverse, R^⊤R = I. Everything downstream (the closed-form inverse ^baseT_map, the “round-trip returns identity” guarantee) rests on this, so let us not assert it — let us multiply it out. Take R = R(40°) with cos 40° = 0.7660, sin 40° = 0.6428:

The transpose just flips the two off-diagonal terms across the diagonal — no arithmetic, you copy the matrix and swap the signed corners. Now form R^⊤R entry by entry. The top-left entry is row 0 of R^⊤ dotted with column 0 of R:

That is the Pythagorean identity cos²θ + sin²θ = 1 falling out of a matrix product — the diagonal entries are 1 precisely because sine and cosine live on the unit circle. The off-diagonal entry is row 0 of R^⊤ dotted with column 1 of R:

The two products are equal and opposite — the same ±cos·sin term with flipped signs — so they cancel exactly. By the identical algebra (R^⊤R)₁₀ = 0 and (R^⊤R)₁₁ = 1, giving:

Now transform an obstacle. Suppose the lidar, mounted so its point comes out already in the body frame, sees a wall at (4, 0) in base_link — 4 m dead ahead. Apply ^mapT_base as R·p + t:

The wall the sensor called “4 m ahead” lands at (8.02, 4.09) on the global costmap the planner will read. That single number pair — produced by two matrix multiplies — is the perception-to-planning handoff for one point. Every obstacle the planner ever sees arrived through exactly this arithmetic.

One worry is worth killing before it takes root: did we get the same answer only because we composed the matrices first? What if we had instead carried the point one frame at a time — lift it into odom, then into map? If those disagreed, the whole “compose the tree” shortcut would be a lie. Let us check, because matrix multiplication is associative and this is where you feel it. Path B lifts the sensor point (4, 0) into the odom frame first, using ^odomT_base = R(30°)·(4,0) + (3,0):

Now lift that into map with the correction ^mapT_odom = R(10°)·(6.4641, 2.0) + (2, 1):

Identical to the last decimal — the difference is 2×10⁻¹⁵, pure floating-point dust. That is associativity made physical: it does not matter whether you pre-multiply the transforms into one ^mapT_base and apply it, or walk the point up the tree link by link. This is why a robot can cache one composed transform per frame and reuse it for a thousand lidar points instead of re-walking the tree for each — the answer is provably the same, so you pay the composition cost once.

The handoff runs both ways: pushing a goal back to the controller

We just carried an obstacle forward — from the sensor, up through the frame tree, onto the planner’s global costmap. But the seam is bidirectional. The planner finishes its work in the map frame and hands back a goal (a lookahead point on the path, or the next waypoint). The controller does not steer in map; it steers relative to the robot body, in base_link. So the goal must make the inverse trip — map → base_link — before the wheels can chase it. Skip this and the robot turns toward a point defined in the wrong frame, which on a spinning robot means turning the wrong way entirely.

The inverse of a homogeneous transform is not the inverse matrix you would reach for blindly — there is a closed form that never actually inverts anything. For ^mapT_base = [R   t ; 0   1], the inverse is:

Why this works: a rotation matrix is orthonormal, so its inverse is just its transpose, R⁻¹ = R^⊤ — no arithmetic, you literally flip the 2×2 block across its diagonal. The translation part then follows because undoing “rotate then shift by t” means “shift back by t, then un-rotate,” which lands on −R^⊤t. Applying ^baseT_map to a map-frame point p is therefore R^⊤(p − t): subtract the robot’s position, then un-rotate by its heading.

Grind it with the pose we just built — the robot at t = (4.9544, 1.5209) in map, heading 40°, so R = R(40°) with cos 40° = 0.7660, sin 40° = 0.6428. Suppose the planner returns a goal at (8.0, 4.0) in map — essentially the wall corner the robot detected, a point it must approach. Transpose the rotation (R^⊤ just swaps the off-diagonal signs), subtract t, and multiply:

The goal lands at (3.93, −0.06) in base_link — and read what that says physically: the point is 3.93 m almost dead ahead of the robot (x-forward), only 6 cm off the centreline. That is a goal a differential-drive controller can act on directly: drive nearly straight, trim a hair right. The forward trip put the wall on the planner’s map at (8.02, 4.09); the inverse trip pulls the planner’s waypoint back onto the robot’s own axes. Same two-multiply machinery, run in reverse — and because R^⊤R = I, composing the two round-trips returns the identity, the arithmetic proof that the inverse is exact and nothing was lost crossing the seam twice.

“Returns the identity” is another asserted-but-never-shown claim — so let us make it runnable. Build the closed-form inverse from scratch (transpose R, negate R^⊤t), confirm R^⊤R is the identity, then send a point out to base_link and straight back and check it lands on itself to the last decimal:

python # prove the inverse is exact: R^T R = I, and map->base->map is a no-op
import numpy as np

def se2(x, y, theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1]])

def inv_se2(T):                              # closed form: no solver, just a transpose
    R, t = T[:2, :2], T[:2, 2]
    Ti = np.eye(3)
    Ti[:2, :2] = R.T                       # R^-1 = R^T  (orthonormal)
    Ti[:2, 2]  = -R.T @ t                    # undo the shift, then un-rotate
    return Ti

T_map_base = se2(4.9544, 1.5209, np.radians(40))
R = T_map_base[:2, :2]
print(np.round(R.T @ R, 6))              # [[1. 0.] [0. 1.]]  <- exactly I

T_base_map = inv_se2(T_map_base)
p_map  = np.array([8.0186, 4.0921, 1.0])   # the wall the forward trip produced
p_base = T_base_map @ p_map                  # pull it down to base_link
p_back = T_map_base @ p_base                 # push it straight back up to map
print(np.round(p_base[:2], 4))           # [4. 0.]  <- the original sensor point!
print(np.round(p_back[:2], 4))           # [8.0186 4.0921]  <- back to where it started
print(float(np.max(np.abs(p_back - p_map))))  # 0.0  <- zero drift, exact

Rates and latencies: the numbers at the seam

Draw the pipeline with real clocks on it, because the failures at the handoff are almost all timing failures.

Read the structure: a slow, global planner finds a route on the whole costmap; a fast, local planner follows that route while dodging what perception just saw. The dynamic obstacles from Chapter 3 enter at the local planner, as short-horizon predictions (the tracked velocity says where the forklift will be in 1–2 s), so the robot yields to where it is going, not where it was.

Put real ratios on that “slow vs fast” so the widget below is not a hand-wave. Take a 20 Hz controller, a 5 Hz localizer, and a global planner that replans at 1 Hz. Divide the rates and the nesting falls out:

So inside the one second the global planner spends thinking, the controller has already issued 20 motor commands and localization has snapped the pose 5 times. At 1.5 m/s the robot covers 1.5 m per global replan but only 1.5/20 = 7.5 cm per control tick — which is exactly why control cannot wait for the global planner: in the 1 s it takes to replan, an uncorrected robot would have driven a metre and a half blind. The fast local loop, running on the smooth odom frame, fills that gap 20 commands at a time. This nesting — 20 fast ticks wrapping every slow strategic decision — is precisely what the pulsing blocks in the widget below make visible.

“Where it will be in 1–2 s” is another sentence that hides arithmetic — let us compute an actual predicted cell, because that is what the planner marks lethal. Take the forklift the frontend already placed on the global costmap at (8.02, 4.09) in map (the very point our forward handoff produced). Chapter 3’s tracker attached a velocity to that track: say (−0.6, 0.0) m/s — it is rolling toward the robot along the map’s x-axis at 0.6 m/s. A constant-velocity projection over a horizon of 1.5 s is just p + v·Δt:

Now turn that into a costmap cell, because the planner never sees metres — it sees a grid. At a typical 5 cm resolution (0.05 m/cell) with the grid origin at map (0, 0), the cell index is floor(coord / resolution):

The prediction shifts the lethal cell by 160 − 142 = 18 columns — exactly 18 × 0.05 = 0.90 m, the distance we said the forklift covers. So the planner does not merely mark cell (160, 81); it inflates the whole swept corridor from column 160 back to 142 along row 81, and routes around where the forklift is heading. And note the update budget the rates table demands: at the 12 Hz costmap rate the forklift advances 0.6 m/s × (1/12 s) = 0.05 m per frame — precisely one cell per update, so a costmap running any slower would let the obstacle skip cells between frames and tear a gap in the swept corridor. The rate in the table above is not arbitrary; it is set by how fast a cell can be crossed.

One more layer: from lethal cell to inflated corridor, by hand

Marking cells (160, 81) through (142, 81) lethal is not yet a costmap a planner can steer on — the robot is not a point, so a path that grazes a lethal cell would clip the obstacle with the robot’s body. The costmap fixes this with an inflation layer: around every lethal cell it grows a halo of decreasing cost, so the planner is repelled before it touches the obstacle. Let us size that halo in cells, because that is what actually gets written to the grid.

The inflation radius is the robot’s footprint radius plus a safety margin. Take a footprint radius of 0.30 m and a 0.10 m margin, so the inflation radius is 0.40 m. At the same 0.05 m/cell resolution that is:

So each lethal cell grows an 8-cell halo. Along row 81 the lethal core already spans 18 columns (142–160); the inflation extends 8 cells past each end, so the corridor the planner must avoid is:

The 0.90 m swept core has become a 1.70 m no-go corridor — the extra 0.80 m is the robot’s own body plus margin, kept clear on both sides. Now the halo is not a cliff; it decays, so the planner prefers routes that give the obstacle a wide berth but will squeeze closer if it must. nav2’s decay is exponential in the distance d from the nearest lethal cell, past an inscribed radius of 0.20 m:

Grind the cost at three distances out from the core, converting cells to metres as d = cells × 0.05:

The cost falls off smoothly — 253 at the inscribed edge, 187 a cell-and-a-half out, 138 at the full inflation radius. Solve for where it crosses a “prefer to avoid” threshold of 50 and you get d = ln(252/50)/3 + 0.20 = 0.74 m ≈ 15 cells: past there the planner treats the space as essentially free. That gradient is what turns a binary lethal grid into a field the global A* can descend — it does not just avoid the forklift, it leans away from it in proportion to how close the geometry forces it.

Watch that field actually steer a plan. Global A* sums a cell’s traversal cost (the inflation value) into the path cost, so given two routes of similar length it prefers the one that keeps more clearance. Take two candidate cells the planner weighs: cell P sits 6 cells (0.30 m) from the forklift’s lethal core, cell Q sits 12 cells (0.60 m) out. From the same exp decay:

Cell Q is 110.8 cost-units cheaper to traverse. If the planner weights inflation cost at, say, λ = 0.02 metres-of-detour per cost-unit, it will happily accept up to 0.02 × 110.8 = 2.2 m of extra path to route through the roomier Q instead of squeezing past P. That single number — how much detour a clearance is worth — is the entire behavioural difference between a robot that hugs walls and one that gives forklifts a wide berth, and it falls straight out of the exp decay we sized above. Tune the decay steeper and the robot becomes timid; flatten it and it cuts corners.

CODE (from scratch): compose two SE(2) transforms and move a point

Before reaching for a library, build the handoff by hand so nothing is opaque. The whole operation is: turn each pose (x, y, θ) into a 3×3 homogeneous matrix, @-multiply T_map_odom by T_odom_base to compose the frames, then apply the product to a sensor point as R·p + t. This is the arithmetic we just did by hand — here it is in nine lines of numpy, and it prints the very same (8.02, 4.09):

python # the handoff, from scratch: SE(2) compose + apply, no ROS
import numpy as np

def se2(x, y, theta):                          # pose -> 3x3 homogeneous transform
    c, s = np.cos(theta), np.sin(theta)
    return np.array([[c, -s, x],
                     [s,  c, y],
                     [0, 0, 1]])

T_odom_base = se2(3.0, 0.0, np.radians(30))   # body in the smooth odom frame
T_map_odom  = se2(2.0, 1.0, np.radians(10))   # localization's drift correction

T_map_base  = T_map_odom @ T_odom_base           # COMPOSE: map <- odom <- base

p_base = np.array([4.0, 0.0, 1.0])            # obstacle 4 m ahead, homogeneous
p_map  = T_map_base @ p_base                      # apply: this IS R@p + t

print(np.round(T_map_base, 4))                 # [[0.766 -0.6428 4.9544] ...]
print(np.round(p_map[:2], 4))                # [8.0186 4.0921]  <- planner reads this

# the R@p+t is not magic: the bottom (0,0,1) row of se2() carries the
# translation THROUGH the matmul. Peel it apart to see it plainly:
R, t = T_map_base[:2, :2], T_map_base[:2, 2]
print(np.round(R @ p_base[:2] + t, 4))       # [8.0186 4.0921]  <- identical

CODE (library form): the same handoff as one TF lookup

In ROS the entire handoff collapses to one transform lookup — the library builds and composes those matrices for you across the whole tree. Take an obstacle observed in the sensor frame at time t, and express it in the planning (map) frame using the transform as it was at time t. Getting the time right is the whole game.

python # the handoff: put a sensor-frame obstacle into the planner's frame
def obstacle_to_map(pt_sensor, stamp, tf_buffer):
    # look up map <- sensor AT THE MEASUREMENT TIME, not "now"
    T = tf_buffer.lookup_transform("map", pt_sensor.frame_id,
                                   stamp)              # <-- stamp is load-bearing
    return do_transform_point(pt_sensor, T)          # now planner can consume it

# the classic bug: using rclpy.time.Time() (== "latest") instead of `stamp`
# composes a fresh pose with an old measurement -> the obstacle lands
# displaced by however far the robot moved during the latency.

What lookup_transform(…, stamp) hides: interpolating between two samples

That one library call looks like a dictionary lookup, but the scan’s stamp almost never lands exactly on a buffered transform. Transforms arrive at discrete ticks — odometry feeds TF at, say, 10 Hz — and the scan is stamped between two of them. So the library does not fetch a stored transform; it interpolates a new one straddling the query time. Understanding that interpolation is the difference between trusting the call and being able to reproduce it when the buffer is empty and you must do it by hand.

The buffer holds two samples of ^mapT_base: sample A at t_A and sample B at t_B. The scan is stamped at t_scan in between. First compute the interpolation fraction α — how far along the interval the scan falls:

Then blend the two poses at that fraction. Translation is easy — a straight-line lerp (linear interpolation), component by component. Rotation is the subtle part: you cannot lerp two heading numbers naively (359° and 1° would average to 180°, pointing backwards). You take the shortest arc between them — the 2-D case of slerp (spherical linear interpolation) — which is just an angle lerp after wrapping the difference into [−π, π].

That parenthetical — “359° and 1° average to 180°” — is worth grinding out, because it is the single trap that turns a working interpolator into one that snaps the robot’s heading a half-turn backwards for one frame. Suppose sample A is heading 359° and sample B is heading 1° — physically 2° apart, a hair either side of straight-north. The naive lerp at the midpoint α = 0.5 treats these as plain numbers:

The interpolated heading is 180° — pointing due south, the exact opposite of the ~0° the robot is actually facing. For one interpolated tick the entire scan is placed rotated 180°, and every obstacle lands on the wrong side of the robot. The bug is that the raw difference b − a = 1 − 359 = −358° took the long way around the circle instead of the short −2° hop across the 360°/0° seam.

The fix is the wrap formula from the code below, d = (b − a + π) mod 2π − π, which folds any difference into [−180°, 180°] so it always describes the shortest arc. Work it in degrees (using 180° for π, 360° for 2π):

Now the shortest arc is a clean +2°, and the midpoint becomes θ = 359 + 0.5·2 = 360° ≡ 0° — straight-north, exactly where it should be, instead of 180° backwards. The modulo did the whole job: adding 180° recentres the seam, mod 360 wraps the −178 up to 182, and subtracting 180° brings it back to +2. One line of arithmetic separates a heading that points where the robot looks from one that points where it does not.

Make it concrete. Sample A at t_A = 1.00 s: pose (4.90, 1.50, 38°). Sample B at t_B = 1.10 s: pose (5.06, 1.62, 44°). The scan is stamped t_scan = 1.06 s. The fraction is:

Lerp the translation at α = 0.60 — move 60% of the way from A to B:

Slerp the heading. The shortest arc from 38° to 44° is +6° (already inside [−180°, 180°], no wrap needed), so:

The interpolated transform at the scan’s exact stamp is therefore (4.996, 1.572, 41.6°) — and this matrix, not either buffered neighbour, is what places the scan. That case sat comfortably mid-range; now run the same machinery across the seam to prove the wrap earns its place inside the loop. Keep α = 0.60 but make the samples straddle 0°: sample A heading 358°, sample B heading 4° (physically 6° apart, straddling north). The naive heading lerp gives:

145.6° — the robot is facing roughly north, and the interpolator claims it is facing south-east, off by more than a right angle. Feed the difference through the wrap instead:

The wrapped answer is 1.6° — 60% of the way along the short +6° arc from 358°, landing just past north, exactly right. The gap between 1.6° and the naive 145.6° is 144° of pure error, injected on precisely the frames where the robot crosses heading zero. This is why the % (2*np.pi) line in the code below is load-bearing and not decoration: delete it and the interpolator is correct everywhere except the one place a spinning robot visits twice a revolution.

Here is the whole interpolation from scratch, the arithmetic the library performs invisibly on every lookup — the shortest-arc line is the same (b - a + π) % (2π) - π we just ground out by hand:

python # what lookup_transform(..., stamp) does BETWEEN two buffered samples
import numpy as np

def interp_se2(poseA, tA, poseB, tB, t):
    alpha = (t - tA) / (tB - tA)                 # fraction along the interval
    x = poseA[0] + alpha*(poseB[0] - poseA[0])   # lerp translation x
    y = poseA[1] + alpha*(poseB[1] - poseA[1])   # lerp translation y
    d = (poseB[2] - poseA[2] + np.pi) % (2*np.pi) - np.pi  # shortest arc
    theta = poseA[2] + alpha*d                    # slerp (2-D case)
    return (x, y, theta)

A = (4.90, 1.50, np.radians(38))     # buffered sample at t=1.00 s
B = (5.06, 1.62, np.radians(44))     # buffered sample at t=1.10 s
x, y, th = interp_se2(A, 1.00, B, 1.10, 1.06)  # scan stamp between them
print(round(x,3), round(y,3), round(np.degrees(th),1))  # 4.996 1.572 41.6

It is worth pricing out all three ways to answer “what was the pose at t = 1.06 s?” side by side, because the two lazy options both look reasonable in code. Grab the earlier sample A, grab the later sample B, or interpolate — and measure each against the true interpolated heading of 41.6°, converting the heading error to the lateral throw of a 5 m beam (r·tan(err)):

Both snaps land a beam a fifth to a third of a metre off — and note the asymmetry: snapping to the older sample A is worse (31.5 cm) than snapping to the newer B (21.0 cm) here only because α = 0.60 puts the true pose closer to B. Slide the scan stamp earlier and the ranking flips. There is no “safe” sample to grab; the only answer with zero error is the interpolated one. The translation snaps badly too — snapping to A misplaces x by 4.996 − 4.90 = 9.6 cm — but on a lidar the heading error dominates because it multiplies by range.

DEBUG: obstacles smear when the robot turns fast

Symptom. The robot drives straight and the costmap is crisp. But the moment it rotates quickly, walls in the costmap smear into thick arcs, and the local planner suddenly refuses to move, boxed in by phantom obstacles that appear on rotation and clear when it stops.

Cause. A timestamp / transform-timing mismatch at the handoff. The lidar scan is stamped at time t, but the code transforms it into the map frame using the pose at time now (t + latency). During fast rotation the robot’s orientation changed appreciably in that latency, so every beam is placed at the wrong bearing — the whole scan is rotated by the angle the robot turned during the delay, smearing surfaces into arcs. When the robot is still, the latency does not matter; when it spins, it dominates.

The metric that reveals it. Log the age of the transform used for each scan: now − scan.stamp. If smearing correlates with angular velocity × transform-age, it is the timing. Do not take the “24 cm” on faith — derive it. Suppose a 30 ms transform latency while the robot yaws at 90°/s. In that window the robot turns:

The whole scan is stamped with the old orientation but placed with the new one, so every beam is rotated by that 2.7°. To see how far that throws a point, convert to radians and take the lateral offset at the beam’s range. A point 5 m out swings sideways by range × tan(Δθ):

Twenty-four centimetres — exactly the width of the smear on the costmap, and far more than the 5–10 cm inflation radius a planner uses, so the phantom arc really does box the robot in. The lateral throw scales with range: the same 2.7° error is only 4.7 cm at 1 m but 47 cm at 10 m, which is why the smear fans out with distance. The fix is to transform using the pose at the scan’s timestamp (a TF lookup at scan.stamp, buffered), never at now.

Turn the single number into a design table. One derivation gives one data point; an engineer needs the whole surface, because the question in review is never “is 30 ms bad?” but “which combinations of turn speed and pipeline latency am I allowed to ship?” Hold the range fixed at 5 m and sweep yaw rate against transform latency — each cell is just Δθ = yaw·latency, then smear = 5·tan(Δθ):

Read it the way you would defend it at a whiteboard. The middle row (23.6 cm, rounding to the 24 cm we derived above) already blows past a 5–10 cm inflation radius. Halving the pipeline to a tighter 10 ms while slowing the turn to 45°/s drops the smear to 3.9 cm — under the inflation, so it hides inside the safety margin and the costmap stays crisp. But let latency slip to 50 ms at a brisk 90°/s and you are at 39 cm; ask for an in-place 180°/s pirouette and even a good 30 ms pipeline smears 47 cm. The lesson the table teaches, that one row cannot: the tolerable latency is a function of how fast you let the robot rotate. A stack that is fine cruising in straight lines can be unshippable the instant it spins in place — which is exactly the manoeuvre (docking, turning in a doorway) where obstacles are closest and a phantom arc is most dangerous. Either cap the yaw rate or buy back the latency; the table tells you the exchange rate.

The smear has two components, not one. We have chased only the rotational smear so far — but during the same latency window the robot also translated, and that adds a second, independent displacement. Keep the 30 ms latency and the 90°/s turn, but now let the robot also creep forward at a docking speed of 0.4 m/s. The translation smear is simply how far the body moved in the window:

The crucial difference from the rotational smear: this 1.2 cm is range-independent — the whole scan slides 1.2 cm regardless of how far each beam reaches, because the sensor origin itself moved. The rotational smear, by contrast, is range×tan(Δθ) — it fans out with distance. Tabulate both at the same 2.7° rotation to see where each dominates:

Solve r·tan(2.7°) = v·L for the crossover range and you get r = 0.012 / tan(2.7°) ≈ 0.25 m: beyond a mere quarter-metre the rotational smear already exceeds the translational one, and the gap widens linearly with range. This is why the DEBUG hunt fixates on rotation — for any obstacle worth avoiding (well past 0.25 m out) the turning error swamps the driving error. But the translational 1.2 cm never vanishes; it is the noise floor of the smear, present even when the robot rotates not at all, which is exactly why a pure-translation stack still wants its scans transformed at scan.stamp and not at now.

A second timing number: the stale pose at the seam. The same clock mismatch bites even when the robot drives straight, through the pose rather than the scan. Localization runs slow — say 2 Hz — so between fixes the freshest global pose can be a full update period old:

If the robot is moving at 1.5 m/s and the code composes an obstacle against that stale map→odom correction, the obstacle is displaced by how far the robot travelled during the staleness:

Three-quarters of a metre — enough to plant an obstacle on the wrong side of a doorway. This is precisely why the frame tree keeps the smooth odom frame between the slow map correction and the fast controller: odom covers the 500 ms gap continuously, so the pose the robot acts on is never 0.75 m stale even though the global correction is. (Drag the localization-rate slider in the pipeline widget above down to 2 Hz and watch the “pose age” readout climb into the red — that number times your speed is this error.)

FRONTIER: closing the perception–planning gap

The classical handoff is a hard interface: perception produces a costmap, planning consumes it, and the two are designed separately. The frontier is erasing that seam — and it is being attacked from two directions at once, one engineering, one learned.

The systems thread keeps the interface explicit but industrialises it. nav2 (Macenski et al., Science Robotics / IROS 2020) is the modern, production ROS-2 navigation stack that standardises exactly the pipeline this chapter walked — costmap layers → global/local planners → controllers — and orchestrates it with behaviour trees so the timestamped frame handoff we hand-derived becomes battle-tested plumbing rather than glue code you rewrite per robot. The seam stays visible; the contribution is making it reliable, recoverable, and reconfigurable.

The learning thread dissolves the interface instead of hardening it. Here the seam itself is trained through: “perception-aware planning” systems make the planner differentiable back into perception, so gradients from a bad plan sharpen the representation that fed it. In driving, occupancy-flow and end-to-end stacks — e.g. UniAD (CVPR 2023 best paper) — predict future occupancy and plan on it jointly, so the handoff is a learned latent representation rather than a hand-drawn costmap and there is no discrete grid to hand across at all.

The two threads disagree on whether the seam should be sharpened or erased, but they agree on what cannot be waived. The frame contract — smooth odom vs global map, and timestamps that match — survives every architecture, learned or classical, because it is a statement about physics and clocks, not a design choice you can train away.

Chapter 5: The Mapping Bench

This is the payoff. Drive a robot with a range sensor through a scene and watch an occupancy grid build itself out of nothing but noisy beams. Chapter 1 introduced the pieces; this chapter puts them together and makes each one a knob you can turn, so we re-derive the update here from scratch rather than send you back for it.

An occupancy grid is a lattice of cells, each holding one number: how sure we are that this cell is solid. Storing that as a probability p ∈ [0,1] is a trap — combining a new reading means multiplying likelihoods and renormalising, which is fiddly and drifts numerically. The fix is to store the log-odds l = ln(p/(1−p)) instead. In log-odds, Bayes’ rule for a fresh independent reading collapses to a single addition: l ← l + Δ, where Δ is a fixed increment per hit or per pass. No multiply, no renormalise. To read the belief back out you invert with the sigmoid, p = 1/(1+e^−l). A cell at l = 0 is p = 0.5, maximal ignorance; that is why an unseen cell starts at 0.

There is no separate quiz section — but answer this one before you touch a slider.

DESIGN: the numbers this bench actually runs

None of the parameters below are arbitrary, and it is worth surfacing them because in an interview the follow-up to “build an occupancy grid” is always “how many rays, over what fan, at what update size, and why those.” This bench fires 90 rays across a 270° fan (fov = 1.5π radians) out to a 26-cell range. Three numbers, three reasons:

• fov = 1.5π = 270°. A planar lidar like an RPLIDAR or a Hokuyo sweeps a wide arc, not a narrow camera cone; 270° is a common real spec (the back ~90° is blocked by the mounting). We want the bench to look like a lidar so free space carves out to the sides, not just straight ahead.
• 90 rays. Spread 90 rays evenly across 270° and consecutive beams are fov/(rays−1) = 4.7124/89 = 0.0529 rad = 3.03° apart. That is the angular resolution. It has to be fine enough that at the far edge of range no wall cell slips between two beams and stays unmarked: at 26 cells out, adjacent beams are 26 · 0.0529 ≈ 1.4 cells apart, so a 1-cell-wide obstacle can still be missed at max range — which is exactly why distant, grazing cells in the demo stay uncertain while near ones harden. More rays would close that gap at the cost of more work per frame.
• maxr = 26 cells. The grid is 32×18, so 26 cells reaches across most of it but not infinitely — a real range sensor has a max range past which returns are noise, and clearing free space out to infinity would erase walls the robot cannot actually see yet.

The subtlest parameter is the 0.35-cell clearing step. When a beam clears the free cells in front of a hit, it marches outward in increments of 0.35 cells (for d = 0.3; d < end−0.35; d += 0.35) and floors each sample to a cell index. Why 0.35 and not, say, 1.0? Because a diagonal beam crosses a cell’s corner: sampling in whole-cell steps would skip cells the ray truly passes through, leaving un-cleared holes in the free space (the same gap Bresenham solves exactly). A sub-cell step of 0.35 guarantees every cell along the ray gets sampled at least once. But it also means a single cell can be sampled two or three times by one beam — so we must not add the full free-increment each sample or we would triple-count one beam’s evidence. That is why the clearing pass adds L_FREE · 0.35, a fraction sized to the step: three samples of one beam through one cell sum to roughly one full L_FREE, and the beam is counted once, not thrice. This is the classic double-counting hazard of the inverse-sensor model, handled by scaling the increment to the oversampling rate.

Watch the corners. A cell the robot has driven straight past, seen from many angles, hardens into a confident wall. A cell it glimpsed once, from far, at a grazing angle, stays uncertain. This is the difference between the parts of a map you can trust and the parts you cannot — and it is exactly the information a good exploration policy uses to decide where to look next (the frontier between known and unknown, which the occupancy-mapping lesson turns into an exploration strategy).

One beam, start to finish

The bench integrates 90 beams every frame, but a single beam is the whole algorithm in miniature. Follow one all the way through — it is exactly four steps, and every cell in the grid is built out of nothing but these four repeated.

Trace it concretely. Say the robot is at cell (2, 2), a beam points along +x, and the true wall is at cell (7, 2) — so end ≈ 5 cells. Step 1 raycasts and returns a hit at (7, 2). Step 2 clears the cells the beam crossed — (3,2), (4,2), (5,2), (6,2) — each getting −0.1713; a cell that starts unknown at l = 0 drops to l = −0.1713, i.e. p = 1/(1+e^0.1713) = 0.457, now leaning free. Step 3 marks (7,2) with +0.9445, so l = +0.9445, p = 0.720, leaning occupied. Step 4 does nothing yet — both values are far inside ±8. Next frame, the same beam from a slightly different pose repeats all four, and (7,2) climbs from 0.720 toward the clamp while the free cells sink toward 0. That accumulation is the map.

Under the hood: how one beam finds its cells

The bench above does something on every beam that we have taken for granted: it walks the straight line of cells from the robot to the hit, marking each free, and marks the endpoint occupied. That walk is Bresenham’s line algorithm — an integer-only rasteriser that steps through exactly the grid cells a ray passes through, no floating-point rounding, no gaps. It is the workhorse under the “clearing” pass, and the Studio session at the top of the dock is built around it. Implement it once and you own the free-space update.

The rule looks cryptic stated as “e2 = 2·err; if e2 > −dy step x, if e2 < dx step y” — so before you code it, execute it by hand once. It is the kind of thing that is opaque until you turn the crank and obvious forever after.

Chapter 6: The Field Guide

It is 2 a.m., you are on-call, and the robot just froze in the doorway of an open lab — wheels stopped, planner reporting “no valid path,” a clear three-metre gap dead ahead. The map on your laptop shows a smear of “occupied” cells streaking across the floor where nothing exists. You have maybe ninety seconds before someone asks what is wrong. Where do you look first?

You do not re-derive occupancy theory at 2 a.m. You reach for the one signal that names the fault, confirm it, and push a fix. That reflex — symptom → the metric that reveals it → the one-line correction — is what this chapter loads into muscle memory. (For the frozen-doorway case: the phantom streak is transient hits that were never cleared, and the metric is the clear:mark ratio — if it has collapsed toward 1, ray-clearing is off. Section 4 catalogues the rest.)

Chapters 1–5 derived the ideas from zero; this chapter is what stays loaded when you are actually building or debugging a stack under time pressure — the equations compressed to one line, the numbers you should be able to produce cold, and the failure catalogue with its diagnostic metric. Read it once through, work the arithmetic in sections 3 and 6 by hand until it is automatic, then treat it as a lookup.

1 · The cheat sheet

2 · System-design patterns

• Rolling local + persistent global. Keep a small robot-centred local costmap that scrolls (bounded memory, fast, reactive) and a larger global map (strategic). The local one holds live obstacles; the global one holds the known structure.
• Layer the costmap. Static layer (prior map) + obstacle layer (live log-odds) + inflation layer (safety). Each is independently testable; the master costmap is their max.
• Store thin, extract on demand. A TSDF stores only a shell around surfaces (voxel hashing / octree); mesh it with Marching Cubes only when you need geometry, and query D directly for clearance.
• Heavy-slow perception, light-fast fusion. Run the expensive detector at a modest rate and a cheap tracker fast, using the motion model to interpolate between detections.
• Frames are a contract. Everything downstream of localization must agree on which frame it lives in, and every obstacle carries the timestamp of the pose that placed it. Transform at the measurement time, never at “now.”
• Clamp everything that accumulates. Log-odds, TSDF weights — cap them so the map can adapt to a changing world in bounded time.

3 · Coding drills (do these from memory)

1. Write the log-odds update: given a prior l, a hit adds log(0.7/0.3), a pass adds log(0.4/0.6), clamp to ±8, convert back with 1 - 1/(1+exp(l)). (Code Lab 1.)
2. Raycast a beam into a grid with Bresenham: mark every cell along the ray free, the endpoint occupied. Predict the free-cell list for a ray from (2,2) to (7,5). (Studio build kernel.)
3. Compute a TSDF from a depth: clamp((depth - range)/mu, -1, 1). Fuse two and show the zero-crossing lands between them. (Code Lab 3.)
4. Write IoU and greedy NMS. Hand-compute IoU for boxes A = (10,10,60,60) and B = (30,30,80,80) in (x₁,y₁,x₂,y₂) form. Intersection: the overlap box runs from max(10,30)=30 to min(60,80)=60 in x, and max(10,30)=30 to min(60,80)=60 in y, so its width and height are both 60−30 = 30, giving 30×30 = 900. Union: each box is 50×50 = 2500, so union = 2500 + 2500 − 900 = 4100 (subtract the overlap once, or you double-count it). IoU = 900 / 4100 = 0.220. Say “below a 0.5 NMS threshold, so these survive as two separate boxes” as you write it.
5. Write the Mahalanobis gate: r @ inv(P) @ r < gamma. With P = diag(1,1), inv(P) = diag(1,1) too, so d² is just the plain squared length of the residual. For a residual of (0.5, 0.2): d² = 0.5²/1 + 0.2²/1 = 0.25 + 0.04 = 0.29 — well under the χ²₂(0.99) = 9.21 gate, so it passes. For (4, 4): d² = 4² + 4² = 16 + 16 = 32, far above 9.21, so it is rejected. Note that a non-identity P shrinks the axes it is confident about — the same 4-unit residual could pass or fail depending on the covariance.

4 · Debugging catalogue — symptom → cause → the metric that reveals it

5 · Classical vs modern

6 · The numbers to carry

These are the seven you should be able to produce cold, with the arithmetic in your head, not the answers memorised. In an interview the derivation is the answer — anyone can quote “0.22,” only someone who has built it can walk the intersection out of the coordinates. Work each chain below once by hand before you rely on the result.

• Log-odds increments. A hit multiplies the odds by 0.7/0.3, which in log-space is an add of ln(0.7/0.3) = ln(2.333) = +0.847. A pass adds ln(0.4/0.6) = ln(0.667) = −0.405. Starting from an uninformative prior (p = 0.5, so l₀ = 0), three consecutive hits give l = 3 × 0.847 = 2.542. Convert back with p = 1 − 1/(1 + e^l) = 1 − 1/(1 + e^2.542) = 1 − 1/13.71 = 0.927. Three confident hits, and the cell is 93% occupied — but note it took three, because a single spurious hit only reaches p = 0.70.
• Grid footprint. A 10 m × 10 m world at 5 cm resolution is 10 / 0.05 = 200 cells per side, so 200 × 200 = 40,000 cells. At one byte per cell (a clamped log-odds fits in an int8) that is 40 KB — small enough to live in L2 cache, which is why occupancy grids are fast. Halve the resolution to 2.5 cm and it quadruples to 160 KB; the cost grows with the square of the fineness.
• Beam cost. A single 10 m beam at 5 cm steps through 10 / 0.05 = ~200 cells as it walks (Bresenham marks each free, the last occupied). One scan of 1,000 beams therefore touches 1,000 × 200 = ~200,000 cell writes. At, say, 10 scans per second that is 2 million writes per second — trivially cheap, which is the whole point: the integer log-odds update was chosen so this loop never becomes the bottleneck.
• TSDF fusion. Two depth cameras see the same wall; camera A reports it 0.5 m too near, camera B 0.5 m too far. With equal weights the fused signed distance is the average, D = (1·(−0.5) + 1·(+0.5)) / (1 + 1) = 0 / 2 = 0 exactly at the true surface. The zero-crossing lands on truth even though neither frame did — symmetric errors cancel in the weighted mean. (Bias them: weights 3 and 1 give (3·(−0.5) + 1·(0.5))/4 = −1.0/4 = −0.25 m, so trust the near camera three-to-one and the surface pulls a quarter-metre toward it.)
• IoU. Boxes A = (10,10,60,60) and B = (30,30,80,80). The intersection box spans x ∈ [max(10,30), min(60,80)] = [30,60] and the same in y, so it is 30 wide by 30 tall = 900. Each box is 50 × 50 = 2500, so union = 2500 + 2500 − 900 = 4100 (subtract the shared 900 once). IoU = 900 / 4100 = 0.220 — below a 0.5 threshold, so NMS keeps both.
• Gate. The 99% gate for a 2-DOF residual is χ²₂(0.99) = 9.21. With P = I the squared Mahalanobis distance is just the squared length: residual (0.5, 0.2) gives d² = 0.5² + 0.2² = 0.25 + 0.04 = 0.29, far inside 9.21, so it passes. Residual (4, 4) gives d² = 16 + 16 = 32, far outside, so it is rejected. The gate is a soft ellipse whose radius the covariance sets.
• Smear bug. A scan stamped 30 ms stale while the robot yaws at 90°/s was rotated by the wrong angle by 0.030 s × 90°/s = 2.7°. An obstacle 5 m out is displaced by an arc length of 5 m × 2.7° = 5 × (2.7 × π/180) rad = 5 × 0.0471 = 0.236 m — nearly a quarter-metre of phantom smear from a 30 ms timestamp error. This is why you transform at the scan’s stamp, never at “now.”

7 · Recommended reading

• Occupancy grids: Elfes, “Occupancy Grids: A Probabilistic Framework…” (1989); Thrun, Burgard & Fox, Probabilistic Robotics (2005), ch. 9.
• Octree occupancy: Hornung et al., “OctoMap”, Autonomous Robots (2013).
• TSDF & fusion: Curless & Levoy (1996); Newcombe et al., KinectFusion (ISMAR 2011); Oleynikova et al., Voxblox (IROS 2017).
• Detection & tracking: Bewley et al., SORT (2016); Wojke et al., DeepSORT (2017); Zhang et al., ByteTrack (2022); Carion et al., DETR (2020).
• Navigation stack: Macenski et al., “The Marathon 2 / nav2” (IROS 2020); REP-105 (coordinate frames).
• On this site: Occupancy Mapping & Frontier Exploration, 3D Vision, Robotics Simulation, Uncertainty & Least Squares.

8 · The field-guide sandbox

A reference you cannot re-run is just trivia. Below, the three mini-calculations from sections 3 and 6 — the log-odds belief after N hits, the IoU of two boxes, and the Mahalanobis gate test — recompute live as you drag one slider each. Watch the number move: log-odds is convex (the first hit buys the most belief), IoU falls to zero the instant two boxes stop touching, and the gate flips from PASS to REJECT the moment d² crosses 9.21. If you can predict where each threshold sits before you drag to it, you have the numbers cold.

“The map is not the territory — but a robot has nothing else. Build it honestly.”