您的位置: 首页  >  文章  >  ICP

ICP

分类: 文章 • 2024-10-02 07:52:28

迭代最近点算法（ICP）

　　在20世纪80年代中期，很多学者开始对点集数据的配准进行了大量研究。1987年，Horn[1]、Arun[2]等人用四元数法提出点集对点集配准方法。这种点集与点集坐标系匹配算法通过实践证明是一个解决复杂配准问题的关键方法。1992年，计算机视觉研究者Besl和Mckay[3]介绍了一种高层次的基于自由形态曲面的配准方法，也称为迭代最近点法ICP（Iterative Closest Point）。以点集对点集（PSTPS）配准方法为基础，他们阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法。从测量点集中确定其对应的最近点点集后，运用Faugera和Hebert提出的方法计算新的最近点点集。用该方法进行迭代计算，直到残差平方和所构成的目标函数值不变，结束迭代过程。ICP配准法主要用于解决基于自由形态曲面的配准问题。
　　迭代最近点法ICP最近点法经过十几年的发展，不断地得到了完善和补充。Chen和Medioni[4]及Bergevin等人[5]提出了point-to-plane搜索最近点的精确配准方法。Rusinkiewicz和Levoy提出了point-to-p rojection搜索最近点的快速配准方法。Soon-Yong和Murali提出了Contractive-projection-point搜索最近点的配准方法。此外，Andrew和Sing[6]提取了基于彩色三维扫描数据点纹理信息的数据配准方法，主要在ICP算法中考虑三维扫描点的纹理色彩信息进行搜索最近点。Natasha等人[7]分析了ICP算法中的点云数据配准质量问题。[8]

ICP目的：就是求解两堆点云之间的变换关系

思路：既然不知道R和t(针对刚体运动)，就假设为未知量，然后通过某些方法求解。

具体求法：假设有两堆点云，分别记为两个集合X=x1,x2,...,xm 和Y=y1,y2,...,ym（m并不总是等于n）。然后呢，我们不失一般性的，假设两个点云之间的变换为R（旋转变换）和t（平移变换），这两个就是我们要求的东西啦～那我们将求解这个问题描述成最小化均方误差：e(X,Y)=∑i=1m(Rxi+t−yi)2　　

初级的ICP方法对上面的优化问题的处理思路如下：　　

（1）初始化R 和 t ：确定初始的R 和 t 的方法很多，如果什么方法都不知道，那随便赋一个R 和 t ，然后就迭代的算呀。随便给一个值从原理上来说也可以得到最终的一个结果呀，但是准不准就不知道了。相信有基本的优化概念的人都知道，初始值的选取很重要，如果初始值选的不好很容易收敛到一个局部最优解，然后局部最优解好不好那就另说了。ICP发展了这么多年了，当然有很多的方法来估计初始的R和t了，像PCL给的SampleConsensusInitalAlignment函数以及TransformationEstimationSVD函数都可以得到较好的初始估计。　　

（2）迭代 ：得到初始的估计后，然后，对于X中的每一个点用当前的R 和 t在Y中找最近的点（比如用欧式距离），然后这两个点就成了一对了～就这样，对所有的点都这么做一次，然后我们就得到了所有的匹配对了～然后呢，用每一对的坐标列一个方程，就得到一系列的方程。然后就求解最优的R和t最小化上面的误差。如此循环往复。

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

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

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

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

ICP

ICP

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

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

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

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

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

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

基本原理

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

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

 

迭代最近点法目标函数

三维空间中两个3D点，ICP ，他们的欧式距离表示为：
ICP
三维点云匹配问题的目的是找到P和Q变化的矩阵R和T，对于 ICP，ICP，利用最小二乘法求解最优解使：
ICP
最小时的R和T。
 

数据预处理

实验中采集了五个面的点如下所示：
ICP
由于第一组（第一排第1个）和第三组（第一排第三个）采集均为模型正面点云，所以选用一和三做后续的实验。
ICP
首先利用Geomagic Studio中删除点的工具，去除原始数据中的一些隔离的噪点，效果如下：
ICP
 

平行移动和旋转的分离

先对平移向量T进行初始的估算，具体方法是分别得到点集P和Q的中心：
 ICP
 
分别将点集P和Q平移至中心点处：
ICP
则上述最优化目标函数可以转化为：
ICP
 
最优化问题分解为：
  1. 求使E最小的 ICP
  2. 求使 ICP
平移中心点的 具体代码为：
[cpp] view plain copy ICPICP
  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. }  
初始平移效果如下：
ICP
 

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

在确定对应关系时，所使用的几何特征是空间中位置最近的点。这里，我们甚至不需要两个点集中的所有点。可以指用从某一点集中选取一部分点，一般称这些点为控制点（Control Points）。这时，配准问题转化为：
ICP
这里，pi，qi为最近匹配点。
在Geomagic Studio中利用三个点就可以进行两个模型的“手动注册”（感觉这里翻译的不好，Registration，应该为“手动匹配”）。
ICP
 
我们将手动选择的三个点导出，作为实验初始的控制点：
ICP

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

 ICP

ICP ，ICP为ICP的转置矩阵。


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

ICP
[cpp] view plain copy ICPICP
  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的最小特征值和特征向量，具体代码：
[cpp] view plain copy ICPICP
  1. //使用奇异值分解计算B的特征值和特征向量  
  2.         double eigen, qr[4];  
  3.         MatrixEigen(B, &eigen, qr, 4);  

[cpp] view plain copy ICPICP
  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最小的 ICP
  2. 求使 ICP
因此还需要通过中心点计算平移矩阵。
[cpp] view plain copy ICPICP
  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);  

一次旋转计算得到的矩阵如下：
ICP
效果在Geomagic Studio中显示如图：
ICP
 

迭代最近点

在初始匹配之后，所点集P`中所有点做平移变化，在比较点集合P`和Q`的匹配度，（或迭代次数）作为算法终止的条件。
具体为对点集P中每个点，找Q中离他最近的点作为对应点。在某一步利用前一步得到的ICP，求使下述函数最小的ICP：
 ICP
这里， ICP
[cpp] view plain copy ICPICP
  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);  


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

ICP
 
封装后的效果图：

ICP

ICP算法（Iterative Closest Point迭代最近点算法）

2 ICP变种　　

除了经典的ICP方法外，还有一些变种，如point-to-point的，point-to-plane的以及plane-to-plane。

三种方法区别 ：就是上面的误差函数的定义不一样而已。在上面讲经典ICP的时候，求和的每一项是X中的每一个点到Y中的每一个点的距离，那就是point-to-point了，那么将求和的每一项变成X中的每一个点到Y中的平面的距离，那就是point-to-plane。同理，如果把求和的每一项变成X中的平面到Y中的平面的距离，那就是plane-to-plane了。我们说了这么久的平面，那么平面到时是怎么定义的呢？　

　point-to-plane的误差函数定义为：Mopt=argminR,t∑i((R⋅xi+t−yi)⋅ni)


　　一、基本原理[3]
　　三维空间R3存在两组含有n个坐标点的点集PL和PR，分别为：三维空间点集PL中各点经过三维空间变换后与点集PR中点一一对应，其单点变换关系式为：
　　(0-1)上式中，R为三维旋转矩阵，t为平移向量。在ICP配准方法中，空间变换参数向量X可表示为[9] 。参数向量中四元数参数满足约束条件为：
　　(0-2)根据迭代的初值X0，由式(0-1)计算新点集Pi为：
　　(0-3)式中，P表示原始未修改过的点集，Pi的下标i表示迭代次数，参数向量X的初始值X0为 。根据以上数据处理方法，ICP配准算法可以概括为以下七个步骤：
　　1） 根据点集Plk中的点坐标，在曲面S上搜索相应最近点点集Prk；
　　2） 计算两个点集的重心位置坐标，并进行点集中心化生成新的点集；
　　3） 由新的点集计算正定矩阵N，并计算N的最大特征值及其最大特征向量；
　　4） 由于最大特征向量等价于残差平方和最小时的旋转四元数，将四元数转换为旋转矩阵R；
　　5） 在旋转矩阵R被确定后，由平移向量t仅仅是两个点集的重心差异，可以通过两个坐标系中的重心点和旋转矩阵确定；
　　6） 根据式(0-3)，由点集Plk计算旋转后的点集P’lk。通过Plk与P’lk计算距离平方和值为fk+1。以连续两次距离平方和之差绝对值 作为迭代判断数值；
　　7） 当 时，ICP配准算法就停止迭代，否则重复1至6步，直到满足条件 后停止迭代。
　

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

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

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

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



相关推荐