M3-T5 — ICP Fundamentals

Learning objectives

After this lesson, you should be able to:

• Write the point-to-point ICP loop and state its stopping criteria.
• Derive the closed-form rigid transform for known correspondences (centroids and an SVD or atan2 step).
• Explain why ICP converges to a local minimum and what determines its basin of convergence.
• Name common variants (point-to-line, trimmed, robust, generalised) and what each fixes.

The problem and the intuition

Imagine two tracings of the same floor plan on transparent sheets, one a little shifted and rotated. You cannot tell which dot matches which. A sensible strategy: pretend that every dot on your sheet corresponds to the nearest dot on the other, find the single slide-and-turn that best fits those pairs, apply it, and repeat. After the move, the nearest dots are better pairs than before. ICP does exactly this: guess matches, fix motion, repeat.

If the correspondences were known, the problem would be easy and exact. ICP replaces the unknown matches by the nearest neighbours and improves both together.

The algorithm: point-to-point ICP

Problem. Given a source point set P = {p_i} and a target set Q = {q_j}, find the rotation R and translation t that minimise the sum of squared distances between each transformed source point and its corresponding target point.

Assumptions. The two sets sample the same surfaces; they overlap substantially; the initial alignment is within the basin of convergence; noise is small and there are few outliers.

The closed-form step for known correspondences

For pairs (p_i, q_i), minimise Σ |R p_i + t − q_i|². The translation follows from the centroids and the rotation from the centred points. In 2D, with p̃_i = p_i − p̄ and q̃_i = q_i − q̄:

In 3D the same result comes from the SVD of the covariance H = Σ p̃_i q̃_iᵀ = U Σ Vᵀ: R = V diag(1, 1, det(V Uᵀ)) Uᵀ and t = q̄ − R p̄. The determinant factor prevents a reflection (Arun, Huang and Blostein, 1987; Umeyama, 1991).

p̄, q̄

centroids (means) of the paired source and target points.

p̃, q̃

points with their centroid subtracted.

H

the 3×3 (or 2×2) cross-covariance of the centred pairs.

det

determinant; +1 for a rotation, −1 for a reflection.

Pipeline

1. Initialise the transform T with the best available guess (odometry), or the identity.
2. Transform the source points: p′_i = T p_i.
3. Match: for each p′_i find its nearest neighbour q_i in the target (use a k-d tree). Optionally discard pairs with distance above a threshold.
4. Solve: compute the closed-form R, t for these pairs and compose it with the current T.
5. Check convergence: stop when the change in transform is below a tolerance (for example 1e-4 m and 1e-4 rad), the mean error stops decreasing, or a maximum number of iterations (20–50) is reached.

Properties

• Monotonic. Each iteration cannot increase the mean squared distance, so ICP always converges to some local minimum.
• Local. The nearest-neighbour correspondences are only right when the scans already almost overlap. With a large initial error, the correspondences are wrong and the minimum reached may be a different alignment (a wall matched to another wall).
• Slow near the end for point-to-point matching on flat walls (the points can slide along the surface). Point-to-line and point-to-plane distances converge much faster.
• Cost. Nearest-neighbour search dominates: O(N log M) per iteration with a k-d tree.

Variants and what they fix

A worked example: three points, one exact step

Source points P = (0, 0), (1, 0), (0, 1). The target is P rotated by 30° and translated by (0.5, 0.2): Q = (0.5, 0.2), (1.366, 0.7), (0.0, 1.066). Assume the correspondences are known.

1. Centroids. p̄ = (0.333, 0.333). q̄ = (0.622, 0.655).
2. Centred points. p̃ = (−0.333, −0.333), (0.667, −0.333), (−0.333, 0.667). q̃ = (−0.122, −0.455), (0.744, 0.045), (−0.622, 0.411).
3. Sums. Σ dot = Σ(p̃ₓq̃ₓ + p̃_yq̃_y) = 1.155 and Σ cross = Σ(p̃ₓq̃_y − p̃_yq̃ₓ) = 0.667. θ = atan2(0.667, 1.155) = 30.0°.
4. Translation. t = q̄ − R(30°) p̄ = (0.622, 0.655) − (0.333 cos30° − 0.333 sin30°, 0.333 sin30° + 0.333 cos30°) = (0.622, 0.655) − (0.122, 0.455) = (0.5, 0.2). The exact transform is recovered in one step because the correspondences were right.
5. With unknown correspondences, ICP would pair each source point with its nearest target point. If the true motion is small (a few centimetres, a few degrees), most pairs are right and a few iterations converge. If the rotation were 90°, the nearest neighbour of (1, 0) rotated might be (0, 1)’s image: a wrong pair, and the first step moves the wrong way. The next iteration pairs again, but now from a worse position.

This is why the lab asks you to first run ICP on a good start and then to find how far you can push it before it fails.

Engineering implications and limitations

Engineering implications

• Provide a good initial guess from wheel odometry or an IMU. Even a rough guess within a few degrees and tens of centimetres is usually enough for a room-scale scan.
• Downsample large clouds with a voxel or random filter: nearest-neighbour search is the cost.
• Reject outliers by distance (a gate that shrinks over the iterations), trimming, or a robust loss.
• Monitor the result: final mean error, inlier ratio and the movement from the guess. Refuse results with a poor score and fall back to odometry.
• Check degeneracy (M3-T4): ICP in a corridor converges happily to a pose that is wrong along the corridor.
• Use point-to-plane in 3D when normals are available; it is the default in most LiDAR odometry pipelines (LOAM, LIO-SAM and others in Module 5).

Limits

ICP is a local method: no number of iterations will rescue a bad initial guess. It is sensitive to the sampling pattern (points on a wall seen from different distances have different densities), to outliers and to partial overlap. It gives a point estimate with no uncertainty: covariance has to be estimated separately, for example from the Hessian of the final objective. For global alignment use features, correlative matching or learned descriptors (Modules 5 and 8).

Common misconceptions

Engineering takeaways

1. ICP alternates between nearest-neighbour correspondences and the closed-form rigid transform for those pairs.
2. The closed form uses centroids for translation and atan2 (2D) or SVD (3D) of the centred cross-covariance for rotation.
3. It converges monotonically to a local minimum, so the starting guess decides the outcome.
4. Point-to-line or point-to-plane, trimming and robust losses fix slow convergence and outliers.
5. Always check the quality of the final match and the geometry (degeneracy) before using it.

References and further reading

Established fundamentals. Learned registration (Module 8) is an active research area.

• Besl, P. J. and McKay, N. D. (1992). A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence 14(2).
• Arun, K. S., Huang, T. S. and Blostein, S. D. (1987). Least-squares fitting of two 3-D point sets. IEEE Transactions on Pattern Analysis and Machine Intelligence 9(5).
• Chen, Y. and Medioni, G. (1992). Object modelling by registration of multiple range images. Image and Vision Computing 10(3).
• Segal, A., Haehnel, D. and Thrun, S. (2009). Generalized-ICP. Robotics: Science and Systems.
• Rusinkiewicz, S. and Levoy, M. (2001). Efficient variants of the ICP algorithm. International Conference on 3-D Digital Imaging and Modeling.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

ICP gives a registration; the next lessons use it inside filters. First, the problem of estimating the robot and the map together with a Bayes filter.

Related content

Nothing below is required to finish this lesson.

• Before
  M3-T4: Scan Matching

  Pose the scan-to-scan and scan-to-map alignment problem and name its main approaches and failure modes.

• Lab
  M3-L3: Diagnose Scan-Matching Failure

  ICP converges to the wrong alignment: diagnose initial guess, outliers and geometry degeneracy and repair them.