ICP Algorithm for Point Cloud Stitching

ICP stands for Iterative Closest Point algorithm. It is a well-known algorithm used to align two point clouds.

Iterative Closest Point

1. Search for correspondences.
2. Reject bad correspondences.
3. Estimate a transformation using the good correspondences.
4. Iterate.

Many tutorials are available online for ICP algorithm such as http://www.cs.technion.ac.il/~cs236329/tutorials/ICP.pdf

The popular PCL library for point cloud processing provides ICP class here: http://pointclouds.org/documentation/tutorials/iterative_closest_point.php

Here is another example of Matlab implementation for ICP: http://www.csse.uwa.edu.au/~ajmal/code/icp.m

Despite the popularity of ICP, when using it, I find that the result is not always satisfactory. The biggest problem is that there is no guarantee that ICP will converge to global optimum. In practice, it often converges to local optimum. As shown in the picture below, when point clouds A and B merge, depending on the starting positions, it is possible that ICP only merges part of the clouds.