Registration and ICP

Lesson 03 handed us point clouds — but each depth scan is partial (a sensor sees one side of the world) and each sits in its own coordinate frame (wherever the camera happened to be). To fuse many scans into a single model, or to localize a live scan against a prior map, we must align them: find the rigid motion that carries one cloud onto another. That is not a new kind of problem — it is exactly the SE(3) estimation of lesson 02, now driven by data instead of given. This lesson derives the closed-form solution when we know which point matches which, then the iterative one (ICP) when we don't, and finally the failure modes that decide whether alignment succeeds at all.

The plan

Four moves. (1) State registration precisely — two overlapping clouds in different frames, find the T=(R,t)∈SE(3) that best maps one onto the other — and split it into two regimes by whether correspondences are known. (2) Known correspondences give a closed form: the orthogonal Procrustes / Kabsch solution via SVD, with the reflection fix that keeps it a rotation. (3) Unknown correspondences give Iterative Closest Point: alternate "guess the matches" with "solve the pose", including the point-to-plane variant that dominates in practice. (4) The failure modes — bad initialization, outliers, geometric degeneracy — and their fixes: robust kernels, global registration for the init, and the bridge to pose-graph SLAM when pairwise alignments accumulate drift.

1 · The problem — align two clouds in different frames

You have two point clouds that overlap. Call one the source P = {p₁, …, p_n} and the other the target Q = {q₁, …, q_m}. They were captured from different viewpoints, so they are expressed in different coordinate frames — the same physical corner of a table sits at different numbers in P and Q. Registration is the task of recovering the rigid transform T=(R,t)∈SE(3) (lesson 02) that carries the source into the target's frame, minimizing the squared distance between corresponding points:

T^* = arg min_{R∈SO(3), t} Σ_i ‖q_i − (R p_i + t)‖² .

The whole difficulty hides in that subscript i: it presumes we know that p_i corresponds to q_i. That single assumption cleaves the problem into two regimes, and the rest of the lesson is one section per regime:

Regime A — §2

Correspondences known → closed form (Procrustes/Kabsch), solved in one SVD, no iteration.

Regime B — §3

Correspondences unknown → ICP alternates guessing matches and solving the pose until it settles.

Why this matters is two products that look different but are the same estimation:

Multi-scan reconstruction. Rotate an object on a turntable, or walk a scanner around a room, and you collect a dozen partial clouds. Registering each to the last (or to a growing model) stitches them into one complete surface — the input to the meshing of lesson 05.
Localization. A robot or headset holds a prior map (a reference cloud) and receives a live scan from its sensor. Aligning the live scan to the map is computing the device's pose in the map. Registration and localization are the same operation read in two directions: solve for T to fuse geometry, or to place the sensor.

2 · Known correspondences → the closed-form solution

Suppose an oracle hands us the matched pairs {(p_i, q_i)}. Then the objective above is a clean least-squares problem over SE(3), and it has an exact solution — no iteration, no learning rate. It is called orthogonal Procrustes (the rotation-only case is Kabsch's algorithm, familiar from aligning molecules). The derivation is worth internalizing because it reappears inside every ICP step.

Step 1 — center both clouds. Compute the centroids p̄ = (1/n)Σ_i p_i and q̄ = (1/n)Σ_i q_i, and subtract them to get centered points p̃_i = p_i − p̄, q̃_i = q_i − q̄. This decouples rotation from translation: once both sets are centered, the optimal t is whatever aligns the centroids, so it drops out of the rotation problem entirely and we solve for R alone on the centered points.

Step 2 — form the cross-covariance. Accumulate the 3×3 matrix

H = Σ_i p̃_i q̃_i^⊤ (a 3×3 matrix, the sum of outer products) .

Step 3 — take the SVD. Factor H = UΣV^⊤.

Step 4 — read off the rotation. The optimal rotation is

R = V U^⊤ .

Step 5 — the reflection fix (do not skip). The formula V U^⊤ is only guaranteed to be orthogonal — it can come out with \det = −1, which is an improper rotation: a reflection, not a physical rotation. This happens with noisy or nearly-coplanar data. The fix flips the sign of the least-significant singular direction:

R = V · diag(1, 1, det(V U^⊤)) · U^⊤ ,

i.e. if \det(V U^⊤) < 0 we negate the third column, forcing \det R = +1 while giving up the least amount of fit.

Step 6 — recover the translation. Un-center: the translation is whatever maps the rotated source centroid onto the target centroid,

t = q̄ − R p̄ .

Why SVD solves it

Expand the centered objective: minimizing Σ_i‖q̃_i − R p̃_i‖² means minimizing Σ_i(‖q̃_i‖² + ‖p̃_i‖² − 2 q̃_i^⊤R p̃_i). The first two terms don't depend on R, so the problem is equivalent to maximizing Σ_i q̃_i^⊤R p̃_i = trace(R^⊤H) over rotations. Writing H = UΣV^⊤, one shows trace(R^⊤H) is maximized by R = V U^⊤. That maximizer is only constrained to be orthogonal; the \det term in Step 5 enforces the extra SO(3) constraint \det R = +1 that separates a rotation from a reflection — the piece that actually bites with noisy or coplanar data.

This closed form is the workhorse. It is fast, exact, and needs no initial guess — but only because the oracle gave us correspondences. In the field, no oracle exists. That is the problem of §3, and this six-step solver is precisely the subroutine that lives inside it.

3 · Unknown correspondences → Iterative Closest Point

Reality gives you two clouds and no correspondences. And here is the chicken-and-egg that defines the whole subject: to solve for the pose you need to know which points match, but to know which points match you need the pose — align the clouds first and corresponding points become the nearby ones. Each unknown supplies the other. Iterative Closest Point (ICP) breaks the circle by guessing one and refining, in the style of an EM alternation:

E-step — assign correspondences. With the current estimate of T, transform the source, and for each transformed source point find its nearest neighbor in the target. This is the kd-tree query from lesson 03 — O(\log m) per point instead of O(m), which is what makes ICP tractable on real clouds.
M-step — solve the pose. Treat those nearest-neighbor pairs as if they were true correspondences and solve the Procrustes problem of §2 in closed form to get an updated T.
Apply and repeat. Apply the update to the source and loop, stopping when the change in T (or the error) drops below tolerance.

Each iteration cannot increase the error, so ICP converges — but only to a local minimum. The basin it falls into is decided entirely by where it starts, which is the subject of §4.

Pose updates are done on-manifold: rather than perturbing the entries of R (which would leave SO(3)), each M-step's rotation is composed onto the running estimate, and incremental refinements are parameterized as twists retracted through the exp map — exactly lesson 02's recipe for staying on the manifold of valid transforms.

Point-to-point vs point-to-plane

The objective in §1 measures point-to-point distance: the straight-line gap between a source point and its matched target point. But a real surface is locally flat, and a source point rarely lands on the exact target sample — it lands somewhere on the same surface. Penalizing the full point-to-point distance then fights a motion that merely slides the point along the surface, which should be free. Point-to-plane ICP fixes this by measuring only the distance along the target normal n_i (the normals estimated in lesson 03):

min_R,t Σ_i ((R p_i + t − q_i) · n_i)² .

By projecting the residual onto the normal, tangential slip costs nothing — points are free to slide along the surface tangent to find their true match. On smooth surfaces this converges in far fewer iterations than point-to-point, and it is the practical default (KinectFusion and most real-time SLAM front-ends use it). The cost: it needs reliable normals, and the linearized system is solved per iteration rather than by a single SVD.

Variant	Residual per pair	Needs	Convergence	Typical use
Point-to-point	‖R p_i+t − q_i‖	points only	slower (many iters)	coarse / no normals
Point-to-plane	((R p_i+t − q_i)·n_i)	target normals	fast on smooth surfaces	practical default (KinectFusion)

The widget below runs point-to-point ICP in 2D so you can watch the alternation converge — and, by cranking the initial offset, watch it fail.

Interactive ICP in 2D

The green target is a fixed spiral; the blue source is that spiral rotated by the offset below, shifted, and jittered with noise. Press step to run one ICP iteration: each source point is matched to its nearest target point (thin gray lines), then a 2D Procrustes solve snaps the source onto the new estimate. From a small offset it converges to zero error in a few steps. Now set the offset near 180° and step: it locks into a wrong local minimum and the RMSE stalls on a high plateau — the §4 initialization trap, made visible.

initial rotation offset: 35°

Iteration

RMSE (nearest-nbr)

—

Estimated rotation

0°

Status

ready

Show the core JS

// ONE ICP iteration, point-to-point, in 2D.
// src = current source points; tgt = fixed target points.

// E-STEP: nearest target neighbour for each source point (brute force in 2D).
var pairs = src.map(function(p){
  var best = null, bd = Infinity;
  for (var j = 0; j < tgt.length; j++){
    var d = (p.x-tgt[j].x)*(p.x-tgt[j].x) + (p.y-tgt[j].y)*(p.y-tgt[j].y);
    if (d < bd){ bd = d; best = tgt[j]; }
  }
  return { p: p, q: best };
});

// M-STEP: 2D Procrustes closed form on the matched pairs.
var pm = centroid(pairs.map(function(m){return m.p;}));   // source centroid
var qm = centroid(pairs.map(function(m){return m.q;}));   // target centroid
var Sxy = 0, Sxx = 0;
pairs.forEach(function(m){
  var px = m.p.x - pm.x, py = m.p.y - pm.y;               // centered source
  var qx = m.q.x - qm.x, qy = m.q.y - qm.y;               // centered target
  Sxy += px*qy - py*qx;                                   // ∝ sin θ
  Sxx += px*qx + py*qy;                                   // ∝ cos θ
});
var theta = Math.atan2(Sxy, Sxx);                         // optimal rotation
var c = Math.cos(theta), s = Math.sin(theta);
// t = q̄ − R p̄
var tx = qm.x - (c*pm.x - s*pm.y);
var ty = qm.y - (s*pm.x + c*pm.y);

// apply R,t to every source point (snap onto the new estimate)
src = src.map(function(p){
  return { x: c*p.x - s*p.y + tx, y: s*p.x + c*p.y + ty };
});

4 · Failure modes and their fixes

ICP is a local optimizer, so its failures are the failures of local optimization on a non-convex landscape. Three matter, and each has a standard remedy.

(a) It needs a good initialization. ICP only has a local basin of attraction: it descends into whatever minimum it starts nearest. Give it a source rotated ~180° from the target and it will happily converge to a wrong alignment that locks distant points to spurious near neighbors, leaving a stubbornly high residual (the widget's plateau). ICP is a refiner, not a solver from scratch — it assumes you are already close.

The trap

Never treat ICP as a global aligner. Fed a large initial misalignment it does not "eventually find" the right pose — it confidently returns a wrong one with a plausible-looking (but non-zero) error. The fix is to supply a coarse initialization from a method that does search globally (below), then let ICP polish it.

(b) Partial overlap and outliers. Real scans overlap only partially, so many source points have no true match in the target — their nearest neighbor is garbage. A single distant false pair, squared, can dominate the least-squares sum and drag the whole solution off. Fixes: reject correspondences whose distance exceeds a threshold; use trimmed ICP, which keeps only the best-fitting fraction of pairs each iteration; or swap the squared loss for a robust (Huber) kernel that grows linearly, not quadratically, for large residuals so outliers stop dominating.

Why plain least-squares is fragile here

The squared residual weights a pair by the square of its distance, so one bad correspondence at distance 10d counts as much as a hundred good ones at distance d. With partial overlap you always have some non-matching points, so unrobustified point-to-point ICP is the wrong default on real data — trimming or a Huber kernel is not optional polish.

(c) Geometric symmetry and degeneracy. If the overlapping geometry is under-constrained, no algorithm can pin the pose. A flat plane can slide and spin within itself with zero error (translation in the plane and rotation about its normal are unobservable); a sphere can rotate arbitrarily; a long corridor is free to slide along its axis. These are not solver bugs — the data genuinely does not determine T. Recognizing degenerate geometry (e.g. via the conditioning of the linearized system) tells you the pose is only partially observable, and that you must lean on other sensors or constraints.

Global registration for the initialization

To supply the coarse pose that ICP refines, use a method that searches without an initial guess. The standard feature-based pipeline: compute local geometric feature descriptors — e.g. FPFH (Fast Point Feature Histograms), which summarize the local surface around keypoints using the normals from lesson 03 — match descriptors between the two clouds to get putative correspondences, then run RANSAC on minimal sets of 3 correspondences (three non-collinear pairs fix a rigid transform) to propose a coarse T, keeping the hypothesis with the most inliers. That coarse alignment lands inside ICP's basin, and ICP finishes the job. Global registration finds the basin; ICP finds the bottom.

The bridge to SLAM

Chaining pairwise alignments — scan 1 to 2, 2 to 3, 3 to 4 — makes each small registration error compound: the estimate drifts, so by the time a loop returns to its start the two ends no longer meet. The fix is to treat ICP as the front-end that produces relative-pose edges between scans, and hand those edges to a back-end — pose-graph optimization — that enforces global consistency. When the sensor revisits a place, a loop-closure edge ties the two visits together, and optimizing the graph distributes the accumulated error around the loop. This is exactly the SLAM structure of CV lesson 05: ICP is the local odometry, pose-graph optimization is the global map. Registration is one edge; SLAM is the graph.

Where this points next

We can now take the raw, partial, per-frame clouds of lesson 03 and fuse them into a single dense, consistent cloud in one coordinate frame — by solving Procrustes when matches are known, running ICP when they aren't, initializing it globally, and stitching many scans with a pose graph. But a dense point cloud is still just a bag of samples: it has no surface, no notion of "inside", no faces to render or to compute watertight volumes. The obvious next question is how to turn that fused cloud into an actual surface. Lesson 05 does exactly that, two ways: the explicit route (triangle meshes via Poisson reconstruction, using the normals again) and the implicit route (occupancy and signed-distance functions, the eikonal property, DeepSDF), with marching cubes as the bridge that extracts a mesh from an implicit field — bringing us back to the explicit/implicit dialogue that lesson 01 promised runs through the whole track.

Takeaway

Registration is SE(3) estimation from data: find T=(R,t) minimizing Σ_i‖q_i − (R p_i+t)‖². With known correspondences it is a one-shot closed form — center both clouds, take the SVD of the cross-covariance H=Σ p̃_iq̃_i^⊤, set R=V U^⊤ with the det fix R=V\,diag(1,1,\det VU^⊤)\,U^⊤ to forbid reflections, then t=q̄−R p̄. With unknown correspondences, ICP alternates nearest-neighbor matching (kd-tree) with that closed-form solve, converging to a local minimum; point-to-plane uses target normals to let points slide along the surface and converges far faster (the KinectFusion default), with pose updates retracted on-manifold via lesson 02's exp map. It fails on bad initialization, outliers/partial overlap (reject far pairs, trim, or use Huber), and degenerate geometry (planes, spheres). Global registration (FPFH + RANSAC on 3 pairs) supplies the coarse init; chaining alignments drifts, which pose-graph optimization with loop closure corrects — ICP as SLAM's front-end, the graph as its back-end.

Interview prompts

Given known point correspondences, derive the optimal rigid transform. (§2 — center both clouds, H=Σ p̃_iq̃_i^⊤, SVD H=UΣV^⊤, R=VU^⊤, t=q̄−Rp̄; minimizing the residual = maximizing trace(R^⊤H).)
Why do you need the determinant fix in the Procrustes solution, and when does it fire? (§2 — VU^⊤ is only orthogonal and can be a reflection (\det=−1); force \det=+1 via diag(1,1,\det VU^⊤). Bites with noisy or coplanar data.)
Explain ICP and the chicken-and-egg it resolves. (§3 — need the pose to match points and the matches to solve the pose; alternate nearest-neighbor assignment (E) with the closed-form solve (M); converges to a local minimum.)
Point-to-point vs point-to-plane ICP — which and why? (§3 — point-to-plane projects the residual onto the target normal so points slide along the surface tangent; far fewer iterations on smooth surfaces; the KinectFusion default, but needs normals.)
ICP returns a bad alignment on two scans that overlap. What are the likely causes and fixes? (§4 — bad initialization (global registration first), outliers/partial overlap (reject far pairs, trimmed ICP, Huber), or degenerate geometry (plane/sphere — unobservable).)
How do you get a coarse initial pose without any guess, and what happens when you chain many pairwise alignments? (§4 — FPFH descriptors + RANSAC on 3 correspondences for the init; chaining drifts, so pose-graph optimization + loop closure enforces global consistency — ICP is the front-end.)