迭代最近点算法 Iterative Closest Points
研究生课程系列文章参见索引《在信科的那些课》
基本原理
假定已给两个数据集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,应该为“手动匹配”)。
,为的转置矩阵。
对于每一对矩阵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);
循环结束后能得到较好的匹配效果:
迭代最近点算法 Iterative Closest Points的更多相关文章
- 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算法,下 ...
- ICP算法(迭代最近点)
参考博客:http://www.cnblogs.com/21207-iHome/p/6034462.html 最近在做点云匹配,需要用c++实现ICP算法,下面是简单理解,期待高手指正. ICP算法能 ...
- ICP(迭代最近点)算法
图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对 ...
- K closest points
Find the K closest points to a target point in a 2D plane. class Point { public int x; public int y; ...
- [Swift]LeetCode973. 最接近原点的 K 个点 | K Closest Points to Origin
We have a list of points on the plane. Find the K closest points to the origin (0, 0). (Here, the d ...
- [Solution] 973. K Closest Points to Origin
Difficulty: Easy Problem We have a list of points on the plane. Find the K closest points to the ori ...
- LeetCode 973 K Closest Points to Origin 解题报告
题目要求 We have a list of points on the plane. Find the K closest points to the origin (0, 0). (Here, ...
- Microsoft - Find the K closest points to the origin in a 2D plane
Find the K closest points to the origin in a 2D plane, given an array containing N points. 用 max hea ...
随机推荐
- 高仿360界面的实现(用纯XML和脚本实现)
源码下载:360UI 本项目XML的桌面渲染依赖GQT开源项目(请感兴趣的朋友加入QQ讨论群:101189702,在群共享文件里下载GQT源码),以下是360界面实现的全部XML代码,所有的代码都在3 ...
- Spring Boot使用Log4j Implemented Over SLF4J生成日志并在控制台打印
Spring Boot设置切面,执行方法的时候在控制台打印出来,并生成日志文件 引入依赖: <!--日志--> <dependency> <groupId>org. ...
- 美团外卖iOS多端复用的推动、支撑与思考
背景 美团外卖2013年11月开始起步,随后高速发展,不断刷新多项行业记录.截止至2018年5月19日,日订单量峰值已超过2000万,是全球规模最大的外卖平台.业务的快速发展对技术支撑提出了更高的要求 ...
- maven 发布jar包到远程仓库
有的时候我们需要发布一些自己写的相关jar包到maven私服,供项目组使用. 首先在setting.xml文件添加,这里 注意 要保证该账户有发布的权限 <servers> <ser ...
- webstorm2018最新激活码license server
2018.7.5最新激活码: license server:https://s.tuzhihao.com:666/ 以后持续更新....
- Urllib库的基本用法
1.什么是url? 统一资源定位符是对可以从互联网上得到的资源的位置和访问方法的一种简洁的表示,是互联网上标准资源的地址. 基本URL包含模式(或称协议).服务器名称(或IP地址).路径和文件名,如“ ...
- 浏览器在线查看pdf文件 pdf.js的使用教程
谷歌浏览器可以直接在线查看pdf,而IE内核浏览器无法在线查看,默认是下载. 这里用到的是pdf.js,不仅支持IE内核浏览器,而且兼容手机查看pdf 官网地址:http://mozilla.gith ...
- 订单超时、活动过期解决方案:php监听redis key失效触发回调事件
Redis 的 2.8.0 版本之后可用,键空间消息(Redis Keyspace Notifications),配合 2.0.0 版本之后的 SUBSCRIBE 就能完成这个定时任务的操作了,定时的 ...
- android activity 窗口 样式
韩梦飞沙 韩亚飞 313134555@qq.com yue31313 han_meng_fei_sha 将 活动 设置成 窗口样式, 可以设置 主题 为 对话框, 或者 半透明. 安卓:主题= ...
- 【搜索+DP】codevs1066-引水入城
[题目大意] 一个N行M列的矩形,如上图所示,其中每个格子都代表一座城 市,每座城市都有一个海拔高度.现在要在某些城市建造水利设施.水利设施有两种,分别为蓄水厂和输水站.蓄水厂的功能是利用水泵将湖泊中 ...