Iterative nearest point algorithm iterative closest points_ iteration recent point

A series of articles on postgraduate courses see the Basic principles of those courses in the Faith section

Assuming that two data sets P and Q have been given, the space transformation F of the two point sets is given to enable them to perform spatial matching. The problem here is that f is an unknown function, and the points in the two-point set are not necessarily the same. The most common way to solve this problem is to iterate over the nearest point method (iterative closest Points algorithm).

The basic idea is to match the data according to some geometrical characteristics, and to set these matching points as imaginary corresponding points, and then to solve the motion parameters according to the corresponding relation. The data are transformed using these motion parameters. The new corresponding relation is determined by using the same geometrical feature, and the above process is repeated.

Iterative nearest point method two 3D points in three-dimensional space of the objective function, their Euclidean distance is expressed as:

The aim of the three-dimensional point cloud matching problem is to find the matrix R and T of P and Q variations, and to solve the optimal solution by least squares method:

The most hours of R and T.

The data preprocessing experiment collected five points as follows: Because the first group (1th) and the third group (the first row third) are the model of the frontal cloud, so choose one and three follow-up experiments. First of all, use the Geomagic studio to remove the point of the tool to remove some of the original data isolated noise, the effect is as follows:


The separation of parallel motion and rotation first estimates the translation vector T, and the specific method is to get the center of Point set P and Q respectively:

Shift the point set P and Q to the center point:
The above optimization objective function can be converted to:
The optimization problem is decomposed into:
The specific code to make the minimum of E for the translation center point is:
[cpp]  View plain copy//Compute point Cloud P's Center point mean   Void calculatemeanpoint3d (vector<point3d>  &p, point3d &mean)    {       vector<point3d>:: iterator it;       mean.x = 0;        mean.y = 0;       mean.z = 0;        for (It=p.begin ();  it!=p.end ();  it++) {            mean.x += it->x;           mean.y +=  it->y;           mean.z += it->z;        }       mean.x = mean.x/p.size ();        mean.y = mean.y/p.size ();       mean.z& nbsp; = mean.z/p.size ();  }   The initial translation effect is as follows:

Using the control point to find the initial rotation matrix the geometric feature used in determining the corresponding relationship is the nearest point in the space. Here, we don't even need all the points in the two-point set. You can refer to selecting a portion of a point from a set of points, which is generally referred to as the control Points. At this point, the registration problem is transformed into: here, Pi,qi is the nearest match.
Use three points in Geomagic studio to perform manual registration for two models (feel bad translation here, registration, should be "manual matching").

We will export the three points manually selected as the initial control point of the experiment:

For the first-pair point, the matrix Ai for the computed point pair:
, the transpose matrix for the.
(* Here in Miss Cha's class gave a wrong matrix transformation formula) for each pair of matrix AI, compute matrix B:
[cpp]  View Plain copy double b[16];           for (int  i=0;i<16;i++)                b[i] =0;           for (Itp=p.begin (), Itq=Q.begin (); Itp!=P.end (); itp++,itq++ ) {               double  divpq[3]={itp->x,itp->y,itp->z};                double addpq[3]={itp->x,itp->y,itp->z};                double q[3]={itq->x,itq->y,itq->z};                matrixdiv (divpq,q,3,1);                matrixadd (addpq,q,3,1);                double a[16];                for (int i=0;i<16;i++)                     A[i]=0;               for (int i=0;i<3;i++) {                    a[i+1]= divpq[i];                    A[i*4+4]=divpq[i];                    A[i+13]=addpq[i];                }                double at[16],amul[16];               matrixtran (A,AT,4,4);                matrixmul (A,AT,AMul, 4,4,4,4);               matrixadd (B,AMul, 4,4);           }  
The original optimization problem can be converted to the minimum eigenvalue and eigenvector of B, the specific code:
[CPP] View plain copy///use singular value decomposition to compute the eigenvalues and eigenvectors of B (double eigen, qr[4]; Matrixeigen (B, &eigen, QR, 4);
[cpp]  View plain copy//Compute the eigenvalue decomposition of n-order positive definite matrix M: eigen is eigenvalue, q is eigenvector    Void matrixeigen (double *m,  double *eigen, double *q, int n)    {        double *vec, *eig;       vec = new double[n*n];        eig = new double[n];        Cvmat _m = cvmat (n, n, cv_64f, m);       cvmat  _vec = cvmat (N, n, cv_64f, vec);       CvMat  _eig = cvmat (N, 1, cv_64f, eig);       // Using matrix operation in OPENCV Open Source Library to solve matrix eigenvalue and eigenvector        cveigenvv (&_m, &_vec, & _eig);       *eigen = eig[0];       for ( Int i=0; i<n; i++)            q[i] = vec[i];        delete[] vec;       delete[] eig;  }      /calculation rotation matrix    void calculaterotation (double *q, double *r)     {       r[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q [2] - q[3]*q[3];       R[1] = 2.0 *  (q[1]*q[2] &NBSP;-&NBSP;Q[0]*Q[3]);       R[2] = 2.0 *  (q[1]*q[3] + &NBSP;Q[0]*Q[2]);       R[3] = 2.0 *  (q[1]*q[2] + q [0]*q[3]);       r[4] = q[0]*q[0] - q[1]*q[1] + q[2 ]*q[2] - q[3]*q[3];       R[5] = 2.0 *  (Q[2]*q[3]  -&NBSP;Q[0]*Q[1]);       R[6] = 2.0 *  (q[1]*q[3] - q [0]*q[2]);       R[7] = 2.0 *  (q[2]*q[3] + q[0]*q[1 ]);       r[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2]  + q[3]*q[3];  }  
The 4-number representation of the choice matrix can be obtained in the translation matrix calculation 2.4, because in the separation of parallel motion and rotation, we decompose the optimal problem into:
The minimum of E is required so that the translation matrix is also computed through a central point.
[cpp]  View Plain Copy//the rotation matrix R1 and translation matrix are computed by eigenvector t1            Calculaterotation (QR,&NBSP;R1);           double mean_q[ 3]={_mean_q.x,_mean_q.y,_mean_q.z};           double  mean_p[3]={_mean_p.x,_mean_p.y,_mean_p.z};           double  meanT[3]={0,0,0};           int nt=0;           for (Itp=p.begin (), Itq=q.begin (); Itp!=p.end ();itp++,itq++  ) {               double tmpp[3]={ itp->x,itp->y,itp->z};                double tmpQ[3]={itq->x,itq->y,itq->z};                double tmpmul[3];                matrixmul (r1, mean_p, tmpmul, 3, 3, 3, 1);               matrixdiv (tmpQ,tmpMul,3,1);                matrixadd (meanT,tmpQ,3,1);                nt++;            }           for (int  i=0; i<3; i++)                 t1[i] = meant[i]/(Double) (NT);  
The matrix of one rotation calculation is as follows:
The effect appears in Geomagic studio as shown:
The most recent point in the iteration, after the initial match, is a bit of a translation change in the point set P ' and the matching degree (or iteration number) of the comparison point set P ' and Q ' as the condition for terminating the algorithm.
Specifically for each point in the point set P, find the point in Q that is closest to him as the corresponding point. The smallest of the following functions obtained by taking advantage of the previous step:
  Here,  [cpp]  view Plain copy//Calculation error and d           d& nbsp;= 0.0;           if (round==1) {                findclosestpointset (Data,P,Q);           }            int pcount=0;           for (Itp = P.begin (), Itq=q.begin () itp!=p.end ();  itp++, itq++) {                double sum =  (itp->x - itq->x) * (itp->x -  itq->x)  +  (itp->y - itq->y) * (itp->y - itq->y)                      +  ( Itp->z&nBSP;-&NBSP;ITQ-&GT;Z) * (itp->z - itq->z);                d += sum;                pcount++;           }           d=d/(Double) (Pcount);  
After the end of the loop can get a better matching effect:

Encapsulated Effect Chart:

from:http://blog.csdn.net/xiaowei_cqu/article/details/8470376