ICP Iterative Closest Point Method

Guess a snap, pair each point to its nearest neighbor, solve the best move, repeat. ICP is that loop, and it locks when close.

Nearest pairs pull the moving scan onto the fixed scan step by step

Why Does This Exist?

Point cloud registration gets close with coarse matches, but maps need centimeter polish. ICP exists as that polisher: starting from a decent guess, it alternates correspondence and closed-form alignment until movement stops. Every lidar odometry and scan cleaner runs it last.

Global starts come from coarse matching or Deep Global Registration.

Think of It Like This

Shuffling two combs together

Press two combs offset and let teeth find nearest gaps, shift, and repeat. Teeth seat deeper each round until no shift helps. Start combs a full width off and teeth seat wrongly.

Nearest pairing is the teeth finding gaps. The analogy stops at rotation: combs slide while scans rotate, needing SVD rather than a push.

How It Actually Works

Each round pairs every moving point to its nearest fixed point, optionally with normals for point-to-plane residuals $r = n^T (R p + t - q)$ . The rigid step solves by SVD of the cross-covariance for $R$ , then $t$ from centroids. Trimming keeps the best $70$ percent of pairs to resist outliers. Convergence needs initial error inside about $0.5$ m and $15$ degrees; outside that nearest neighbors are wrong and the loop walks into a local minimum.

Worked numbers

Start $0.3$ m and $8$ degrees off on a room overlap. Mean pair distance falls $0.30$ to $0.12$ to $0.05$ to $0.02$ m over $20$ steps. Final translation error $0.02$ m with rotation $0.4$ degrees. Starting $2.0$ m off converges to a wrong wall with deceptively low $0.06$ m residual, the classic local-minimum trap.

Code

1def icp_step_error(errors: list[float], keep: float = 0.7) -> float:2    ordered = sorted(errors)  # trimmed mean resists outliers3    kept = ordered[: max(1, int(len(ordered) * keep))]4    return sum(kept) / len(kept)5
6
7print(round(icp_step_error([0.3, 0.12, 0.05, 0.5], 0.75), 3))  # -> 0.157

Watch Out For

Running vanilla point-to-point on flat walls

Point-to-point slides along planes because any slide scores the same, converging slowly with sideways drift. The symptom is walls that align in depth but smear laterally. Use point-to-plane with normals on structured scenes.

The Quick Version

ICP alternates nearest pairing with SVD rigid solving.
Point-to-plane with normals beats point-to-point on flat structure.
Trimming the worst pairs contains outliers each round.
Convergence needs starts within about $0.5$ m and $15$ degrees.
Rooms settle near $0.02$ m in twenty steps from $0.3$ m away.
Far starts lock onto wrong geometry with low residuals.