Pose by agreement I: aligning scans

Lesson 3 gave us a pose we can nudge without leaving the space of motions, but not a direction to nudge it. The direction comes from the data: two scans of one room must coincide where they overlap. With the pairs of corresponding points known, that agreement is a sum of squared distances with an exact solution (centroids, then one atan2). The pairs are not known, so we pair each point with its nearest neighbour, solve, and repeat: ICP, which cannot raise its error and stops at the nearest local minimum. The lesson measures where that works, where it crawls, where the scene cannot pin the pose, and what the loop demands of the sensor: ranges.

1 · Agreement, written as a number

Take the room of Flatland (12 m by 8 m, two pillars and a shelf) and a planar LiDAR with 180 beams two degrees apart and range noise σ = 1 cm. Scan 1 is taken at the pose P₁; the sensor then moves to P₂ = P₁T by an unknown relative motion T (it turns 17.2° and shifts 0.76 m, which we pretend not to know) and takes scan 2. Each scan is a list of points in its own sensor's frame, p_i for scan 2 and q_j for scan 1, and the two lists are all we have. A pose is lesson 3's, with one angle for the rotation: T = (θ, t) carries a point of scan 2 into the frame of scan 1 by p ↦ R(θ)p + t, with R(θ) the counter-clockwise turn, x to the right and z up. (In the plane the exponential step of lesson 3 is plain addition, θ ← θ + δθ, because the rotations form a single circle.)

With no correction the typical point of scan 2 lies 68 cm from the nearest point of scan 1 (the disagreement number of lesson 3, as a median); at the true pose it lies 4.3 cm. "The scans agree" must mean that the points of scan 2, moved by T, lie where scan 1 also saw something. Suppose someone gives us the partner of every point: the point q_i of scan 1 that is the same place in the world as p_i. Then agreement is a sum of squares,

E(T) = Σ_i ‖R(θ)p_i + t − q_i‖²

with three unknowns, θ, t_x, t_z, and two equations per pair: a least-squares problem, which can be solved rather than searched.

2 · If the pairs are known: one step, no search

Set the derivative of E with respect to t to zero: Σ(Rp_i + t − q_i) = 0, so t = c_q − R c_p, where c_p and c_q are the centroids of the two lists: the translation carries the turned centroid of scan 2 onto the centroid of scan 1. Centre both lists, p′_i = p_i − c_p and q′_i = q_i − c_q. With this t, Rp_i + t − q_i = Rp′_i − q′_i, and a rotation preserves length, so

E(θ) = Σ‖Rp′_i − q′_i‖² = Σ‖p′_i‖² + Σ‖q′_i‖² − 2 Σ q′_i·(R p′_i)

and only the last sum depends on θ; E is smallest where it is largest. In the plane q′·(R(θ)p′) = cos θ (p′·q′) + sin θ (p′×q′), with p′×q′ = p′_xq′_z − p′_zq′_x, so the sum is

A cos θ + B sin θ = √(A² + B²) · cos(θ − θ*),   A = Σ p′_i·q′_i,   B = Σ p′_i×q′_i,   θ* = atan2(B, A)

That is the whole solution: θ* = atan2(B, A), t = c_q − R(θ*)c_p, and the smallest possible disagreement is E_min = Σ‖p′‖² + Σ‖q′‖² − 2√(A² + B²). Pair i asks for a turn by the angle φ_i from p′_i to q′_i and adds |p′_i||q′_i| times cos φ_i to A and times sin φ_i to B, so θ* is the average of the requested turns weighted by |p′_i||q′_i|: far points count most. On exact pairs it is exact to 10⁻¹².

A and B are entries of the cross-covariance H = Σ p′_iq′_i^T (A = H₁₁ + H₂₂, B = H₁₂ − H₂₁), and the sum to maximise is Σ q′_i·(Rp′_i) = tr(RH). Everything up to here holds in any dimension; only the atan2 belongs to the plane. In 3-D, where a rotation is no longer one angle (lesson 3), the maximiser comes from the singular value decomposition H = UDV^T (U, V orthogonal, D diagonal and non-negative): the best orthogonal matrix is R = VU^T, with t = c_q − Rc_p as before (Kabsch, 1976; Arun, Huang and Blostein, 1987; Horn, Hilden and Negahdaripour, 1988; Horn, 1987, used unit quaternions, which are always rotations). It is the "best rotation" of lesson 3's widget (there with no translation), which tangent-space descent needed 22 steps to reach and this reaches at once. But an orthogonal matrix can be a mirror image, and no motion of a rigid scanner is a mirror. Umeyama (1991) noted that the solutions of Arun et al. and of Horn et al. sometimes return a reflection when the data are severely corrupted, and gave the repair R = V diag(1, …, 1, det(VU^T)) U^T: where det(VU^T) = −1 it flips the direction of the smallest singular value, which makes det R = +1, a rotation in lesson 3's sense. The atan2 route never meets the problem, since it searches only rotations. Where does the SVD route meet it?

Take p = (0, 0), (2, 0), (0, 1) and partners that are its mirror image, q = (0, 0), (2, 0), (0, −1), as if the second sensor spun the other way. Then det H = −1.33. The unguarded VU^T is the reflection diag(1, −1) and fits perfectly (E = 0), which no real motion can; the best rotation has θ* = 33.7° and E = 1.86. The sign of det H tells which case you are in, and it is not rare: points on a straight wall are collinear, H has one significant singular value, and noise decides the sign of its determinant. In 504 of 1000 simulated fits of a seven-point wall (0.5 m apart, 1 cm of noise) the unguarded SVD returned a mirror image; the guarded SVD and the atan2 formula returned a rotation every time. A flat wall in 3-D does the same: for a 3×3 patch of points (0.5 m apart, 1 cm of noise) the unguarded SVD returned a mirror image in 488 of 1000 fits, the guarded one in none.

3 · If they are not: match, solve, repeat

The pairs are the catch. Three ways to get them, and what each costs here:

No choice can be exact, because the exact partner does not exist: two scans sample one surface at different places. At the true pose the nearest scan-1 point is a median 4.3 cm from a scan-2 point (about a quarter of the 18 cm between neighbouring samples) and no distance is zero. So take the best partner we can name and correct it as we go. Given a pose T_k: (1) match: move every p_i by T_k and pair it with its nearest neighbour q_m(i) in scan 1; (2) solve: apply §2 to those pairs to get T_k+1. Repeat. This is the iterative closest point algorithm (Besl and McKay, 1992; Chen and Medioni, 1991).

It never makes things worse. Let E(T, m) = Σ‖Tp_i − q_m(i)‖² for a pose and a pairing, and f(T) = Σ min_j‖Tp_i − q_j‖² the error with the best pairing for that pose. If m_k is the nearest-neighbour pairing at T_k, then

f(T_k+1) ≤ E(T_k+1, m_k) ≤ E(T_k, m_k) = f(T_k)

the first step because m_k is only one pairing, the second because the solve minimises over poses, the equality because matching is the best pairing. The error cannot rise and cannot fall below zero, and a strict fall needs a pairing not used before, of which there are finitely many; so the loop stops, at a pose where matching and solving change nothing: a local minimum of f. Besl and McKay (1992) state the same: ICP converges monotonically to the nearest local minimum of a mean-square distance. Recomputing f with brute-force neighbours at every pose of 600 seeded runs (14295 poses; four kinds of data, three noise levels, three trimming levels) found no rise.

One run, from a start 50 cm and 25° off: the RMS distance of the matches, √(f/N) over the N pairs, falls from 94.3 cm to 16.2 cm in 21 iterations. The position error does not follow: it reaches 4.6 cm at iteration 11 and ends at 7.0 cm (and 0.44°), because ICP minimises f, not the distance to the truth, and the minimum of f is not at the truth. §5 says why.

4 · How close is close enough

ICP descends f, so it ends in a local minimum of f, and which one depends on the start. Fix the position error at 0.5 m (in one direction), sweep the starting rotation error from −180° to 180° in 6° steps (61 starts) and record where each run ends. 22 end at the true pose (within 10 cm and 1.5°), 21 end near a half turn away (the room is nearly symmetric under a half turn; only the pillars and the shelf tell the two alignments apart, and the wrong one has RMS 64 cm against 16 cm), and the other 18 stop at scattered poses. The run of successes around zero, the basin of attraction, spans −60° to +66°.

Why there? A match helps if it pairs a point with the part of the room it belongs to, because then its pull points toward the truth. A rotation error ψ moves a point at range L by about Lψ, and near a corner or on a wall seen at a slant that is enough to leave it nearer to another wall. The engine knows which wall each beam hit (ICP does not), so we can count the first matches that land on the right wall: 100 % with no rotation error, 66 % at 30°, 33 % at 60°, 0 % at 180°. In this sweep every start that ended at the true pose had at least 29 % of its first matches on the right wall, and every other start had at most 27 %. The edge moves with the scene and the position error (the widget shows how), which is why ICP is fed a rough guess from elsewhere. A wrong minimum is silent: from 180° the loop stops 140 cm and 175° from the truth, as normally as ever.

5 · The distance that matters is to the surface

At the true pose we know the answer, so look at what point-to-point ICP is asked to minimise there. Split each residual into the part across the wall and the part along it, using the wall's normal n, estimated from the scan itself by a line fit through neighbouring beams (161 of 180 points get one). Across the wall the RMS is 1.7 cm, about the range noise of two scans. Along the wall it is 17.4 cm: 99 % of the squared residual lies along the wall, where the samples of scan 1 happen to fall and where shadows leave points without a partner. Each pair holds its point to one particular sample, like a spring along the wall, and every iteration re-attaches the springs to other samples. Take the shadows away (a copy of the room without the pillars and the shelf) and the springs alone still cost point-to-point 28 iterations and 1.3 cm of error, where the remedy below needs 4 and 0.17 cm; with the shadows the point-to-point error is 7.0 cm.

The remedy, introduced by Chen and Medioni (1991), is to measure the distance to the surface: keep the match, replace the matched point by the line through it with normal n_j, and use only the across-wall residual r_i = n_j·(Tp_i − q_j). This is point-to-line (point-to-plane in 3-D). It has no closed form, since r depends on θ through a sine and a cosine, so linearise. A small step ξ = (δ, δθ) moves a transformed point u = Tp to u + δθ·u^⊥ + δ, with u^⊥ = (−u_z, u_x), so

r_i(ξ) ≈ r_i + J_iξ,   J_i = (n_x, n_z, u×n),   G ξ = −Σ J_i^Tr_i,   G = Σ J_i^TJ_i

a 3×3 system per iteration. The step is applied as a rigid motion (turn by δθ, shift by δ), so the estimate stays a pose: lesson 3's tangent-space step. The monotone argument of §3 does not carry over, since a linearised step does not minimise exactly. Rusinkiewicz and Levoy (2001) split ICP variants into six stages (select, match, weight, reject, error metric, minimise); this lesson varies the error metric and the rejection (§6). Over 171 seeded starts (rotation errors up to 45°, position errors 0.3 to 1 m, three directions) point-to-point missed the truth in 4 and point-to-line in 0. On the 167 that both solved:

In all 167 of them point-to-line stopped sooner, by a median factor of 4.0, and ended closer: once the lines are right the problem is nearly linear and a Gauss–Newton step is nearly exact.

6 · When the scene cannot pin the pose, and when the data lie

Geometry. The matrix G of §5 says how much each pose change is constrained: moving by s along a unit eigenvector (s in metres, rotation counted in radians) adds λs² to the sum of squared distances, λ being its eigenvalue. In the room the three are 42, 113 and 789. In a corridor (two parallel walls, longer than the sensor's range) every normal is (0, ±1), the first column of every J_i is zero, and the smallest eigenvalue is 0.07: not exactly zero only because noise tilts the line fits, and below 10⁻³ without noise. Its eigenvector is a slide along the corridor, which changes no point's distance to its wall. Start ICP 0.5 m away, 38 cm of it along the corridor, and 3° off: point-to-line fixes the angle and the offset across, reaches a fit at the noise level (RMS 1.0 cm) in 3 iterations, and stops 40 cm from the truth. Point-to-point ends 41 cm away. Converged does not mean right; the eigenvalue says which direction not to believe.

Data. Some points have no partner: the shadow of a pillar in one scan only (2.8 % of scan 2 lies more than 30 cm from anything in scan 1 even at the true pose), or a person who moved between the scans. Under a squared loss a point pulls on the solution in proportion to how wrong it is. Put a person in scan 2 only, 1.5 m from the sensor: 10 beams (5.6 %) land on him, and the answer moves to 15.0 cm off (point-to-point) or 15.4 cm (point-to-line). The remedy is to stop counting the worst pairs: rank them by residual and solve with the best 90 %. For point-to-point the argument of §3 survives (for a fixed pose the best 90 % minimise the sum over all choices of 90 %, and the solve minimises over the pose), so the trimmed error still never rises. With the best 90 % the errors become 0.91 cm and 0.26 cm. Trimming helps in the plain room too, where point-to-point ends 0.95 cm off instead of 7.0 cm, which fits §5: the long residuals at shadows were biasing it. The price is a narrower basin there, −66° … +42° and −78° … +36°.

qx[i] = c * src.x[i] - s * src.z[i] + T.x; qz[i] = s * src.x[i] + c * src.z[i] + T.z;
var nn = ICP.nearest(mod, qx[i], qz[i]);
jj[i] = nn.j; d2[i] = nn.d2;
...
for (k = 0; k < idx.length; k++) { i = idx[k]; P.push({ x: src.x[i], z: src.z[i] }); Q.push({ x: mod.x[mt.j[i]], z: mod.z[mt.j[i]] }); }
var al = ICP.align(P, Q);
res.Tnew = { a: al.a, x: al.x, z: al.z };
...
var c = ICP.crossCov(P, Q), a = atan2(c.B, c.A), ca = cos(a), sa = sin(a);
return { a: a, x: c.qc.x - (ca * c.pc.x - sa * c.pc.z), z: c.qc.z - (sa * c.pc.x + ca * c.pc.z), E: c.spp + c.sqq - 2 * sqrt(c.A * c.A + c.B * c.B) };
...
jr[0] = nx; jr[1] = nz; jr[2] = mt.qx[i] * nz - mt.qz[i] * nx;
for (var a = 0; a < 3; a++) { g[a] -= jr[a] * mt.r[i]; for (var b = 0; b < 3; b++) Hm[a * 3 + b] += jr[a] * jr[b]; }

What to try. Open the page as it stands: room, point-to-point, a start 25° and 0.5 m off. At iteration 0 the matches are 94.3 cm apart (RMS) and 73 % of the grey lines join a point to its own wall. Press run: 21 iterations, the RMS falls to 16.2 cm, and the pose ends 7.0 cm and 0.44° from the truth. Switch to point-to-line and run again: 5 iterations and 0.3 cm. Drag the rotation slider: the basin readout gives −60° … +66° for point-to-point and −90° … +84° for point-to-line, and at 180° point-to-point ends a half turn away, 140 cm off. Raise the position error to 2 m: the point-to-point basin shrinks to −36° … +60°, point-to-line's moves to −96° … +84°; set it back to 0.5 m. Tick drop the worst 10 %: the plain room now ends 1.0 cm off with point-to-point, and the basins narrow. Choose the person: the errors (point-to-point, point-to-line) are 15.0 and 15.4 cm without the tick and 0.9 and 0.3 cm with it; red lines mark the pairs left out. Untick, choose the corridor, point-to-line and a rotation error of 3°: it stops after 3 iterations at RMS 1.0 cm, still 40 cm from the truth; the weakest direction reads 0.07 against 42 in the room, and the basin reads none. Choose bearings only: the two circles coincide, the RMS reads 0.0 cm, and the estimate sits on sensor 1, 76.2 cm from the ring.

7 · What the loop was resting on

Every step so far used one fact: the sensor reports where each surface point is, as a range along a known beam. Take the range away and keep what a camera gives, the direction of each ray (lesson 1); the loop needs a guess for the rest, say 5 m for every beam. A scan becomes a circle around its sensor, and two circles around different centres coincide only when the centres do. So ICP from the default start walks sensor 2 onto sensor 1: after 7 iterations (point-to-point) the RMS distance of the matches is 0.0 cm, a perfect fit, and the pose is 76.2 cm from the truth, exactly the distance the sensor moved. A small residual says that the points agree, not that the pose is right. With ranges the same start ends 7.0 cm (point-to-point) and 0.26 cm (point-to-line) from the truth.

Common mistakes / failure modes

Checkpoint exercise

Where this points next

Aligning scans turned "the scans should coincide" into a least-squares problem: exact in one step when the pairs are known, a loop that cannot raise its error when they are not, faster once it measures the distance to the surface, and reliable only from a start inside its basin. All of it rests on one fact, that the sensor reports where each surface point is. Keep only the direction of each beam and ICP reaches a perfect fit at a pose 76.2 cm from the truth: with directions alone there are no points to bring together. How can poses be recovered from images alone, and what plays the role of "the scans should coincide"?

Interview prompts

• Derive the closed-form alignment of two paired point sets in the plane. (§2 — centroids give the translation; maximise A cos θ + B sin θ, so θ = atan2(B, A).)
• What can go wrong with R = VU^T from the SVD of the cross-covariance, and what is the fix? (§2 — a reflection when det H < 0, as with noisy points on a line or in a plane; flip the last singular direction by det(VU^T), in 2-D and 3-D alike.)
• Why does ICP never increase its error, and what does it converge to? (§3 — matching minimises over pairings, solving over poses; the error is bounded below, so it settles at a local minimum.)
• Why does point-to-plane need fewer iterations than point-to-point? (§5 — point-to-point pins each point to one sample, and most of its error lies along the surface, where it says nothing.)
• When is the pose not determined, and how do you detect it? (§6 — when ΣJ^TJ has a near-zero eigenvalue, as in a corridor; its eigenvector is the direction not to trust.)
• Why can ICP not align two camera images directly? (§7 — it needs points with ranges; with a guessed range the loop reaches a perfect fit at a wrong pose.)

Companion reads: Computer Vision · 05 Multi-view, depth and SLAM (alignment run online as SLAM), Computer Graphics · 02 Transforms and spaces (rigid transforms as matrices).