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)。
基本思想是:根据某种几何特性对数据进行匹配,并设这些匹配点为假想的对应点,然后根据这种对应关系求解运动参数。再利用这些运动参数对数据进行变换。并利用同一几何特征,确定新的对应关系,重复上述过程。
迭代最近点法目标函数





数据预处理

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

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

平行移动和旋转的分离
先对平移向量T进行初始的估算,具体方法是分别得到点集P和Q的中心:

分别将点集P和Q平移至中心点处:


最优化问题分解为:
- 求使E最小的
- 求使
平移中心点的 具体代码为:
- //计算点云P的中心点mean
- void CalculateMeanPoint3D(vector<Point3D> &P, Point3D &mean)
- {
- vector<Point3D>::iterator it;
- mean.x = 0;
- mean.y = 0;
- mean.z = 0;
- for(it=P.begin(); it!=P.end(); it++){
- mean.x += it->x;
- mean.y += it->y;
- mean.z += it->z;
- }
- mean.x = mean.x/P.size();
- mean.y = mean.y/P.size();
- mean.z = mean.z/P.size();
- }
初始平移效果如下:

利用控制点求初始旋转矩阵
在确定对应关系时,所使用的几何特征是空间中位置最近的点。这里,我们甚至不需要两个点集中的所有点。可以指用从某一点集中选取一部分点,一般称这些点为控制点(Control Points)。这时,配准问题转化为:

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


对于第i对点,计算点对的矩阵 Ai:

,
为
的转置矩阵。
对于每一对矩阵Ai,计算矩阵B:

- double B[16];
- for(int i=0;i<16;i++)
- B[i]=0;
- for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){
- double divpq[3]={itp->x,itp->y,itp->z};
- double addpq[3]={itp->x,itp->y,itp->z};
- double q[3]={itq->x,itq->y,itq->z};
- MatrixDiv(divpq,q,3,1);
- MatrixAdd(addpq,q,3,1);
- double A[16];
- for(int i=0;i<16;i++)
- A[i]=0;
- for(int i=0;i<3;i++){
- A[i+1]=divpq[i];
- A[i*4+4]=divpq[i];
- A[i+13]=addpq[i];
- }
- double AT[16],AMul[16];
- MatrixTran(A,AT,4,4);
- MatrixMul(A,AT,AMul,4,4,4,4);
- MatrixAdd(B,AMul,4,4);
- }
原最优化问题可以转为求B的最小特征值和特征向量,具体代码:
- //使用奇异值分解计算B的特征值和特征向量
- double eigen, qr[4];
- MatrixEigen(B, &eigen, qr, 4);
- //计算n阶正定阵m的特征值分解:eigen为特征值,q为特征向量
- void MatrixEigen(double *m, double *eigen, double *q, int n)
- {
- double *vec, *eig;
- vec = new double[n*n];
- eig = new double[n];
- CvMat _m = cvMat(n, n, CV_64F, m);
- CvMat _vec = cvMat(n, n, CV_64F, vec);
- CvMat _eig = cvMat(n, 1, CV_64F, eig);
- //使用OpenCV开源库中的矩阵操作求解矩阵特征值和特征向量
- cvEigenVV(&_m, &_vec, &_eig);
- *eigen = eig[0];
- for(int i=0; i<n; i++)
- q[i] = vec[i];
- delete[] vec;
- delete[] eig;
- }
- //计算旋转矩阵
- void CalculateRotation(double *q, double *R)
- {
- R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
- R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
- R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
- R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
- R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
- R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
- R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
- R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
- R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
- }
平移矩阵计算
2.4中可以得到选择矩阵的4元数表示,由于在"平行移动和旋转的分离"中,我们将最优问题分解为:
- 求使E最小的
- 求使
因此还需要通过中心点计算平移矩阵。
- //通过特征向量计算旋转矩阵R1和平移矩阵T1
- CalculateRotation(qr, R1);
- double mean_Q[3]={_mean_Q.x,_mean_Q.y,_mean_Q.z};
- double mean_P[3]={_mean_P.x,_mean_P.y,_mean_P.z};
- double meanT[3]={0,0,0};
- int nt=0;
- for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){
- double tmpP[3]={itp->x,itp->y,itp->z};
- double tmpQ[3]={itq->x,itq->y,itq->z};
- double tmpMul[3];
- MatrixMul(R1, mean_P, tmpMul, 3, 3, 3, 1);
- MatrixDiv(tmpQ,tmpMul,3,1);
- MatrixAdd(meanT,tmpQ,3,1);
- nt++;
- }
- for(int i=0; i<3; i++)
- T1[i] = meanT[i]/(double)(nt);

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

效果在Geomagic Studio中显示如图:

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

这里,
- //计算误差和d
- d = 0.0;
- if(round==1){
- FindClosestPointSet(data,P,Q);
- }
- int pcount=0;
- for(itp = P.begin(),itq=Q.begin();itp!=P.end(); itp++, itq++){
- double sum = (itp->x - itq->x)*(itp->x - itq->x) + (itp->y - itq->y)*(itp->y - itq->y)
- + (itp->z - itq->z)*(itp->z - itq->z);
- d += sum;
- pcount++;
- }
- d=d/(double)(pcount);
循环结束后能得到较好的匹配效果:

ICP算法(迭代最近点)的更多相关文章
- ICP(迭代最近点)算法
图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对 ...
- ICP算法(Iterative Closest Point迭代最近点算法)
标签: 图像匹配ICP算法机器视觉 2015-12-01 21:09 2217人阅读 评论(0) 收藏 举报 分类: Computer Vision(27) 版权声明:本文为博主原创文章,未经博主允许 ...
- 【转】ICP算法(Iterative Closest Point迭代最近点算法)
原文网址:https://www.cnblogs.com/sddai/p/6129437.html.转载主要方便随时可以查看,如有版权要求请及时联系. 最近在做点云匹配,需要用c++实现ICP算法,下 ...
- 迭代最近点算法 Iterative Closest Points
研究生课程系列文章参见索引<在信科的那些课> 基本原理 假定已给两个数据集P.Q, ,给出两个点集的空间变换f使他们能进行空间匹配.这里的问题是,f为一未知函数,而且两点集中的点数不一定相 ...
- ICP 算法
ICP 算法是一种点云到点云的配准方法. 在SLAM中通过空间点云的配准(可以通过相机或者3D激光雷达获取点云数据),可以估计相机运动(机器人运动,旋转矩阵R与平移向量t),累积配准,并不断回环检测, ...
- PCL点云库:ICP算法
ICP(Iterative Closest Point迭代最近点)算法是一种点集对点集配准方法.在VTK.PCL.MRPT.MeshLab等C++库或软件中都有实现,可以参见维基百科中的ICP Alg ...
- icp算法基本思想
Icp基本思想参考资料:http://www.cnblogs.com/jian-li/articles/4945676.html ,包括点-点,点-面的各种icp变种 Icp算法就是两个点云X.Y之间 ...
- 三维点集拟合:平面拟合、RANSAC、ICP算法
ACM算法分类:http://www.kuqin.com/algorithm/20080229/4071.html 一: 拟合一个平面:使用SVD分解,代码里面去找吧 空间平面方程的一般表达式为: A ...
- 点云匹配和ICP算法概述
Iterative Closest Point (ICP) [1][2][3] is an algorithm employed to minimize the difference between ...
随机推荐
- ORA-01790: 表达式必须具有与对应表达式相同的数据类型
出现这种错误,要先看一下是不是sql中有用到连接:union,unio all之类的,如果有,需要注意相同名称字段的数据类型一定要相同.
- 架构发展史Spring Cloud
转自:https://www.iteye.com/news/32734 Spring Cloud作为一套微服务治理的框架,几乎考虑到了微服务治理的方方面面,之前也写过一些关于Spring Cloud文 ...
- HBase 表和Region
- 大牛就别进来了.npm --save-dev --save 的区别
--save-dev 是你开发时候依赖的东西,--save 是你发布之后还依赖的东西. 比如,你写 ES6 代码,如果你想编译成 ES5 发布那么 babel 就是devDependencies.如果 ...
- PAT甲级——A1040 Longest Symmetric String
Given a string, you are supposed to output the length of the longest symmetric sub-string. For examp ...
- 使用ssh时报错:Service对象空指针异常
有可能是spring容器不能自动生成service对象,导致空指针异常,常见的情况可能是在service前面加@Service注释
- 使用MySQL会话变量实现窗口函数
一.MySQL窗口函数 (1) 序号函数 row_number()在相等的两条记录上随机排序,但序号按照1.2递增,然后后面的序号继续递增为3,中间不会产生序号间隙: rank()/dense_ran ...
- Eureka客户端无法连接服务注册中心
转载自:https://my.oschina.net/kousm/blog/2249003 服务端 application.yml配置 spring: application: name: eurek ...
- CentOS 7 忘记root密码的修改方法
1.开机按esc 2.选择CentOS Linux (3.10.0-693.......) 按 e 键: 3.光标移动到 linux 16 开头的行,找到 ro 改为 rw init=sysr ...
- windows console 控制台自启动
var fileName = Assembly.GetExecutingAssembly().Location; System.Diagnostics.Process.Start(fileName);