码迷，mamicode.com
首页 > 编程语言 > 详细

ICP算法（迭代最近点）

时间：2017-12-04 23:43:16      阅读：586      评论：0      收藏：0      [点我收藏+]

标签：set   阈值   平移   还需要   基本原理   .net   乘法   start   word   

参考博客：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);  


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

技术分享图片
 
封装后的效果图：

技术分享图片

 

ICP算法（迭代最近点）

标签：set   阈值   平移   还需要   基本原理   .net   乘法   start   word   

原文地址：http://www.cnblogs.com/haoyul/p/7979289.html
踩
(0)
赞
(0)
   
举报
评论 一句话评论（0）
登录后才能评论！
分享档案
更多>
2021年07月29日 (22)
2021年07月28日 (40)
2021年07月27日 (32)
2021年07月26日 (79)
2021年07月23日 (29)
2021年07月22日 (30)
2021年07月21日 (42)
2021年07月20日 (16)
2021年07月19日 (90)
2021年07月16日 (35)
周排行
mamicode.com排行更多图片
更多
友情链接
兰亭集智   国之画   百度统计   站长统计   阿里云   chrome插件   新版天听网
关于我们 - 联系我们 - 留言反馈
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码！