ICP (Iterative closest point) is a popular range data processing method that can be used for range registration. Given two sets of slightly overlapping point clouds the ICP computes the registeration of one scan over the other in an iterative fashion.

Mathematically we can construct the problem as:

Given two point setwe need to find the translation and rotation that

If the correct correspondances are known, we can calculate the alignment using SVD. It can be done using first subtracting the value of the center of mass of the two point clouds from the points in the set and then taking the SVD of the accumulation matrix defined by:The rotation matrix is given by

The registeration algorihtm is briefly described in the following:

• For each point in the first scan, find the closest point in the second scan.
• Find the relative position of each scan so that the MSE (Mean squared error) calculated as the mean of the distance between the closest points is minimized.
• Repeat until the MSE is below certain threshold or stops changing.