Iterative Closest Point (ICP)

Iterative closest point (ICP) aligns two point clouds of the same rigid object or scene by finding the rotation and translation that bring one onto the other. It needs no matched features: it guesses correspondences by pairing each point with the nearest point of the other cloud, solves for the rigid motion that best fits those pairs, applies it, and repeats. Given a rough initial alignment, ICP is the standard tool for registering range scans into a model, estimating the motion between LiDAR scans, and tracking a depth camera.

Problem

Given a source cloud $P = \{\mathbf{p}_i\}$ and a target cloud $Q = \{\mathbf{q}_j\}$ sampled from overlapping parts of the same surface, find a rotation $R$ and translation $\mathbf{t}$ such that $R\mathbf{p}_i + \mathbf{t}$ lies on the surface sampled by $Q$. If the correspondences were known, so that $\mathbf{p}_i$ matched $\mathbf{q}_i$, this would be the absolute orientation problem, a least-squares fit with a closed-form solution. In registration they are not known: the clouds are independent samplings with different densities, partial overlap, noise, and points that have no counterpart.

Inputs and Outputs

• Inputs: the source and target clouds; an initial estimate $(R_0, \mathbf{t}_0)$, often the identity or a prediction from odometry; for the point-to-plane variant, surface normals of the target; and a stopping rule.
• Outputs: the rigid transform $(R, \mathbf{t})$, the final correspondences, and the residual error.

Intuition

If the clouds are already nearly aligned, the nearest target point to each source point is close to its true counterpart. Fitting a rigid motion to these approximate pairs moves the source closer, which improves the next round of nearest neighbors. Far from alignment the nearest neighbors are mostly wrong, and the iteration can settle in a wrong but self-consistent pose. ICP is therefore a local refinement method, not a search.

Algorithm

Starting from $(R, \mathbf{t}) = (R_0, \mathbf{t}_0)$:

1. Transform the source: $\mathbf{p}'_i = R\mathbf{p}_i + \mathbf{t}$.
2. Match: for each $\mathbf{p}'_i$, find the closest target point $\mathbf{q}_{j(i)}$, using a k-d tree over $Q$ built once.
3. Reject implausible pairs, for example those farther apart than a distance threshold.
4. Minimize the error metric over the remaining pairs to get an increment $(\Delta R, \Delta\mathbf{t})$.
5. Update $R \leftarrow \Delta R\, R$ and $\mathbf{t} \leftarrow \Delta R\, \mathbf{t} + \Delta\mathbf{t}$.
6. Stop when the error stops decreasing by more than a tolerance, or after a maximum number of iterations; otherwise return to step 1.

Besl and McKay (1992) stated the algorithm in this form, with closest points computed against points, lines, or triangles of a model. In the same period, Chen and Medioni (1992) developed a registration method that minimizes the distance from points to tangent planes of the other surface, the basis of point-to-plane ICP.

Mathematical Formulation

Point-to-point: the closed-form rigid fit

With pairs $(\mathbf{p}_i, \mathbf{q}_i)$, $i = 1, \dots, N$, the point-to-point step minimizes

$$E(R, \mathbf{t}) = \sum_{i=1}^{N} \lVert R\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \rVert^2 .$$

Setting the derivative with respect to $\mathbf{t}$ to zero gives $\mathbf{t} = \bar{\mathbf{q}} - R\bar{\mathbf{p}}$, where $\bar{\mathbf{p}}$ and $\bar{\mathbf{q}}$ are the centroids, so the translation follows once the rotation is known. With centered points $\tilde{\mathbf{p}}_i = \mathbf{p}_i - \bar{\mathbf{p}}$ and $\tilde{\mathbf{q}}_i = \mathbf{q}_i - \bar{\mathbf{q}}$, Arun, Huang and Blostein (1987) form the $3 \times 3$ matrix

$$H = \sum_{i=1}^{N} \tilde{\mathbf{p}}_i \tilde{\mathbf{q}}_i^\top = U \Sigma V^\top$$

and take $R = VU^\top$. That product is orthogonal but can have determinant $-1$, a reflection. Arun et al. show that without noise this happens only for degenerate (coplanar or collinear) points; for coplanar points, flipping the sign of the last column of $V$ yields the rotation; with very large noise it can occur for non-planar data too. Umeyama (1991) gave the strict solution, which always returns a rotation:

$$R = V \operatorname{diag}\!\left(1,\, 1,\, \det(VU^\top)\right) U^\top .$$

Horn (1987) derived a closed form with unit quaternions: the optimal rotation is the eigenvector of a symmetric $4 \times 4$ matrix built from the same sums of products, for its most positive eigenvalue. Horn and Umeyama also estimate a scale factor. A quaternion represents only rotations, which is why Besl and McKay preferred it for 3D registration.

Convergence of point-to-point ICP

Besl and McKay proved that point-to-point ICP, with no pairs rejected, converges monotonically to a local minimum of the mean squared distance. The least-squares fit cannot increase the error of the current pairs, since the previous pose is among the candidates; re-matching cannot increase any individual distance, since each point’s new closest point is at least as close as its old partner. The error is therefore non-increasing and bounded below.

Point-to-plane

Neighboring scans rarely sample the same surface points, so even at the correct pose a source point’s nearest target point is offset along the surface. Point-to-point ICP penalizes that offset; the point-to-plane metric measures only the distance along the target normal $\mathbf{n}_i$:

$$E(R, \mathbf{t}) = \sum_{i=1}^{N} \left( \mathbf{n}_i^\top (R\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i) \right)^2 .$$

Points may then slide along the surface at no cost. The metric has no closed-form minimizer. Rusinkiewicz and Levoy (2001) note two options: a general nonlinear least-squares solver such as Levenberg–Marquardt, or linearizing on the assumption that the incremental rotation is small. Low (2004) details the second. Writing $R \approx I + [\boldsymbol{\omega}]_\times$ for a small rotation vector $\boldsymbol{\omega}$, each residual becomes linear in $(\boldsymbol{\omega}, \mathbf{t})$:

$$\mathbf{n}_i^\top (\mathbf{p}_i + \boldsymbol{\omega} \times \mathbf{p}_i + \mathbf{t} - \mathbf{q}_i) = (\mathbf{p}_i \times \mathbf{n}_i)^\top \boldsymbol{\omega} + \mathbf{n}_i^\top \mathbf{t} - \mathbf{n}_i^\top (\mathbf{q}_i - \mathbf{p}_i).$$

Stacking the rows $[(\mathbf{p}_i \times \mathbf{n}_i)^\top,\ \mathbf{n}_i^\top]$ gives an $N \times 6$ linear least-squares problem $A\mathbf{x} \approx \mathbf{b}$ in $\mathbf{x} = (\boldsymbol{\omega}, \mathbf{t})$, with $b_i = \mathbf{n}_i^\top(\mathbf{q}_i - \mathbf{p}_i)$, solved through the $6 \times 6$ normal equations $A^\top A\,\mathbf{x} = A^\top \mathbf{b}$. For fixed correspondences this is a Gauss–Newton step on the point-to-plane cost. The solution is turned back into a proper rotation, for instance with the exponential map, before it is applied.

Parameters

• Initial pose. The most important input, from odometry, an IMU, the previous frame’s motion, user input, or global registration that matches 3D features and fits the pose with RANSAC.
• Rejection threshold. Pairs farther apart than a distance are discarded. Too small a value rejects true pairs while the clouds are still far apart; too large a value lets non-overlapping regions bias the fit.
• Stopping tolerance and iteration limit. A tolerance on the change in error and a maximum iteration count end the loop.
• Sampling and normals. The number of source points per iteration trades speed for accuracy; point-to-plane normals need a neighborhood sized to the noise and sampling density.

Complexity

Besl and McKay give each iteration an average cost of $O(N_P \log N_Q)$ for the closest-point queries, which a k-d tree over the fixed target achieves, plus $O(N_P)$ for the fit. Matching dominates. Rusinkiewicz and Levoy’s fastest variant replaced the tree search with projective association, projecting each source point into the target range image, and aligned two range images in a few tens of milliseconds on 2001 hardware.

Implementation

The example samples a smooth height field twice, independently, moves the source by a known rigid transform, and adds 2 mm of noise. Point-to-point ICP uses the SVD solution with the determinant correction; point-to-plane uses the linearized step with target normals estimated by PCA of 10 neighbors. Both start from the identity, reject pairs more than 10 cm apart, and stop when the mean squared error decreases by less than $10^{-7}$.

import numpy as np
from scipy.spatial import cKDTree
from scipy.spatial.transform import Rotation

rng = np.random.default_rng(1)

def surface(n):
    """n random samples of a smooth height field z = f(x, y) on [-1, 1]^2."""
    xy = rng.uniform(-1, 1, size=(n, 2))
    z = 0.3 * np.sin(2 * xy[:, 0]) * np.cos(1.5 * xy[:, 1]) + 0.2 * xy[:, 0] * xy[:, 1]
    return np.column_stack([xy, z])

# Target ("model") and source ("data"): independent samples of the same surface,
# so no source point coincides with a target point. The source is then moved by
# a known rigid transform (R_true, t_true) and given 2 mm of noise.
target = surface(20000)
R_true = Rotation.from_euler("xyz", [8, -5, 12], degrees=True).as_matrix()
t_true = np.array([0.10, -0.05, 0.08])
source = surface(5000) @ R_true.T + t_true + rng.normal(scale=0.002, size=(5000, 3))

# Target normals by PCA of 10 nearest neighbours (see the point cloud article).
tree = cKDTree(target)
_, idx = tree.query(target, k=10)
nb = target[idx] - target[idx].mean(axis=1, keepdims=True)
normals = np.linalg.eigh(np.einsum("nki,nkj->nij", nb, nb))[1][:, :, 0]

def best_rigid(p, q):
    """Closed-form R, t minimizing sum ||R p_i + t - q_i||^2 (SVD, det fix)."""
    p0, q0 = p.mean(axis=0), q.mean(axis=0)
    H = (p - p0).T @ (q - q0)
    U, _, Vt = np.linalg.svd(H)
    D = np.diag([1.0, 1.0, np.sign(np.linalg.det(Vt.T @ U.T))])
    R = Vt.T @ D @ U.T
    return R, q0 - R @ p0

def point_to_plane_step(p, q, n):
    """Linearized point-to-plane step: small rotation omega and translation t."""
    A = np.hstack([np.cross(p, n), n])          # rows: [ (p x n)^T, n^T ]
    b = np.sum((q - p) * n, axis=1)
    x, *_ = np.linalg.lstsq(A, b, rcond=None)
    R = Rotation.from_rotvec(x[:3]).as_matrix()  # re-orthonormalize the update
    return R, x[3:]

def icp(src, metric, max_iter=200, tol=1e-7, reject=0.1):
    R, t = np.eye(3), np.zeros(3)
    prev = np.inf
    for it in range(1, max_iter + 1):
        moved = src @ R.T + t
        d, j = tree.query(moved)                 # closest-point correspondences
        keep = d < reject                        # reject distant pairs
        if metric == "point-to-point":
            dR, dt = best_rigid(moved[keep], target[j[keep]])
            err = np.mean(d[keep] ** 2)
        else:
            res = np.sum((target[j] - moved) * normals[j], axis=1)
            dR, dt = point_to_plane_step(moved[keep], target[j[keep]], normals[j[keep]])
            err = np.mean(res[keep] ** 2)
        R, t = dR @ R, dR @ t + dt               # compose with the current estimate
        if prev - err < tol:
            break
        prev = err
    return R, t, it

# ICP estimates the transform taking the source back onto the target,
# i.e. the inverse of the one applied: R = R_true^T, t = -R_true^T t_true.
for metric in ["point-to-point", "point-to-plane"]:
    R, t, iters = icp(source, metric)
    rot_err = np.degrees(Rotation.from_matrix(R @ R_true).magnitude())
    trans_err = np.linalg.norm(t + R_true.T @ t_true)
    print(f"{metric:15s} iterations {iters:3d}   "
          f"rotation error {rot_err:.3f} deg   translation error {1000 * trans_err:.2f} mm")

Output:

point-to-point  iterations 104   rotation error 0.590 deg   translation error 3.16 mm
point-to-plane  iterations   4   rotation error 0.004 deg   translation error 0.09 mm

Point-to-plane reaches the true pose to within the noise in four iterations. Point-to-point creeps: its error decreases every iteration here, but by ever smaller amounts, and it stops after 104 iterations still more than half a degree off. Part of the gap is slow sliding along the gently curved surface; part is that each source point is pulled toward a discrete sample rather than the surface, so the point-to-point minimum lies near, not at, the true pose. Iterating to full convergence still left about half a degree here, while a ten times denser target reduced the error markedly.

Properties and Behavior

• Global registration by restarts. Because the guarantee is only local, Besl and McKay handled global registration by running ICP from a set of initial rotations and translations and keeping the best result.
• Convergence speed. Point-to-point ICP converges quickly in the first iterations and then slowly; Besl and McKay accelerated it by extrapolating along consistent update directions. In Rusinkiewicz and Levoy’s comparisons point-to-plane converged substantially faster, and on a plane with grooves only point-to-plane reached the correct alignment, because point-to-point does not let the surfaces slide over each other.
• Variants by stage. Rusinkiewicz and Levoy classified ICP variants by the stage they change: selection of points (all, uniform or random subsampling, or their normal-space sampling, which spreads samples over normal directions so that small features are represented), matching (closest point, normal shooting, or projection into a range image), weighting of pairs, rejection of pairs (by distance, by worst percentage, by a multiple of the standard deviation of distances, or at mesh boundaries), and the error metric and its minimization. They found that weighting and rejection had little effect on convergence speed, though rejection can matter for accuracy and stability.
• Degenerate geometry. Points on a plane constrain only one translation and two rotations, as Rusinkiewicz and Levoy point out; the in-plane motions are left free. More generally, a surface that a continuous motion maps onto itself leaves that motion undetermined: rotation about and translation along the axis of a cylinder, or translation along a long featureless corridor.

Limitations

• Needs a good start. Large rotations or repeated structure can lead ICP to a wrong local minimum.
• Sensitive to outliers and partial overlap. The squared-error fit is pulled by pairs from regions seen by only one scan. Besl and McKay treated outlier removal as a preprocessing step outside the algorithm; practical systems reject distant pairs, keep only the best-matching fraction (trimming), or replace the squared error by a robust loss.
• Rigid only. Moving objects or a deforming subject violate the single rigid motion.

Variants

• Point-to-plane ICP (Chen and Medioni, 1992), described above, is the usual choice for range data with reliable normals.
• Generalized-ICP (Segal, Haehnel and Thrun, 2009) keeps closest-point matching but replaces the minimization by a probabilistic model in which each point has a covariance. With identity and zero covariances it reduces to point-to-point ICP, and point-to-plane appears as a limiting case. Its plane-to-plane form gives every point in both scans a covariance that is small along the surface normal and large within the tangent plane.
• The normal distributions transform (NDT; Biber and Straßer, 2003) needs no explicit correspondences: it divides the plane of the target scan into cells, models the points in each cell by a normal distribution, and registers the source by optimizing the resulting density at its points with Newton’s method. It was proposed for 2D laser scan matching.
• Projective ICP matches by projecting points into the target’s range image (see Complexity). KinectFusion (Newcombe et al., 2011) combines it with the point-to-plane metric to track a depth camera in real time, aligning each new depth map with a prediction from the model built so far.

ICP variants also underlie scan-to-map registration in LiDAR SLAM and ground-truth generation in datasets such as KITTI.