ICP算法（迭代最近点）

Posted 2020-10-16 远里歌声_why

参考博客：http://www.cnblogs.com/21207-iHome/p/6034462.html

 

最*在做点云匹配，需要用c++实现ICP算法，下面是简单理解，期待高手指正。

ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。

ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对， 计算最优刚体变换，直到满足正确配准的收敛精度要求。

ICP 算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数R和*移参数 T，使得两点数据之间满足某种度量准则下的最优匹配。

假设给两个三维点集 X1 和 X2，ICP方法的配准步骤如下：

第一步，计算X2中的每一个点在X1 点集中的对应*点；

第二步，求得使上述对应点对*均距离最小的刚体变换，求得*移参数和旋转参数；

第三步，对X2使用上一步求得的*移和旋转参数，得到新的变换点集；

第四步， 如果新的变换点集与参考点集满足两点集的*均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

 最*点对查找：对应点的计算是整个配准过程中耗费时间最长的步骤，查找最*点，利用 k－d tree提高查找速度 K－d tree 法建立点的拓扑关系是基于二叉树的坐标轴分割，构造 k－d tree 的过程就是按照二叉树法则生成，首先按 X 轴寻找分割线，即计算所有点的x值的*均值，以最接*这个*均值的点的x值将空间分成两部分，然后在分成的子空间中按 Y 轴寻找分割线，将其各分成两部分，分割好的子空间在按X轴分割……依此类推，最后直到分割的区域内只有一个点。这样的分割过程就对应于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点。这样点的拓扑关系就建立了。

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

作者：hao_09

时间：2015/12/1

文章地址：http://blog.csdn.net/lsh_2013/article/details/50135045

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

研究生课程系列文章参见索引《在信科的那些课》

基本原理

假定已给两个数据集P、Q， ，给出两个点集的空间变换f使他们能进行空间匹配。这里的问题是，f为一未知函数，而且两点集中的点数不一定相同。解决这个问题使用的最多的方法是迭代最*点法（Iterative Closest Points Algorithm）。

基本思想是：根据某种几何特性对数据进行匹配，并设这些匹配点为假想的对应点，然后根据这种对应关系求解运动参数。再利用这些运动参数对数据进行变换。并利用同一几何特征，确定新的对应关系，重复上述过程。

 

迭代最*点法目标函数

三维空间中两个3D点， ，他们的欧式距离表示为：

三维点云匹配问题的目的是找到P和Q变化的矩阵R和T，对于 ，，利用最小二乘法求解最优解使：

最小时的R和T。

 

数据预处理

实验中采集了五个面的点如下所示：

由于第一组（第一排第1个）和第三组（第一排第三个）采集均为模型正面点云，所以选用一和三做后续的实验。

首先利用Geomagic Studio中删除点的工具，去除原始数据中的一些隔离的噪点，效果如下：

 

*行移动和旋转的分离

先对*移向量T进行初始的估算，具体方法是分别得到点集P和Q的中心：

 

 
分别将点集P和Q*移至中心点处：

则上述最优化目标函数可以转化为：

 
最优化问题分解为：

1. 求使E最小的 
2. 求使 

*移中心点的 具体代码为：

1. //计算点云P的中心点mean  
2. void CalculateMeanPoint3D(vector<Point3D> &P, Point3D &mean)  
3. {  
4.     vector<Point3D>::iterator it;  
5.     mean.x = 0;  
6.     mean.y = 0;  
7.     mean.z = 0;  
8.     for(it=P.begin(); it!=P.end(); it++){  
9.         mean.x += it->x;  
10.         mean.y += it->y;  
11.         mean.z += it->z;  
12.     }  
13.     mean.x = mean.x/P.size();  
14.     mean.y = mean.y/P.size();  
15.     mean.z = mean.z/P.size();  
16. }  

初始*移效果如下：

 

利用控制点求初始旋转矩阵

在确定对应关系时，所使用的几何特征是空间中位置最*的点。这里，我们甚至不需要两个点集中的所有点。可以指用从某一点集中选取一部分点，一般称这些点为控制点（Control Points）。这时，配准问题转化为：

这里，pi，qi为最*匹配点。
在Geomagic Studio中利用三个点就可以进行两个模型的“手动注册”（感觉这里翻译的不好，Registration，应该为“手动匹配”）。

 

我们将手动选择的三个点导出，作为实验初始的控制点：

对于第i对点，计算点对的矩阵 Ai：

 

 ，为的转置矩阵。

（*这里在査老师的课上给了一个错误的矩阵变换公式）

对于每一对矩阵Ai，计算矩阵B: 

1. double B[16];  
2.         for(int i=0;i<16;i++)  
3.             B[i]=0;  
4.         for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){  
5.             double divpq[3]={itp->x,itp->y,itp->z};  
6.             double addpq[3]={itp->x,itp->y,itp->z};  
7.             double q[3]={itq->x,itq->y,itq->z};  
8.             MatrixDiv(divpq,q,3,1);  
9.             MatrixAdd(addpq,q,3,1);  
10.             double A[16];  
11.             for(int i=0;i<16;i++)  
12.                 A[i]=0;  
13.             for(int i=0;i<3;i++){  
14.                 A[i+1]=divpq[i];  
15.                 A[i*4+4]=divpq[i];  
16.                 A[i+13]=addpq[i];  
17.             }  
18.             double AT[16],AMul[16];  
19.             MatrixTran(A,AT,4,4);  
20.             MatrixMul(A,AT,AMul,4,4,4,4);  
21.             MatrixAdd(B,AMul,4,4);  
22.         }  

原最优化问题可以转为求B的最小特征值和特征向量，具体代码：

1. //使用奇异值分解计算B的特征值和特征向量  
2.         double eigen, qr[4];  
3.         MatrixEigen(B, &eigen, qr, 4);  

1. //计算n阶正定阵m的特征值分解：eigen为特征值，q为特征向量  
2. void MatrixEigen(double *m, double *eigen, double *q, int n)  
3. {  
4.     double *vec, *eig;  
5.     vec = new double[n*n];  
6.     eig = new double[n];  
7.     CvMat _m = cvMat(n, n, CV_64F, m);  
8.     CvMat _vec = cvMat(n, n, CV_64F, vec);  
9.     CvMat _eig = cvMat(n, 1, CV_64F, eig);  
10.     //使用OpenCV开源库中的矩阵操作求解矩阵特征值和特征向量  
11.     cvEigenVV(&_m, &_vec, &_eig);  
12.     *eigen = eig[0];  
13.     for(int i=0; i<n; i++)  
14.         q[i] = vec[i];  
15.     delete[] vec;  
16.     delete[] eig;  
17. }  
18.   
19. //计算旋转矩阵  
20. void CalculateRotation(double *q, double *R)  
21. {  
22.     R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];  
23.     R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);  
24.     R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);  
25.     R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);  
26.     R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];  
27.     R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);  
28.     R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);  
29.     R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);  
30.     R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];  
31. }  

*移矩阵计算

2.4中可以得到选择矩阵的4元数表示，由于在"*行移动和旋转的分离"中，我们将最优问题分解为：

1. 求使E最小的 
2. 求使 

因此还需要通过中心点计算*移矩阵。

1. //通过特征向量计算旋转矩阵R1和*移矩阵T1  
2.         CalculateRotation(qr, R1);  
3.         double mean_Q[3]={_mean_Q.x,_mean_Q.y,_mean_Q.z};  
4.         double mean_P[3]={_mean_P.x,_mean_P.y,_mean_P.z};  
5.         double meanT[3]={0,0,0};  
6.         int nt=0;  
7.         for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){  
8.             double tmpP[3]={itp->x,itp->y,itp->z};  
9.             double tmpQ[3]={itq->x,itq->y,itq->z};  
10.             double tmpMul[3];  
11.             MatrixMul(R1, mean_P, tmpMul, 3, 3, 3, 1);  
12.             MatrixDiv(tmpQ,tmpMul,3,1);  
13.             MatrixAdd(meanT,tmpQ,3,1);  
14.             nt++;  
15.         }  
16.         for(int i=0; i<3; i++)  
17.             T1[i] = meanT[i]/(double)(nt);  

一次旋转计算得到的矩阵如下：

效果在Geomagic Studio中显示如图：

 

迭代最*点

在初始匹配之后，所点集P`中所有点做*移变化，在比较点集合P`和Q`的匹配度，（或迭代次数）作为算法终止的条件。
具体为对点集P中每个点，找Q中离他最*的点作为对应点。在某一步利用前一步得到的，求使下述函数最小的：

 

这里， 

1. //计算误差和d  
2.         d = 0.0;  
3.         if(round==1){  
4.             FindClosestPointSet(data,P,Q);  
5.         }  
6.         int pcount=0;  
7.         for(itp = P.begin(),itq=Q.begin();itp!=P.end(); itp++, itq++){  
8.             double sum = (itp->x - itq->x)*(itp->x - itq->x) + (itp->y - itq->y)*(itp->y - itq->y)   
9.                 + (itp->z - itq->z)*(itp->z - itq->z);  
10.             d += sum;  
11.             pcount++;  
12.         }  
13.         d=d/(double)(pcount);  

循环结束后能得到较好的匹配效果：

 

封装后的效果图：