3D iterative nearest point algorithm iterative closest points__ algorithm analysis

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:

Compute Point Cloud P's central 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 = Mean.z/p.size ();
}

The initial translation effect is as follows:

using control point to find initial rotation matrixWhen determining the corresponding relationship, the geometric feature used is the point closest to the position 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 called 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:

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:

Using singular value decomposition to compute the eigenvalues and eigenvectors of B
		, double eigen, qr[4];
		Matrixeigen (B, &eigen, QR, 4);

To compute the eigenvalue decomposition of n-order positive definite matrix M: Eigen is a eigenvalue, q is a 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);
	Matrix Eigenvalue and eigenvector
	cveigenvv (&_m, &_vec, &_eig)
	are solved using matrix operation in OPENCV Open Source Library. *eigen = eig[0];
	for (int i=0; i<n; i++)
		q[i] = vec[i];
	Delete[] VEC;
	Delete[] Eig
}

Compute the 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]-q[0]*q[3]);
	R[2] = 2.0 * (Q[1]*q[3] + 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]-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];

translation matrix calculationIn 2.4, we can get the 4-number representation of the choice matrix, 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.

The rotation matrix R1 and translation matrix T1
		calculaterotation (QR, R1) are computed by eigenvector.
		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:
Iteration Recent PointAfter the initial match, the point set P ' has a bit of translational change in the comparison point set P ' and Q ' of the match, (or the number of iterations) as the termination of the algorithm condition.
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:
Over here

Calculation error and D
		d = 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-itq->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: