Registration finds the rigid transform that puts one scan into another’s coordinate frame. Iterative closest point solves it by alternating two steps, each of which is easy, given the answer to the other. That circularity is the entire character of the algorithm: the transform step has a closed-form optimal solution, and the correspondence step is a guess.
The problem, stated precisely
Given a source cloud P and a target cloud Q that overlap, find a rotation R and a translation t minimising the distance between corresponding points. Three things are worth pinning down before any algorithm appears.
It is a six-degree-of-freedom problem — three rotation, three translation — not seven. Scale is not among the unknowns, because two scans of the same building from the same instrument are the same size. If your two clouds are at different scales, ICP will not tell you; it will return the rigid transform that best compromises, and the residual will be small enough to look like noise. Fix scale before registering, not during.
There is also no correspondence given. If you knew which point in P matched which point in Q, the problem would be solved in one step by the arithmetic below. You do not, and in general no exact correspondence exists at all: two scans of a wall from different positions sample different physical points on it, so the best available correspondence is “nearest point on the same surface”, which is an approximation whose error depends on the point spacing.
The ICP loop
- For each point in the source, find the closest point in the target. With a k-d tree over the target this is O(log n) per query, so the step is O(n log n) and is usually where the time goes.
- Reject bad correspondences: pairs further apart than a distance threshold, pairs whose normals disagree beyond an angle threshold, and pairs where the target point lies on the boundary of the scan (a boundary point is almost always matched to something outside the overlap).
- Solve for the R and t that minimise the sum of squared distances between the surviving pairs. This has a closed form.
- Apply the transform to the source, measure the change in mean squared error, and stop when it falls below a tolerance or the iteration cap is reached.
Paul Besl and Neil McKay, who described the algorithm in IEEE Transactions on Pattern Analysis and Machine Intelligence in 1992, proved that the mean squared error is non-increasing across iterations and therefore that the loop converges. What they did not prove, because it is not true, is that it converges to the right answer. It converges to a local minimum, and which one depends entirely on where it started.
Worked: solving one alignment step
Take three source points and their known correspondences in the target. The target is the source rotated 90° about the z axis and translated by (1, 0, 0), but pretend you do not know that — the arithmetic has to recover it.
source P target Q
p1 = (0, 0, 0) q1 = ( 1, 0, 0)
p2 = (2, 0, 0) q2 = ( 1, 2, 0)
p3 = (0, 2, 0) q3 = (-1, 0, 0)
step 1 — centroids
p_bar = (2/3, 2/3, 0)
q_bar = (1/3, 2/3, 0)
step 2 — centre both sets
p1' = (-2/3, -2/3, 0) q1' = ( 2/3, -2/3, 0)
p2' = ( 4/3, -2/3, 0) q2' = ( 2/3, 4/3, 0)
p3' = (-2/3, 4/3, 0) q3' = (-4/3, -2/3, 0)
step 3 — cross-covariance H = sum of p' q'^T
(taking only the x,y block; z is all zero)
from p1': [[-4/9, 4/9], [-4/9, 4/9]]
from p2': [[ 8/9, 16/9], [-4/9, -8/9]]
from p3': [[ 8/9, 4/9], [-16/9, -8/9]]
H = [[ 12/9, 24/9],
[-24/9, -12/9]] = [[ 4/3, 8/3],
[-8/3, -4/3]]
step 4 — SVD of H, then R = V U^T
this H has the singular value decomposition of a scaled
90-degree rotation, and V U^T comes out as
R = [[0, -1, 0],
[1, 0, 0],
[0, 0, 1]]
step 5 — translation
t = q_bar - R p_bar
R p_bar = R (2/3, 2/3, 0) = (-2/3, 2/3, 0)
t = (1/3, 2/3, 0) - (-2/3, 2/3, 0) = (1, 0, 0)
The recovered transform is exactly the one used to build the target. This closed-form solution is due to K. S. Arun, T. S. Huang and S. D. Blostein in 1987 and is what step 3 of the loop actually runs.
One detail almost every summary omits and every implementation includes. If the point sets are degenerate — coplanar, or reflected by noise — V U^T can come out with determinant −1, which is a reflection rather than a rotation. The correction is to compute R = V diag(1, 1, det(V U^T)) U^T, which flips the sign of the last column and forces a proper rotation. Skip it and you will occasionally get a mirrored scan that fits the residual beautifully and is physically impossible.
The correspondence step is the hard one
Everything above assumed the pairs were correct. In the real loop they come from nearest-neighbour search on the current estimate, which means a bad estimate produces bad pairs, which produce a bad update.
The most common consequence is a specific failure mode worth recognising: two scans of a long corridor slide along it. Every correspondence is between two points on the same flat wall, the residual is genuinely near zero at any offset along the corridor, and ICP happily reports convergence at the wrong position. The same happens on a cylinder about its axis and on a plane in both of its directions. The geometry itself is unobservable in those directions; no amount of iteration fixes it, and the tell is a residual that is low while the transform keeps drifting between runs.
The other systematic error is partial overlap. If 40% of the source has no counterpart in the target, those points still get a nearest neighbour — the closest thing on the edge of the target — and they pull the solution toward it. Distance-based rejection and trimmed ICP, which uses only the best-fitting fraction of pairs each iteration, exist for exactly this. Setting the trim fraction near the true overlap is the single most effective parameter change available when a registration keeps landing slightly off.
Point-to-plane, and why it converges faster
Yang Chen and Gérard Medioni proposed in 1991 minimising a different quantity: not the distance between paired points, but the distance from the source point to the plane through the target point, defined by the target’s normal. The residual becomes ((R p + t − q) · n_q) squared.
The effect is that a source point is free to slide within the target surface at no cost, and is only penalised for being off it. On the corridor above that sounds worse; in practice, on scans of built environments, it converges in far fewer iterations, because the point-to-point metric wastes effort trying to make arbitrarily paired points on the same wall coincide when they never can.
The price is that there is no closed-form solution. The standard approach, described by Kok-Lim Low in 2004, linearises the rotation with a small-angle approximation, which turns the problem into a 6×6 linear system that can be solved directly. That approximation is only valid for small rotations — meaning point-to-plane needs a reasonable initial guess even more than point-to-point does, and needs normals on the target, which is an extra estimation step with its own radius parameter.
Generalized ICP, from Aleksandr Segal, Dirk Haehnel and Sebastian Thrun in 2009, unifies both: it models each point as a Gaussian with a covariance flattened along the local surface, so point-to-point and point-to-plane fall out as special cases and mixed geometry is handled without choosing.
Getting the initial guess
ICP is a local refiner and should be treated as one. Something else has to put the two clouds roughly in place first, and there are four practical sources.
- Known pose. A scanner with GNSS and an IMU, a robot with odometry, or a turntable with an encoder gives you an initial transform directly. This is the cheapest and most reliable option and it is why SLAM front ends feed a motion prediction into scan matching rather than starting cold.
- Feature-based coarse alignment. Compute a local descriptor at keypoints in both clouds — Fast Point Feature Histograms, from Radu Rusu and colleagues in 2009, is the classical choice — match them, and run RANSAC over the matches to find a transform supported by a consistent subset. The output is crude and is exactly what ICP wants.
- Targets. Survey practice places spheres or checkerboards visible from multiple setups. Three shared targets fix the transform outright, with no search at all, and the residual on the targets is a direct measurement of registration quality rather than an inferred one.
- Global methods. Go-ICP branch-and-bounds the whole rotation space for a certified global optimum, and TEASER++ solves the correspondence-and-transform problem with certifiable robustness to very high outlier rates. Both are slower than ICP by orders of magnitude and are worth it when the alternative is a human dragging clouds around in a viewer.
Finally, report the right number. Mean squared residual on the surviving correspondences measures how well the pairs agree, not how well the scans agree — a registration that used 8% of the points can have a beautiful residual. Report the residual alongside the overlap fraction and the count of correspondences used, or the number means nothing.
Top comments (0)