Iterative closest point (ICP)

Overview

ICP finds the rigid motion (rotation and translation) that lines up one point cloud with another. It repeats two steps: choose correspondences between the clouds under the current motion, then solve for the best rigid motion for those correspondences 1910.12240 §3.1. The closed-form fit is shared by most variants: centre both sets, form the cross-covariance, take its SVD, and read the rotation and translation off the factors 1910.12240 §3.1.

The variants differ in how they choose correspondences, which residual they minimise and how they weight each residual. The invariant that all of them rely on is the closed-form fit for a fixed correspondence.

Variants

Classic ICP with nearest-neighbour correspondences

The baseline form alternates a nearest-neighbour mapping with the closed-form fit 1910.12240 §3.1. Its stated formulation assumes both clouds have the same number of points, M = N 1910.12240 §3.1. Partial views violate this: usually N ≠ M and only subsets of the two clouds match 1910.12240 §3.2.

On the ModelNet40 partial-to-partial split, the PRNet authors report a rotation MSE of 1134.552 for ICP, against 10.235 for PRNet 1910.12240 tab. 1. These are author-reported results on synthetic motions. The paper does not describe the ICP baseline's settings beyond its text 1910.12240 §4.1. Card: PRNet: Self-Supervised Learning for Partial-to-Partial Registration.

Frame-to-model point-to-plane ICP with robust and semantic weights

SuMa++ aligns each new LiDAR scan to a surfel map rather than to the previous scan 2105.11320 §III-F. Each residual is the distance along the model surface normal at the projected point, so the solve is a Gauss-Newton step on a point-to-plane objective 2105.11320 §III-F. Each residual is weighted by a Huber weight, by a semantic compatibility term, and by an indicator for whether the surfel is stable 2105.11320 §III-F. Where a point's label disagrees with a movable-class surfel, the semantic term lowers its influence, so moving objects count less in the pose estimate 2105.11320 §III-E 2105.11320 §III-F.

The paper's KITTI odometry result is for the full system. The average translational error on training sequences 00–10 falls from 0.83% (SuMa) to 0.70% (SuMa++), author-reported 2105.11320 §IV-B. The result tables compare whole systems and do not isolate the effect of the semantic ICP weights 2105.11320 §IV. Card: SuMa++: Efficient LiDAR-based Semantic SLAM.

Learned iterative matching

PRNet keeps the iteration and the closed-form fit but replaces the nearest-neighbour step with a learned, soft matching between keypoints 1910.12240 §3.2. Each pass samples a near-one-hot matching, solves the motion, moves the source and repeats; training runs three passes 1910.12240 §3.2. The network is trained self-supervised on synthetic motions 1910.12240 §4.1.

On partial ModelNet40 the authors report a rotation MSE of 10.235, against 45.005 for DCP-v2 1910.12240 tab. 1. On real Stanford Bunny scans the model was fine-tuned on 100 synthetic examples per scan; the paper gives qualitative results only 1910.12240 §4.2. Card: PRNet: Self-Supervised Learning for Partial-to-Partial Registration.

ICP as a pre-alignment step for a deformation score

SoGraB uses ICP for a different purpose. It does not estimate motion for its own sake. It aligns the grasped object's point cloud to the pre-grasp cloud, and a Density-Aware Chamfer Distance between the aligned clouds then measures how much the object deformed 2411.19408 §3.1 2411.19408 §3.2. The ICP starts from a pose estimate taken from the robot's kinematics 2411.19408 §3.2. For symmetric objects with large deformation, the paper aligns centres of mass and principal axes instead 2411.19408 §3.2.

The paper says ICP approximates the transform reliably for moderate deformation, given a good initial estimate 2411.19408 §3.2. The reported results are grasp outcomes and scores, with no measure of alignment accuracy 2411.19408 §5. The paper does not name the ICP type. The released code uses point-to-point ICP, with a distance threshold of 10000 and fixed initial matrices chosen by gripper angle, and it has no centre-of-mass or principal-axis step. The card lists these differences card: Paper vs code. Card: SoGraB: A Visual Method for Soft Grasping Benchmarking and Evaluation.

When to use which

Implementation sketch

The common core is the loop below. Variants change the marked lines: the correspondence step, the residual and the weights.

# Classic ICP loop  [arxiv:1910.12240 §3.1]
# Input:  source X (N, 3), target Y (M, 3), initial R (3, 3), t (3,)
# Output: rotation R, translation t

def icp(X, Y, R, t, iters):
    for _ in range(iters):
        # Correspondence step: nearest target point per moved source point
        # (learned variants replace this with a soft matching)  [arxiv:1910.12240 §3.2]
        Z = nearest_neighbour(Y, R @ X + t)
        # Weights: 1 here; SuMa++ multiplies in Huber and semantic terms  [arxiv:2105.11320 §III-F]
        w = ones(N)
        # Closed-form fit: centre, cross-covariance, SVD  [arxiv:1910.12240 §3.1]
        dR, dt = weighted_rigid_fit(X, Z, w)
        R, t = dR @ R, dR @ t + dt
    return R, t

For point-to-plane variants, the fit is not closed form. It solves a weighted normal-equation system for a small pose increment at each step 2105.11320 §III-F. Stop criteria and iteration counts are not stated in the text of the PRNet or SoGraB papers, so set them on your own data.

Contradictions

None known between these three papers. Their numbers are not comparable: one reports synthetic partial ModelNet40 results 1910.12240 tab. 1, another reports KITTI LiDAR odometry for a full mapping system 2105.11320 §IV-B, and the third reports grasp scores on printed objects 2411.19408 §5.

Papers

• PRNet: Self-Supervised Learning for Partial-to-Partial Registration: learned soft matching in an iterative registration loop for partial clouds, with classic ICP as a baseline.
• SuMa++: Efficient LiDAR-based Semantic SLAM: frame-to-model point-to-plane ICP whose residuals are weighted by semantic labels and surfel stability.
• SoGraB: A Visual Method for Soft Grasping Benchmarking and Evaluation: ICP pre-alignment, from a kinematic start, before a density-aware Chamfer distance scores deformation.