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

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

 
封装后的效果图:

ICP算法(迭代最近点)的更多相关文章

  1. ICP(迭代最近点)算法

    图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对 ...

  2. ICP算法(Iterative Closest Point迭代最近点算法)

    标签: 图像匹配ICP算法机器视觉 2015-12-01 21:09 2217人阅读 评论(0) 收藏 举报 分类: Computer Vision(27) 版权声明:本文为博主原创文章,未经博主允许 ...

  3. 【转】ICP算法(Iterative Closest Point迭代最近点算法)

    原文网址:https://www.cnblogs.com/sddai/p/6129437.html.转载主要方便随时可以查看,如有版权要求请及时联系. 最近在做点云匹配,需要用c++实现ICP算法,下 ...

  4. 迭代最近点算法 Iterative Closest Points

    研究生课程系列文章参见索引<在信科的那些课> 基本原理 假定已给两个数据集P.Q, ,给出两个点集的空间变换f使他们能进行空间匹配.这里的问题是,f为一未知函数,而且两点集中的点数不一定相 ...

  5. ICP 算法

    ICP 算法是一种点云到点云的配准方法. 在SLAM中通过空间点云的配准(可以通过相机或者3D激光雷达获取点云数据),可以估计相机运动(机器人运动,旋转矩阵R与平移向量t),累积配准,并不断回环检测, ...

  6. PCL点云库:ICP算法

    ICP(Iterative Closest Point迭代最近点)算法是一种点集对点集配准方法.在VTK.PCL.MRPT.MeshLab等C++库或软件中都有实现,可以参见维基百科中的ICP Alg ...

  7. icp算法基本思想

    Icp基本思想参考资料:http://www.cnblogs.com/jian-li/articles/4945676.html ,包括点-点,点-面的各种icp变种 Icp算法就是两个点云X.Y之间 ...

  8. 三维点集拟合:平面拟合、RANSAC、ICP算法

    ACM算法分类:http://www.kuqin.com/algorithm/20080229/4071.html 一: 拟合一个平面:使用SVD分解,代码里面去找吧 空间平面方程的一般表达式为: A ...

  9. 点云匹配和ICP算法概述

    Iterative Closest Point (ICP) [1][2][3] is an algorithm employed to minimize the difference between ...

随机推荐

  1. springboot自己实现mysql主从数据切换机制

    在很多公司都是实现了数据的读写分离,所谓的读写分离,就是写的时候从主库 ,然后从库就会从主库中复制过去,这样就会形成了数据的读写分离,然而在很多场景是适用的,那么我们怎么做呢,可以利用aop 加注解的 ...

  2. spark编程入门-idea环境搭建

    原文引自:http://blog.csdn.net/huanbia/article/details/69084895 1.环境准备 idea采用2017.3.1版本. 创建一个文件a.txt 2.构建 ...

  3. C++中如何实现像Java中接口功能--C++抽象类(纯虚函数,虚函数)

    在Java中定义个接口,之后可以定义不同的类来实现接口,如果有个函数的参数为这个接口的话,就可以对各自的类做出不同的响应. 如: interface animal { public void info ...

  4. 2.Spring【DI】XML方式

    依赖: 在A类中引用了B类,说明A依赖于B. 注入: 使用Spring框架给A类中的B对象的属性赋值. 直接上代码: 1.只使用IOC public class Person { private St ...

  5. 再不懂时序就 OUT 啦!,DBengine 排名第一时序数据库,阿里云数据库 InfluxDB 正式商业化!

    云数据库 InfluxDB® 版介绍 阿里云数据库 InfluxDB® 版已于近日正式启动商业化 . 云数据库 InfluxDB® 是基于当前最流行的开源数据库 InfluxDB 提供的在线数据库服务 ...

  6. HZOI2019建造游乐园(play)组合数学,欧拉图

    题目:https://www.cnblogs.com/Juve/articles/11186805.html(密码是我的一个oj用户名) solution: 反正我是想不出来... 题目大意就是要求出 ...

  7. touch滑动事件---简单小案例

    html: <!--导航栏头部--><div class="type_nav"> <ul class="clearfix " v- ...

  8. 通过url方式传递中文参数时出现的中文乱码问题的处理

    1.通过url将参数[会有中文的情况]传递到另外一个页面: window.location.href = "/XXXX/XXXX?searchcontent=" + Content ...

  9. C# 全局Hook在xp上不回调

    最近做了个捕捉全局鼠标,获取目标窗体内的控件文本信息,点击的按钮信息.用的全局钩子.在win10上运行正常,部署到xp系统上就没有反应.查了些资料,解决了此问题. 原本安装钩子的写法如下: Nativ ...

  10. OpenCV2.4和OpenCV3之间函数的转变

    OpenCV3里面没有自带opencv-contrib,需要自己手动安装,也很简单,直接在命令行里面打:pip install opencv-contrib-python 就能安装好了 OpenCV3 ...