Scan Matching with ICP

Iterative closest point, ICP, lines up two LiDAR scans of the same place. It pairs each point with the nearest point in the other scan, finds the rotation and shift that best fit those pairs, applies it, and repeats until the scans stop moving.

Starting rotation error Starting shift

Show the point pairs

One iteration

For each point $\mathbf{p}_i$ in the moving scan, find its nearest point $\mathbf{q}_i$ in the reference scan. Then solve for the rigid motion that best matches the pairs:

$$\min_{R,\,\mathbf{t}}\ \sum_i \lVert R\,\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \rVert^2$$

This has a closed-form answer from the SVD of the cross-covariance, as in extrinsic calibration. Apply it and repeat.

What to notice

With a small starting error, ICP converges in a few iterations and the scans snap together. Each pairing is only a guess, but it improves each round. Start with a large rotation and the nearest-point pairs are mostly wrong, so ICP can settle into a wrong alignment, a local minimum. That is why ICP needs a decent initial guess, usually from odometry. Variants that match points to lines or planes converge faster on walls.