ICP & point-cloud registration
Two LiDAR scans of the same place, a few hundred thousand points each, and no correspondences: nothing tells you which point in one scan is the same physical surface as which point in the other. Iterative Closest Point breaks that deadlock with a bold guess — assume the nearest point is the matching point, solve the resulting pose problem exactly, and repeat. This page derives the closed-form solve, shows why point-to-plane converges an order of magnitude faster, and is honest about the local minimum that is ICP's defining weakness.
Symbols used on this page are defined in the Nomenclature → optimisation & robust fitting.
Sensor characteristics are on the LiDAR page; the rotation machinery used throughout is derived in Attitude & rotations.
1 · The problem
1.1 Statement
Given a source cloud $\mathcal{P} = \{\mathbf p_i\}_{i=1}^{N_p}$ and a target cloud $\mathcal{Q} = \{\mathbf q_j\}_{j=1}^{N_q}$, find the rigid transform $(\mathbf R, \mathbf t)$ with $\mathbf R \in SO(3)$ minimising the distance between corresponding points:
$$ \min_{\mathbf R,\,\mathbf t,\,\pi} \; \sum_{i} w_i \left\| \mathbf R\,\mathbf p_i + \mathbf t - \mathbf q_{\pi(i)} \right\|^2 , $$where $\pi$ is the unknown assignment from source indices to target indices and $w_i$ are weights. The difficulty is entirely in $\pi$: with $\pi$ known this is the classic orthogonal Procrustes problem with an exact solution (§2), but $\pi$ ranges over a combinatorial set.
| Symbol | Meaning | Notes |
|---|---|---|
| $\mathbf p_i$ | point in the source ("reading") cloud | the cloud being moved |
| $\mathbf q_j$ | point in the target ("reference") cloud | held fixed |
| $\mathbf n_j$ | surface normal at $\mathbf q_j$ | needed for point-to-plane |
| $\mathbf R, \mathbf t$ | rotation and translation being estimated | 6 DoF total |
| $\pi(i)$ | index of the target point matched to $\mathbf p_i$ | re-estimated every iteration |
| $w_i$ | per-correspondence weight | 0 rejects the pair outright |
| $\mathbf H$ | $3\times3$ cross-covariance of the centred clouds | the matrix that gets the SVD |
| $\bar{\mathbf p}, \bar{\mathbf q}$ | centroids of the matched subsets | recomputed each iteration |
1.2 The loop
Step 2 is the expensive one and step 4 is the interesting one. Note that the cost is evaluated on a different correspondence set every iteration, so the sequence of cost values is not guaranteed monotone in the usual sense — what is guaranteed is that each of steps 2 and 4 individually cannot increase the cost for its own objective, which is enough for convergence.
2 · The closed-form pose solve
2.1 Eliminating the translation
With correspondences fixed, write the objective and set the gradient with respect to $\mathbf t$ to zero:
$$ \frac{\partial}{\partial \mathbf t}\sum_i \left\|\mathbf R \mathbf p_i + \mathbf t - \mathbf q_i\right\|^2 = 2\sum_i \left(\mathbf R \mathbf p_i + \mathbf t - \mathbf q_i\right) = \mathbf 0 , $$ $$ \Longrightarrow \quad \mathbf t^\star = \bar{\mathbf q} - \mathbf R\,\bar{\mathbf p} . $$The optimal translation always maps one centroid onto the other. Substituting back, and defining centred coordinates $\tilde{\mathbf p}_i = \mathbf p_i - \bar{\mathbf p}$ and $\tilde{\mathbf q}_i = \mathbf q_i - \bar{\mathbf q}$, the translation disappears entirely:
$$ \min_{\mathbf R} \sum_i \left\| \mathbf R\,\tilde{\mathbf p}_i - \tilde{\mathbf q}_i \right\|^2 . $$This is the decoupling that makes ICP cheap: a 6-DoF problem becomes a 3-DoF rotation problem plus an algebraic translation.
2.2 The rotation by SVD
Expanding the norm and dropping terms independent of $\mathbf R$ (using $\|\mathbf R\tilde{\mathbf p}\|^2 = \|\tilde{\mathbf p}\|^2$ since $\mathbf R$ is orthogonal):
$$ \sum_i \left\|\mathbf R\tilde{\mathbf p}_i - \tilde{\mathbf q}_i\right\|^2 = \text{const} - 2\sum_i \tilde{\mathbf q}_i^\top \mathbf R\,\tilde{\mathbf p}_i , $$so minimising the cost is maximising $\sum_i \tilde{\mathbf q}_i^\top \mathbf R \tilde{\mathbf p}_i = \operatorname{tr}\!\left(\mathbf R^\top \mathbf H\right)$ where
$$ \mathbf H = \sum_i w_i\,\tilde{\mathbf q}_i\,\tilde{\mathbf p}_i^\top \in \mathbb{R}^{3\times3}. $$Taking the SVD $\mathbf H = \mathbf U \boldsymbol\Sigma \mathbf V^\top$, the trace is maximised by
$$ \boxed{\;\mathbf R^\star = \mathbf U \, \operatorname{diag}\!\left(1,\,1,\,\det(\mathbf U\mathbf V^\top)\right)\, \mathbf V^\top \;} $$The whole solve is therefore: two centroids, one $3\times3$ accumulation, one $3\times3$ SVD. It is $O(N)$ in the correspondences with a tiny constant — which is why the matching step, not the solve, dominates ICP's runtime.
This is the same mathematics as Wahba's problem in attitude determination, where the vectors are star directions rather than surface points. Horn's quaternion method and Arun's SVD method are two solutions to the identical problem; the SVD form is preferred here because the determinant fix is explicit.
3 · Point-to-plane, and why it wins
3.1 The objective
Point-to-point ICP has a structural flaw: it asks each source point to land exactly on a target point. But the target points are just samples of a surface, and the true corresponding location almost certainly lies between them. Penalising the tangential offset fights the sliding motion that would actually align the surfaces. Chen and Medioni's fix is to penalise only the component along the surface normal:
$$ \min_{\mathbf R,\mathbf t} \sum_i w_i \left[ \mathbf n_i^\top \left( \mathbf R\,\mathbf p_i + \mathbf t - \mathbf q_i \right) \right]^2 . $$Each correspondence now contributes one scalar equation instead of three, constraining motion only perpendicular to the local surface and leaving the cloud free to slide along it — which is precisely the freedom needed to converge.
3.2 Linearising for small angles
Unlike point-to-point, this has no closed-form solution. The standard treatment (Low, 2004) linearises the rotation for small angles, $\mathbf R \approx \mathbf I + [\boldsymbol\alpha]_\times$ with $\boldsymbol\alpha = (\alpha, \beta, \gamma)^\top$. The residual becomes
$$ r_i \approx \mathbf n_i^\top\left(\mathbf p_i - \mathbf q_i\right) + \left(\mathbf p_i \times \mathbf n_i\right)^\top \boldsymbol\alpha + \mathbf n_i^\top \mathbf t , $$which is linear in the six unknowns $\mathbf x = (\boldsymbol\alpha^\top, \mathbf t^\top)^\top$. Stacking one row per correspondence,
$$ \mathbf a_i = \begin{bmatrix} \mathbf p_i \times \mathbf n_i \\ \mathbf n_i \end{bmatrix} \in \mathbb{R}^{6}, \qquad b_i = -\,\mathbf n_i^\top(\mathbf p_i - \mathbf q_i), $$gives a $6\times6$ normal-equation system solved by Cholesky:
$$ \left(\sum_i w_i\,\mathbf a_i \mathbf a_i^\top\right)\mathbf x = \sum_i w_i\,b_i\,\mathbf a_i . $$The accumulation is a single pass, trivially parallel, and the $6\times6$ solve is negligible. Because the linearisation is only valid for small angles, this must be iterated — but that is what ICP does anyway.
3.3 The comparison
| Point-to-point | Point-to-plane | |
|---|---|---|
| Solution | closed form (SVD) | linearised, iterated |
| Equations per pair | 3 | 1 |
| Needs normals | no | yes |
| Convergence rate | slow, roughly linear | much faster, near quadratic close in |
| Typical iterations | 30–100+ | 5–15 |
| Sliding along surfaces | penalised — fights alignment | free — enables alignment |
| Behaviour on flat regions | poor | excellent |
| Sensitivity to normal noise | n/a | moderate — bad normals hurt |
| Large initial error | more forgiving | linearisation degrades |
3.4 Estimating normals
Point-to-plane needs $\mathbf n_j$, which the sensor does not provide. The standard estimator takes the $k$ nearest neighbours of $\mathbf q_j$, forms the local covariance
$$ \mathbf C_j = \frac{1}{k}\sum_{m \in \mathcal{N}_k(j)} \left(\mathbf q_m - \bar{\mathbf q}_{\mathcal N}\right) \left(\mathbf q_m - \bar{\mathbf q}_{\mathcal N}\right)^\top , $$and takes $\mathbf n_j$ as the eigenvector of the smallest eigenvalue — the direction of least variance, i.e. perpendicular to the locally fitted plane. The eigenvalues also give a free quality metric: if $\lambda_0 \ll \lambda_1 \le \lambda_2$ the neighbourhood is genuinely planar and the normal is trustworthy; if $\lambda_0 \approx \lambda_1 \approx \lambda_2$ the neighbourhood is a noise ball and the normal is meaningless. Use that ratio to weight or reject the correspondence. The neighbourhood radius is the parameter that matters: too small and normals are noise-dominated, too large and real edges get smoothed into nonsense.
4 · Finding correspondences efficiently
4.1 The cost problem
Brute-force matching is $O(N_p N_q)$. For two 100k-point scans that is $10^{10}$ distance evaluations per iteration — hopeless. Every practical implementation uses a spatial index:
| Structure | Build | Query | Notes |
|---|---|---|---|
| k-d tree | $O(N\log N)$ | $O(\log N)$ average | The default. Degrades toward $O(N)$ in high dimensions, but 3-D is fine |
| Voxel / hash grid | $O(N)$ | $O(1)$ expected | Fastest when density is roughly uniform; trivially parallel; great on GPU |
| Octree | $O(N\log N)$ | $O(\log N)$ | Handles non-uniform density better than a flat grid |
| Projective / image-space | free | $O(1)$ | For depth images: project the source point into the target's frame and read off the pixel. Approximate, but the basis of real-time RGB-D tracking |
| Precomputed distance field | $O(V)$ | $O(1)$ | Build once per target; a lookup replaces the search entirely |
A crucial asymmetry: the tree is built on the target, which does not move, so it is built once and queried every iteration. Building it on the source would mean rebuilding each iteration.
4.2 Subsampling
Using every point is rarely necessary. Options, roughly in order of sophistication:
- Uniform random. Cheap, unbiased; re-randomising each iteration acts as a mild regulariser.
- Voxel-grid downsample. One representative per voxel. Equalises density, which matters because raw LiDAR is far denser near the sensor — without this, nearby points dominate the objective purely because there are more of them.
- Normal-space sampling. Sample to cover the distribution of normal directions rather than space. This is the important one for sparsely featured scenes: in a long corridor, spatial sampling returns mostly wall points that cannot constrain motion along the corridor, whereas normal-space sampling deliberately keeps the rare floor and end-wall points that do.
- Feature/curvature-based. Keep edges and corners, discard flat interiors — the strategy behind LOAM-style odometry, which separates edge and planar features and uses different residuals for each.
5 · Robustness: rejection and weighting
5.1 Why it is needed
The nearest-neighbour assumption manufactures wrong correspondences by construction. Partial overlap is the main source: points visible in one scan but occluded in the other still get assigned some nearest neighbour, and that pair is pure error. Since the closed-form solve is least squares, those pairs have unbounded influence (see RANSAC §1).
| Strategy | Rule | Comment |
|---|---|---|
| Max distance | reject if $d_i \gt d_{\max}$ | Essential. Shrink $d_{\max}$ as iterations proceed — coarse-to-fine |
| Trimmed ICP | keep the best $\lfloor \xi N \rfloor$ pairs by residual | Set $\xi$ to the expected overlap. Principled and effective |
| Normal compatibility | reject if $\mathbf n_p^\top \mathbf n_q \lt \cos\theta_{\max}$ | Cheap and catches matches across a fold or onto a facing surface |
| Robust kernel | Huber / Cauchy / Tukey on the residual | Smooth alternative to hard rejection; needs a scale estimate |
| Sigma-based | reject beyond $k\hat\sigma$ with $\hat\sigma$ from the MAD | Adapts automatically as alignment improves |
| Reciprocity | keep only mutual nearest neighbours | Doubles query cost; excellent at removing many-to-one artefacts |
| Boundary rejection | drop pairs whose target point is on a scan edge | Targets the partial-overlap failure directly |
A many-to-one pathology deserves naming: nothing in nearest-neighbour matching prevents fifty source points from all claiming the same target point. That single target then carries fifty times the weight it deserves and can dominate the solve. Reciprocity checks or one-to-one enforcement fix it.
5.2 The sim
The simulation runs point-to-point ICP on two scans of an L-shaped wall. Three experiments are worth running deliberately:
- Increase the initial misalignment past roughly 40°. Convergence does not merely slow down — it goes to the wrong answer, snapping the short arm of the L onto the long one. That is the local minimum, and no iteration count escapes it.
- Raise the outlier fraction with trimming at zero. The residual plateaus and the final alignment is visibly biased, because the outliers pull the centroid.
- Now raise trimming. The same outlier fraction becomes harmless.
6 · Convergence and the basin of attraction
6.1 What is guaranteed
Besl and McKay's convergence proof is often over-read. What it establishes is that the cost sequence is non-increasing and bounded below, hence convergent. What it does not establish is that the limit is the global minimum, or that it is anywhere near the true transform. ICP converges to a local minimum of a non-convex cost, and the initial guess selects which one.
6.2 Where the local minima come from
- Symmetry and repetition. A corridor, a row of identical columns, a cylinder — geometry that is self-similar under some motion admits many equally good alignments. No local method can resolve this; the information is genuinely absent.
- Wrong-arm snapping. Two nearly parallel surfaces at different offsets. Rotate far enough and every nearest neighbour is on the wrong surface, so the solve confidently reinforces the error.
- Partial overlap. The minimum of the cost is not at the true pose when a large fraction of points have no true correspondence, because the spurious pairs contribute a systematic pull.
- Sparse constraint directions. Even with correct correspondences, if all normals are nearly parallel the $6\times6$ system is ill-conditioned and motion along the unconstrained direction is determined by noise.
6.3 Diagnosing it honestly
The residual is a poor detector of misregistration: a confidently wrong alignment can have a low RMS residual, because the wrong surface fits just as well as the right one. Better checks:
- Condition number of the normal-equation matrix $\sum_i \mathbf a_i \mathbf a_i^\top$. A large ratio of largest to smallest eigenvalue means some DoF is unconstrained. This is the direct analogue of GDOP in GNSS and of the observability analysis on the PnP page — and it is available for free, since the matrix is already formed.
- Overlap fraction. The share of source points with an accepted correspondence. Low overlap with low residual is the signature of a partial-overlap failure.
- Restart from perturbed initial guesses. If several restarts agree, the minimum is probably global; if they disagree, the problem is genuinely ambiguous and the system should say so.
6.4 Getting a good initial guess
Since the basin of attraction is the binding constraint, most engineering effort goes into initialisation rather than into ICP itself:
| Source | Typical accuracy | Notes |
|---|---|---|
| Odometry / IMU integration | excellent over one scan interval | The standard answer in a moving vehicle. See IMU |
| Constant-velocity extrapolation | good at high scan rate | Free, and usually enough between consecutive scans |
| 3-D feature matching + RANSAC | coarse but global | FPFH descriptors with RANSAC or TEASER++; the answer for loop closure |
| Branch and bound | globally optimal | Go-ICP: guarantees the global minimum, but far too slow for a real-time loop |
| Coarse-to-fine multi-resolution | widens the basin | Align heavily downsampled clouds first; cheap and very effective |
7 · Generalisations
7.1 Generalised ICP
GICP (Segal, Hähnel & Thrun, 2009) observes that point-to-point and point-to-plane are both special cases of a probabilistic formulation. Model each point as a Gaussian with covariance $\mathbf C_i$ describing the local surface, and the residual's covariance under the transform is
$$ \boldsymbol\Sigma_i = \mathbf C^{q}_i + \mathbf R\,\mathbf C^{p}_i\,\mathbf R^\top , $$giving the Mahalanobis objective
$$ \min_{\mathbf R,\mathbf t}\sum_i \mathbf d_i^\top \boldsymbol\Sigma_i^{-1} \mathbf d_i , \qquad \mathbf d_i = \mathbf R\mathbf p_i + \mathbf t - \mathbf q_i . $$Choosing $\mathbf C = \mathbf I$ recovers point-to-point; choosing a covariance that is large in the tangent plane and tiny along the normal recovers point-to-plane. GICP instead uses the measured local covariances from the eigen-analysis of §3.4 — "plane-to-plane" — which handles surfaces of mixed character in one framework and is measurably more robust to the neighbourhood-radius choice.
7.2 NDT
The Normal Distributions Transform takes a different route: voxelise the target once and store a Gaussian $(\boldsymbol\mu_v, \boldsymbol\Sigma_v)$ per voxel. Registration then maximises the likelihood of the transformed source points under that piecewise-Gaussian field, with no nearest-neighbour search at all — just a voxel lookup. Because the objective is smooth and analytically differentiable, Newton-type optimisation applies directly.
| ICP | NDT | |
|---|---|---|
| Target representation | raw points + k-d tree | voxel grid of Gaussians |
| Per-iteration cost | $O(N\log N)$ (search) | $O(N)$ (lookup) |
| Memory | all points | compact; scales with volume, not point count |
| Objective | piecewise, non-smooth | smooth and differentiable |
| Key parameter | rejection distance | voxel size — dominates behaviour |
| Basin of attraction | narrow | wider (voxels smooth the cost) |
| Very sparse clouds | fine | poor — voxels get too few points |
7.3 The wider family
- Colored / semantic ICP. Add photometric or class terms so a red point cannot match a blue one. Effectively extra constraint dimensions, which shrink the space of plausible wrong minima.
- Continuous-time / deskewed ICP. A spinning LiDAR samples over ~100 ms, during which the vehicle moves — so a "scan" is not a rigid snapshot. Model the trajectory within the sweep and correct each point by its own timestamp. Skipping this is a large, systematic error at vehicle speeds, and it is the same time-of-validity discipline the IMU page insists on.
- Symmetric ICP. Treat source and target symmetrically to remove the bias introduced by choosing which cloud carries the normals.
- Certifiable global registration. TEASER++ and related work give provable optimality under bounded outliers by truncated least squares and graph-theoretic inlier selection.
- Learned front ends. Deep descriptors improve the correspondence step, which is where classical ICP is weakest, and then hand the result to the same closed-form solve.
8 · Practical checklist
- Force $\det\mathbf R = +1$. The single most common ICP bug (§2.2).
- Voxel-downsample before matching to equalise density; raw LiDAR over-weights nearby surfaces simply by sampling them more.
- Build the k-d tree on the target once, outside the loop.
- Use point-to-plane wherever normals are usable, and check normal quality from the eigenvalue ratio.
- Always set a maximum correspondence distance, and shrink it across iterations.
- Trim to the expected overlap. If you know only 70% overlaps, do not fit 100% of the pairs.
- Deskew the scan using per-point timestamps if the sensor spins and the platform moves.
- Report the condition number, the overlap fraction and the iteration count alongside the pose. A pose without a health metric cannot be safely consumed downstream.
- Cap the iterations and treat hitting the cap as a failure signal, not a result.
- Test the degenerate scenes deliberately — a flat wall, a long corridor, a single plane. These are where a registration front end quietly produces confident nonsense.
9 · Further reading
- Besl & McKay (1992), "A Method for Registration of 3-D Shapes," IEEE TPAMI 14(2). The ICP paper and the convergence proof.
- Chen & Medioni (1992), "Object Modelling by Registration of Multiple Range Images." The point-to-plane formulation, published essentially simultaneously.
- Arun, Huang & Blostein (1987), "Least-Squares Fitting of Two 3-D Point Sets." The SVD solution, including the reflection case.
- Horn (1987), "Closed-form solution of absolute orientation using unit quaternions." The quaternion route to the same answer.
- Rusinkiewicz & Levoy (2001), "Efficient Variants of the ICP Algorithm," 3DIM. The systematic comparison of sampling, matching, weighting and rejection — including normal-space sampling. Still the most useful single paper for an implementer.
- Low (2004), "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration." The small-angle linearisation in full detail.
- Chetverikov et al. (2002), "The Trimmed Iterative Closest Point Algorithm."
- Segal, Hähnel & Thrun (2009), "Generalized-ICP," RSS.
- Magnusson (2009), "The Three-Dimensional Normal-Distributions Transform." The definitive NDT treatment.
- Yang et al. (2016), "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally."
- Yang, Shi & Carlone (2021), "TEASER: Fast and Certifiable Point Cloud Registration," IEEE T-RO.
- Zhang & Singh (2014), "LOAM: Lidar Odometry and Mapping in Real-time," RSS. The edge/planar feature split that shaped a decade of LiDAR odometry.
Related pages: RANSAC · GraphSLAM & pose graphs · LiDAR hardware · Attitude & rotations · Star trackers (Wahba)