The Iterative Closest Point (ICP) algorithm is a method for the registration of two point clouds. Many variants of the algorithm exist. The base algorithm registers the two point clouds iteratively by finding the closest point correspondences from the reference cloud to the target cloud and then minimizing a non-linear least squares objective function to solve for the rotation and translation that perform the rotation. This process is repeated until an error metric measuring the alignment of the two clouds reaches some tolerance and it can be shown that the ICP algorithm always converges to the nearest local minimum monotonically. 

I implemented ICP using point cloud library (PCL) using the point to plane metrics. The image below shows the result of the implemented ICP algorithm for two point clouds from the Kitti dataset. It can be seen that the red and green point clouds are aligned through the ICP algorithm and that the ICP algorithm implementation seems to works well on this test set.