ICP algorithm (iterative Closest point iterative nearest dot algorithm)

Tags: image matching ICP algorithm machine vision2015-12-01 21:09 2217 People read comments (0) favorite reports Classification:Computer Vision (+)

Copyright NOTICE: This article for Bo Master original article, without Bo Master permission not reproduced.

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 to establish the topological relationship of the point is based on the two-axis partition of the binary-trees, construction k-d The process of tree is generated according to the law of Binary tree, 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"

Fundamentals

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 functionTwo 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, using the least squares method to solve the optimal solution: the hour of R and T.Data preprocessingThe points in the experiment where five faces 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, remove some of the isolated noise from the original data using the tool that removes the point in Geomagic Studio, with the following effect:separation of parallel movement and rotationFirst, the translation vector T is initially estimated by getting 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: [CPP]View Plain copy

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 panning effect is as follows:using control points to find the initial rotation matrixWhen 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:

[CPP]View Plain copy

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: [CPP]View Plain copy

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

[CPP]View Plain copy

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. }

translation matrix calculation2.4 can be obtained in the choice matrix of 4-dollar representation, 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.
[CPP]View Plain copy

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:
Effects are displayed in Geomagic Studioiteration Nearest PointAfter the initial match, the point set P ' is a bit of a translation change in the comparison point set P ' and Q ' Matching degrees (or iterations) 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 [CPP]View Plain copy

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 (iterative Closest point iterative nearest dot algorithm)