ICP Algorithm (iteration nearest point)

Reference Blog: http://www.cnblogs.com/21207-iHome/p/6034462.html

Recently in the cloud matching, need to use C + + to implement the ICP algorithm, the following is a simple understanding, expect the master correct.

The ICP algorithm is able to merge point cloud data from different coordinates into the same coordinate system, first to find a usable transform, and the registration operation is actually to find a rigid transformation from coordinate system 1 to coordinate system 2.

The ICP algorithm is essentially an optimal registration method based on least squares. The algorithm repeats the selection of corresponding relation point pairs and calculates the optimal rigid body transformation until the convergence accuracy of the correct registration is satisfied .

The objective of the ICP algorithm is to find the rotation parameters R and T of the cloud data and the reference cloud data, so that the two-point data satisfies the optimal matching under some measure criterion.

Suppose that the registration steps for the two three-dimensional point set X1 and X2,ICP methods are as follows:

The first step is to calculate the corresponding near point of each point in the X2 in the X1 point set;

In the second step, the transformation of the rigid body with the minimum average distance is obtained, and the translation parameters and rotation parameters are obtained.

In the third step, a new set of transform points is obtained for X2 using the translation and rotation parameters obtained from the previous step;

Fourth, if the average distance between the new transform point set and the reference point set satisfies the two-point set is less than a given threshold, the iterative calculation is stopped, otherwise the new set of transform points continues to iterate as a new X2 until the target function is reached.

Recent point-to-find: the calculation of the corresponding point is the most time-consuming step in the entire registration process, find the nearest point, use k-d tree to improve the search speed k-d tree method is based on binary tree axis segmentation, constructs k-d tree The process is generated according to the binary tree law, first, the x-axis to find the dividing line, that is, to calculate a bit of the average value of x, the closest to the average point of the X value of the space is divided into two parts, and then in the sub-space divided by the Y-axis to find the split line, divided into two parts, divided into good sub-space in the X-axis And so on, there is only one point in the end until the split area. This segmentation process corresponds to a binary tree, the sub-node of the binary tree corresponds to a split line, and the binary tree each leaf node corresponds to a point. The topological relationship of this point is established.

******************

hao_09

Date: 2015/12/1

Article Address: http://blog.csdn.net/lsh_2013/article/details/50135045

===================================================

Series of postgraduate courses see the Index "Lessons in the letter section"

Basic principle

Assuming that you have given two datasets P, Q, the spatial transformation F of two point sets gives them the ability to make spatial matches. The problem here is that f is an unknown function, and the number of points in a two-point set is not necessarily the same. The most common way to solve this problem is to iterate the nearest point method (iterative Closest Points algorithm).

The basic idea is to match the data according to some geometrical characteristics, and set these matching points as imaginary correspondence points, then solve the motion parameters according to the corresponding relation. The motion parameters are then used to transform the data. The same geometrical features are used to determine the new correspondence relation and repeat the above process.

Iterative nearest Point method objective function two 3D points in three-dimensional space, their Euclidean distance is expressed as: three-dimensional point cloud matching problem is to find the p and q changes in the matrix R and T, for, with the least squares to solve the optimal solution: the hour of R and T. The points in the data preprocessing experiment where five polygons were collected are as follows: as the first group (the first row 1th) and the third group (the first row of the third) are the model front point cloud, so choose one and three to do the follow-up experiment. First, using the Geomagic studio to remove some of the isolated noise from the original data, the effect is as follows: the separation of the parallel movement and rotation first estimates the translation vector T, the method is to get the center of the point set P and Q respectively:

Shift the point set P and Q to the center point respectively: The above optimization objective function can be converted to:
The optimization problem is broken down into:

1. To make E the smallest
2. Seek to make

The specific code for the Panning center point is:

2. Calculates the center point of the point cloud P mean
4. void Calculatemeanpoint3d (vector<point3d> &p, Point3D &mean)
6. {
8. Vector<point3d>::iterator it;
10. mean.x = 0;
12. MEAN.Y = 0;
14. mean.z = 0;
16. For (It=p.begin (); It!=p.end (); it++) {
18. Mean.x + = it->x;
20. MEAN.Y + = it->y;
22. Mean.z + = it->z;
24. }
26. mean.x = Mean.x/p.size ();
28. MEAN.Y = Mean.y/p.size ();
30. Mean.z = Mean.z/p.size ();
32. }

The initial translation effect is as follows: using control points to find the initial rotation matrix when determining the corresponding relationship, the geometric feature used is the closest point in space. Here, we don't even need all the points in the two-point set. You can refer to selecting a subset of points from a certain point, which is generally called Control Point (Control Points)。 At this point, the registration problem translates to: Here, Pi,qi is the closest match.
The "manual registration" of two models can be done with three points in Geomagic studio (it feels like a bad translation here, registration, should be "manual match"). We will export the manually selected three points as the initial control point for the experiment:

For the I-to-point, the matrix Ai that calculates the pair of points:

A transpose matrix for the.

(* Here's a wrong matrix transformation formula in the teacher's class)

For each pair of matrix AI, compute matrix B:

2. Double b[16];
4. For (int i=0;i<16;i++)
6. b[i]=0;
8. for (Itp=p.begin (), Itq=q.begin (); Itp!=p.end (); itp++,itq++) {
10. Double divpq[3]={itp->x,itp->y,itp->z};
12. Double addpq[3]={itp->x,itp->y,itp->z};
14. Double q[3]={itq->x,itq->y,itq->z};
16. Matrixdiv (divpq,q,3,1);
18. Matrixadd (addpq,q,3,1);
20. Double a[16];
22. For (int i=0;i<16;i++)
24. a[i]=0;
26. For (int i=0;i<3;i++) {
28. A[i+1]=divpq[i];
30. A[i*4+4]=divpq[i];
32. A[i+13]=addpq[i];
34. }
36. Double at[16],amul[16];
38. Matrixtran (a,at,4,4);
40. Matrixmul (a,at,amul,4,4,4,4);
42. Matrixadd (b,amul,4,4);
44. }

The original optimization problem can be converted to the minimum eigenvalues and eigenvectors of B, the specific code:

1. Using singular value decomposition to calculate eigenvalues and eigenvectors of B
2. double Eigen, qr[4];
3. Matrixeigen (B, &eigen, QR, 4);

2. Calculation of eigenvalue decomposition of n-order positive definite array m: Eigen as eigenvalues, q as eigenvectors
4. void Matrixeigen (double *m, double *eigen, double *q, int n)
6. {
8. double *vec, *eig;
10. VEC = new double[n*n];
12. Eig = new double[n];
14. Cvmat _m = Cvmat (n, N, cv_64f, M);
16. Cvmat _vec = Cvmat (n, N, cv_64f, VEC);
18. Cvmat _eig = Cvmat (n, 1, cv_64f, EIG);
20. //using matrix operations in the OpenCV Open Source Library to solve matrix eigenvalues and eigenvectors
22. CVEIGENVV (&_m, &_vec, &_eig);
24. *eigen = eig[0];
26. For (int i=0; i<n; i++)
28. Q[i] = Vec[i];
30. delete[] VEC;
32. delete[] Eig;
34. }
38. Calculating the rotation matrix
40. void Calculaterotation (double *q, double *r)
42. {
44. R[0] = q[0]*q[0] + q[1]*q[1]-q[2]*q[2]-q[3]*q[3];
46. R[1] = 2.0 * (q[1]*q[2]-q[0]*q[3]);
48. R[2] = 2.0 * (Q[1]*q[3] + q[0]*q[2]);
50. R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
52. R[4] = q[0]*q[0]-q[1]*q[1] + q[2]*q[2]-q[3]*q[3];
54. R[5] = 2.0 * (q[2]*q[3]-q[0]*q[1]);
56. R[6] = 2.0 * (q[1]*q[3]-q[0]*q[2]);
58. R[7] = 2.0 * (Q[2]*q[3] + q[0]*q[1]);
60. R[8] = q[0]*q[0]-q[1]*q[1]-q[2]*q[2] + q[3]*q[3];
62. }

In the translation matrix calculation 2.4, we can get the 4-element number of the selection matrix, because in the "separation of parallel movement and rotation", we decompose the optimal problem into:

1. To make E the smallest
2. Seek to make

Therefore, you also need to calculate the translation matrix from the center point.

2. Calculation of rotation matrix R1 and translational matrix T1 by eigenvector
4. Calculaterotation (QR, R1);
6. Double mean_q[3]={_mean_q.x,_mean_q.y,_mean_q.z};
8. Double mean_p[3]={_mean_p.x,_mean_p.y,_mean_p.z};
10. Double meant[3]={0,0,0};
12. int nt=0;
14. for (Itp=p.begin (), Itq=q.begin (); Itp!=p.end (); itp++,itq++) {
16. Double tmpp[3]={itp->x,itp->y,itp->z};
18. Double tmpq[3]={itq->x,itq->y,itq->z};
20. Double tmpmul[3];
22. Matrixmul (R1, Mean_p, Tmpmul, 3, 3, 3, 1);
24. Matrixdiv (tmpq,tmpmul,3,1);
26. Matrixadd (meant,tmpq,3,1);
28. nt++;
30. }
32. For (int i=0; i<3; i++)
34. T1[i] = meant[i]/(double) (NT);

The matrix of one rotation calculation is as follows:
The effect shows the iteration nearest point in Geomagic Studio after the initial match, the point set P ' has a bit of translation change, the Match of P ' and Q ' in the comparison point set, (or iteration number) as the algorithm terminates the condition.
For each point in the point set P, find the nearest point in Q as the corresponding point. In one step, the following functions are minimized by using the previous step:
Over here

2. Calculation errors and D
4. d = 0.0;
6. if (round==1) {
8. Findclosestpointset (DATA,P,Q);
10. }
12. int pcount=0;
14. For (ITP = P.begin (), Itq=q.begin (); Itp!=p.end (); itp++, itq++) {
16. Double sum = (itp->x-itq->x) * (itp->x-itq->x) + (itp->y-itq->y) * (itp->y-itq->y)
18. + (itp->z-itq->z) * (ITP->Z-ITQ->Z);
20. D + = sum;
22. pcount++;
24. }
26. D=d/(Double) (pcount);

A good match can be achieved after the loop is finished:

Post-packaged:

ICP Algorithm (iteration nearest point)