研究生课程系列文章参见索引《在信科的那些课》

基本原理

假定已给两个数据集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. 求使 

平移中心点的 具体代码为：

[cpp] view plaincopy
  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:

[cpp] view plaincopy
  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的最小特征值和特征向量，具体代码：

[cpp] view plaincopy
  1. //使用奇异值分解计算B的特征值和特征向量
  2. double eigen, qr[4];
  3. MatrixEigen(B, &eigen, qr, 4);
[cpp] view plaincopy
  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. 求使 

因此还需要通过中心点计算平移矩阵。

[cpp] view plaincopy
  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中离他最近的点作为对应点。在某一步利用前一步得到的，求使下述函数最小的：

这里， 

[cpp] view plaincopy
  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);

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

封装后的效果图：

from: http://blog.csdn.net/xiaowei_cqu/article/details/8470376

迭代最近点算法 Iterative Closest Points相关推荐

  1. 迭代最近点算法Iterative Closest Point（ICP）以及c++实现代码

    有两组对应的点集(corresponding point sets): 求欧式变换  使得: ICP 算法基于最小二乘法进行迭代计算,使得误差平方和达到极小值: 可通过以下三个步骤进行求解: (1)定 ...

  2. 迭代最近点（Iterative Closest Point, ICP）算法及matlab实现

    前言 通常,使用RGB-D相机或是其他方法获取到物体的三维点云后,由于采集设备不同.拍摄视角不同等等因素的影响,即使是同一个物体所得到的点云也会有较大的差异,主要是旋转或者平移的变化.对于一组图像数据 ...

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

    1. 定义 ICP(Iterative Closest Point,迭代最近点)算法是一种迭代计算方法,主要用于计算机视觉中深度图像的精确拼合,通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合 ...

  4. 绝对不可错过的图形学算法！迭代最近点算法——ICP算法

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

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

    最近在做点云匹配,需要用c++实现ICP算法,下面是简单理解,期待高手指正. ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐 ...

  6. 迭代近邻算法Iterative Closest Point, ICP

    参考的文章链接: https://zhuanlan.zhihu.com/p/35893884 https://blog.csdn.net/u014709760/article/details/9924 ...

  7. VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）

    1.Iterative Closest Points算法 点云数据配准最经典的方法是迭代最近点算法(Iterative Closest Points,ICP).ICP算法是一个迭代的过程,每次迭代中对 ...

  8. ICP（迭代最近点）算法推导详解

    概述 ICP用来求解3D-3D的位姿估计问题.假设有一组配对好的3D点,例如对两幅RGB-D图像进行匹配,或者是EPnP方法中已知参考的在相机坐标系中的坐标和在世界坐标系的坐标(参考我PnP的博文): ...

  9. 迭代硬阈值算法IHT:Iterative Hard-Thresholding

    迭代硬阈值算法IHT:Iterative Hard-Thresholding 前言 硬阈值函数 迭代硬阈值算法 参考 前言 最近在学习压缩感知的重构算法,重构算法整体来看分为三大类: ①贪婪迭代类算法 ...

最新文章

  1. 李飞飞：人工智能应用广泛 但场景理解不如2岁孩子
  2. 系统架构的演变 -----自 罗文浩
  3. airpodspro窃听模式_AirPods Pro实时收听怎么关闭? AirPods Pro实时收听的使用方法
  4. 1、Flutter Widget(IOS Style) - CupertinoApp;
  5. 记一次参加 CrossOver Meetup 的经历
  6. 06 ansible剧本功能实践介绍
  7. 每日小记2017.3.7
  8. B/S开发框架Web安全问题及防范规范之挂马和WebShell
  9. matlab调用自己写的函数时报错： reference to a cleared variable
  10. python 魔法方法
  11. 字节跳动入局全网搜索；思科回应中国区裁员；IntelliJ IDEA 新版发布！ | 极客头条...
  12. 原生JS实现粘贴到剪贴板
  13. mysql5.7.20 sql mode_MySQL5.7中的sql_mode默认值带来的坑及解决方法
  14. CCF201903-5 317号子任务（100分题解链接）
  15. 外卖侠cps V5.6版本小程序源码_支持多种CPS收益和流量主收益
  16. Java 正则表达式
  17. AXI总线简介(二)
  18. weblogic windows 打补丁_weblogic的版本及打补丁
  19. win8 配置要求
  20. 老外的各种no-sql数据库的比较贴

热门文章

  1. 【caffe-Windows】mnist实例编译之model的使用-classification
  2. mysql t-sql,将T-SQL转换为MySQL
  3. [搜索]一种改进的召回率准确率公式计算方式
  4. Spring-基于Spring使用自定义注解及Aspect实现数据库切换
  5. Oracle增加修改删除字段/主键
  6. linux图形图像三剑客,就linux三剑客简单归纳
  7. 索爱麦克风免驱动的语音录入测试
  8. 人脸检测算法_目前最强!开源人脸检测算法:RetinaFace
  9. python旋转矩阵_python实现回旋矩阵方式(旋转矩阵)
  10. 【Java】ArrayList 列表的泛型