ICP算法（迭代最近点）

参考博客：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. void CalculateRotation(double *q, double *R)
  20. {
  21. R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
  22. R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
  23. R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
  24. R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
  25. R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
  26. R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
  27. R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
  28. R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
  29. R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
  30. }

平移矩阵计算

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);

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

封装后的效果图：

时间： 2024-10-12 20:44:33

ICP算法（迭代最近点）的相关文章

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

标签: 图像匹配ICP算法机器视觉 2015-12-01 21:09 2217人阅读 评论(0) 收藏 举报 分类: Computer Vision(27) 版权声明:本文为博主原创文章,未经博主允许不得转载. 最近在做点云匹配,需要用c++实现ICP算法,下面是简单理解,期待高手指正. ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换. ICP算法本质上是基于最小二乘法的最优配准方法.该算法重复进行选

ICP 算法

ICP 算法是一种点云到点云的配准方法. 在SLAM中通过空间点云的配准(可以通过相机或者3D激光雷达获取点云数据),可以估计相机运动(机器人运动,旋转矩阵R与平移向量t),累积配准,并不断回环检测,可以保证机器人定位的精度. 想象三维空间中两组点云PL(参考点) 以及 PR(目标点): 1. 在PL和PR中寻找最近点(对于稀疏点云的微小运动,寻找欧拉空间最近点:对于密集点云或者较大运动,可能需要寻找描述子之间距离的最近点),这一步,不一定需要配准所有的点: 2. 通过初始配准的两个点集,计算各

困境：经典ICP算法的一些问题

因为学校项目的原因,可能要实现一个三维场景重建的功能,然后从经典的ICP算法开始,啃了很多文档,对其原理也是一知半解. 迭代最近点算法综述 大致参考了这份文档之后,照着流程用MATLAB实现了一个简单的ICP算法,首先是发现这份文档中一个明显的错误, 公式6 求两个点集的协方差,其中(Pi-p)和(Qi-p')分别求两个点集的各点与重心的差,都是(3*1)向量,这是不能相乘的,根据后文推断,此物的结果应为(3*3)矩阵,所以我大(zuo)胆(si)的改为(Pi-p)' * (Qi-p'),做一次

ICP算法使用遇到的问题

这几天在学习数据关联的方法,本来想使用ICP算法进行距离测距数据的配准,但是用的过程中出现问题,配的不准,而且偏差更大了. 红色的和黄色的2维激光点进行ICP配准,但将变换矩阵和黄色进行乘之后偏差更大了.怀疑是因为两个点集只有部分数据重合,而ICP算法最好是点能一一对应. 之后使用PCL进行点集匹配测试,出现同样的问题. 于是我自己构造了一个数据,将A点集进行一个刚体变换,之后用ICP配准,再进行对比,发现算法可以配准的很好,应该是部分点集重合造成的.

ICP算法及其变种

Iterative Closest Point (ICP) [1][2][3] is an algorithm employed to minimize the difference between two clouds of points. 分类法(1) •全局匹配算法 Globe •局部匹配算法Local 分类法(2) •基于点的匹配 •基于特征的匹配 •点特征 •VPF •FHPF •… •基于线特征 •"Algorithms for Matching 3D Line Sets."

PCL学习笔记二：Registration (ICP算法)

原文:http://blog.csdn.net/u010696366/article/details/8941938 PCL Registration API Registration:不断调整,把不同角度的3D点数据整合到一个完整的模型中.它的目的在于在一个全局坐标系下找到不同视角的定位与定向(两个视角交叉部分重叠完好为最优).这就是KinectFusion论文中所提到的ICP( Iterative Closest Point )算法.给定输入数据集,首先做一个估计,然后通过旋转和平移变换一个

五子棋AI算法-迭代加深

前面讲到了算杀,其实在算杀之前应该讲一下迭代加深.因为这些文章是我边做边写的一些笔记,所以顺序上可能不是那么严谨. 按照前面的所有算法实现之后(当然不包括算杀),会发现一个比较严重的问题,就是电脑在自己已经胜券在握的情况下(有双三之类的棋可以走),竟然会走一些冲四之类的棋来调戏玩家.这种走法出现的本质就是因为现在的AI只比较最终结果,并没有考虑到路径长短.所以很容易出现在6层搜索到一个双三,其实在4层的时候也有一个双三,因为分数一样,AI会随机选择一个走法.就导致了明明可以两步赢的棋,AI非要走

分治算法应用-最近点对的最小距离-hdu 1007 Quoit Design

题目描述 给出二维平面上的n个点,求其中最近的两个点的距离的一半. 输入包含多组数据,每组数据第一行为n,表示点的个数:接下来n行,每行一个点的坐标.当n为0时表示输入结束,每组数据输出一行,为最近的两个点的距离的一半. 输入样例: 2 0 0 1 1 2 1 1 1 1 3 -1.5 0 0 0 0 1.5 0 输出样例: 0.71 0.00 0.75 题目解析: 采用分治的思想,把n个点按照x坐标进行排序,以坐标mid为界限分成左右两个部分,对左右两个部分分别求最近点对的距离,然后进行合并.

c++容器 算法 迭代

#include <iostream> #include <vector> using namespace std; int main() { // 创建一个向量存储 int vector<int> vec; int i; // 显示 vec 的原始大小 cout << "vector size = " << vec.size() << endl; // 推入 5 个值到向量中 for(i = 0; i <