ICP Iterative recent point optimization __PCL

Sampling iterative nearest point algorithm for local registration problems between point clouds (Iteratedclosest Points, ICP), ICP algorithm because of its simple thinking, high precision features become a local registration of the mainstream algorithm.

Iterative nearest point algorithm (ICP) as the name suggests, is to use the idea of iterative optimization based on space distance as a matching point of choice, by constantly adjusting the position of the point cloud to make matching points between the minimum cumulative distance. Suppose there are two sets of point clouds, one of which is the point cloud a another is the reference point cloud B,ICP algorithm is designed to calculate an optimal rotation matrix R and the shift vector t so that the transformed point cloud a can achieve the most accurate match with B. As shown in the formula (1), obtain the r,t of the matching point error squared and achieve a minimum:

ICP needs to repeat the following two steps until it converges: the first step is to compute the matching point pair between a point set and B point set; the second step, according to the matching point obtained in the previous step, calculates the transformation matrix between a point set and B point set, and makes a corresponding conversion to a point set. On the basis of obtaining the matching points of point set A and B, the solution of the transformation matrix between the points set is divided into two ways: the solution of linear algebra (mainly SVD) and the Solution of nonlinear optimization (similar to bundle adjustment). The singular value decomposition (SVD) is widely used because of its accuracy, stability and high efficiency, and then the conversion matrix calculation method using SVD method is introduced.

First, define the center of mass of two sets of points:

Then, in the error function, do the following:

With a closer look at the left and right, you can see that the left side is only related to the rotation matrix R, while the right-hand side has both R and T, but only the center of mass. So long as you get the R, the second item is zero to get t. Then the most down problem is translated into the following formula:

After the solution of R, we can solve T.