ICP 多视角点云对齐方法介绍

    最近的工作涉及到点云重建部分，所以写下这些博客，用来作为笔记，整理自己的思路。这是该部分文章的第一篇，主要介绍点云对齐的方法。物体从扫描到生成完整的网格的过程：    1.通过扫描仪获得物体的多片点云数据    2.任意两片点云之间进行粗对齐，获得精细对齐的初始位置    3.使用ICP算法进行精细对齐    4.全局优化多片点云的位置    5.去除点云的重叠部分，并且网格化

1.0 三维对齐问题&背景

    对实物进行扫描时，往往不能在同一坐标系下将产品的几何数据一次测量。其原因是产品的尺寸超过了扫描仪的测量范围，并且物体之间相互遮挡，扫描仪在一个角度不太可能扫描到物体的完整数据。得到多篇点云数据后，我们需要一种技术将多片点云数据旋转平移到统一的坐标系下，使它们能够组成一个完整的物体，这种技术就是点云对齐。    点云对齐是任意位置的点云的重叠部分相互匹配的过程，分为刚性和非刚性，我们主要介绍刚性点云对齐。刚性对齐计算一个刚性变换矩阵，把一片点云旋转平移过去，保证两片点云对应部分尽可能多的重合。解决的办法可以将他转化成一个优化问题，即在适当的度量空间中，通过求解最佳旋转和平移转换使这样的数据集之间的重叠区域之间的距离是最小的。

1.1 点云粗略对齐常用方法
1. 固定扫描，将物体放在旋转台上，根据旋转角度计算旋转矩阵进行对齐
2. 在物体表面贴标志点，根据标志点的位置指导点云对齐
3. 扫描两片点云后，手工的在两片点云上选择三对匹配点，指导点云对齐

    1.1.1投票法        由于刚体变换只有6个自由度，所以只要确定3对匹配点，就能计算刚体变换。在点云P和Q上分别任选一个基（3个点），计算刚体变换Ti，利用Ti平移和旋转点云，计算P和Q中重合的点的数目ki，如果ki足够大，Ti可以作为结果，否则重复以上过程。如果P中有M个点，Q中有N个点，最差的时间复杂度为O（M^3 * N^3）    1.1.2改进的投票法        在点云P中选取一个基Bi，然后在模型Q中寻找所有可以和Bi相匹配的基，计算所有刚体变换，选择一个最优的Ti，如果Ti足够好，那么作为结果，否则重复以上过程，最差时间复杂度O（M^3 * N^3）        小结：投票法的优点是可以忍受大量的噪声，在较小重叠情况下也能得到满意的结果。但是时间复杂度过高，对于工业界应用，不可接受。    1.1.3特征法        计算两片点云的特征，利用特征的相似性加快两片点云中基的选择，高维特征计算复杂，低维特征区分率不高。        特征是用来辅助匹配基的选择，由于点云对齐本质上是对点云进行平移和旋转操作，所以我们希望特征具有旋转和平移不变性，并且我们希望两片点云中非匹配点的特征是不同的，所以特征需要具备一定的区分率。        PFH特征：对于点云中每个点p，计算p邻域半径r内的点集F，对于F中任意一对点pi和pj（i不等于j）以及它们的法向量ni和nj（pi是两点连线与其法向量构成的角度较小的点），我们定义一个（u，v, w）坐标系：        u = ni        v = （pj - pi） * u        w = u * v

        根据坐标系（u，v, w），我们可以计算三个标量值

    可以将其看作是一个三维特征空间内的点，每一维特征空间均分成k个子空间，整个特征空间就分成了3^k个子空间。用3^k维向量表示表示每个子空间中的特征的分布概率，这3^k维向量就是每个点p的PFH特征。查询点为p在其半径r中的点，任意两点之间计算这个三维特征。    时间效率分析：假设点云数目为n，任一点的r邻域中有m个点，那么PFH特征计算的效率为O（n*2^m）。随着半径增大或者点云数目增加，时间效率增长是指数级的，这是不可接受的。    FPFH特征：相比于PFH更快，并且具有近似的区分率。步骤：    1.对于点云中每个点p，计算p邻域半径r内的点集F，对于F中每个点pi，计算p与pi的特征    2.对于点云中的每个点，重新计算p邻域r内的点集F，FPFH特征的集合等于F中每个点SPFH特征的有权线性加和。    时间效率分析：时间效率为O（n*m^2）

小结：特征法稳定性没有投票法高，如果点云噪声较大，特征不能准确计算的时候，或者当点云重叠部分较小，重叠部分缺少足够的特征点时，会导致对其失败。由于现在扫描设备获得的点云一般噪声比较小，重叠部分的多少是人工可以控制的，所以特征法比较实用。但是当点云规模达到1M或者10M级别时，由于需要计算两片点云中每个点的特征，达到实时对齐还是很困难的。

1.2点云精确对齐ICP算法

ICP（Iterative Closest Point），最近点迭代算法，是最为经典的点云配准算法。其实施步骤为，通过求取源点云和目标点云之间的k个应点对，计算刚体变换Ti，使得k对匹配点的距离之和最小。利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，如果误差大于阈值，那么继续迭代，直到满足给定误差要求为止。也就是说，每次迭代，整个模型都会靠近一点，每次重新找到最近点，然后计算旋转平移矩阵，比较方差误差，不满足就继续迭代ICP算法采用最小二乘计算变换矩阵，有较好的精度，但是采用迭代算法，导致速度较慢，因此，采用ICP配准时，对待配准点云的初始位置有一定的要求，如果初始位置不合理，容易陷入局部最优。总结：ICP算法能够让在不同坐标下的点云数据合并到同一个坐标系中，首先找到一个可用的变换，配准操作实际是要找从坐标系1到坐标系2的一个刚性变换。ICP算法的本质是基于最小二乘法的最优配准方法，该算法重复进行选择对应关系点对，计算最优刚性变换，直到满足正确配准的收敛精度要求。

1.3 邻域表示方法

PFH是点特征直方图，通过参数化查询点与邻域点之间的空间差异，并形成一个多维直方图对点的k邻域几何属性进行描述。直方图所在的高纬超平面为特征表示提供了一个可度量的空间信息，对点云对应曲面的六维姿态来说，它具有旋转不变形，并且在不同的采样密度或者邻域的噪声等级下具有鲁棒性。1.参数化查询点与邻域点之间的空间差异并形成一个多位直方图对点的k邻域几何属性进行描述2.考虑估计法线方向之间所有的相互作用具体来说，PFH表示法是基于点与其k邻域之间的关系以及它们的估计法线来表示点的空间特征。它考虑估计法线方向之间的相互作用，试图补货最好的样本表面变化情况，用来描述样本的空间几何特征。

如图所示，表示的是一个查询点(Pq) 的PFH计算的影响区域，Pq 用红色标注并放在圆球的中间位置，半径为r， (Pq)的所有k邻元素（即与点Pq的距离小于半径r的所有点）全部互相连接在一个网络中。最终的PFH描述子通过计算邻域内所有两点之间关系而得到的直方图，因此存在一个O(k平方) 的计算复杂性。

1.4配准流程

对两个数据源a，b匹配运算步骤如下：

从其中一个数据源a出发，分析其最能代表两个数据源场景共同点的关键点k
在每个关键点ki处，算出一个特征描述子fi
从这组特征描述子{fi}和他们在a和b中的XYZ坐标位置，基于fi和xyz的相似度，找出一组对应
由于实际数据源是有噪的，所以不是所有的对应都有效，这就需要一步一步排除对匹配起负作用的对应
从剩下的较好对应中，估计出一个变换
匹配过程中模块
Keypoints（关键点）

关键点是场景中有特殊性质的部分，一本书的边角，书上印的字母P都可以称作关键点。PCL中提供的关键点算法如NARF，SIFT，FAST。你可以选用所有点或者它的子集作为关键点，但需要考虑的是按毎帧有300k点来算，就有300k^2种对应组合。

Feature descriptors（特征描述子）

根据选取的关键点生成特征描述。把有用信息集合在向量里，进行比较。方法有：NARF, PFH，FPFH, BRIEF 或SIFT.

Correspondences estimation（对应关系估计）

已知从两个不同的扫描图中抽取的特征向量，找出相关特征，进而找出数据中重叠的部分。根据特征的类型，可以选用不同的方法。

点匹配（point matching， 用xyz坐标作为特征），无论数据有无重组，都有如下方法：

brute force matching（强制匹配）,
kd-tree nearest neighbor search (FLANN)（kd树最近邻搜索）,
searching in the image space of organized data（在图像空间搜索有组织的数据）,
searching in the index space of organized data（按索引搜索有组织的数据）.
特征匹配（feature matching， 用特征做为特征），只有下面两种方法：

brute force matching （强制匹配）
kd-tree nearest neighbor search (FLANN)（kd树最近邻搜索）.
除了搜索法，还有两种著名对应估计：

直接估计对应关系（默认），对点云A中的每一点，搜索在B中的对应关系
“Reciprocal” 相互对应关系估计，只用A，B重叠部分，先从A到B找对应，再从B到A找对应。
Correspondences rejection（剔除错误估计）

剔除错误估计，可用 RANSAC 算法，或减少数量，只用一部分对应关系。有一种特殊的一到多对应，即模型中一个点对应源中的一堆点。这种情况可以用最短路径对应或检查附近的其他匹配过滤掉。

Transformation estimation（最后一步，计算变换）

基于上述匹配评估错误测量值；
评估相机不同pose之间所作的刚性变换（运动估计），使错误测量值最小化；
优化点云结构；
E.g, - SVD 运动估计; - Levenberg-Marquardt用不同内核作运动估计；
用刚性变换旋转/平移源数据到目标位置，可能需要对所有点/部分点/关键点内部运行ICP迭代循环；
迭代，直到满足某些收敛标准。
匹配流程总结

1.5PCL点云配准

void PairwiseICP(const pcl::PointCloud<PointT>::Ptr &cloud_target, const pcl::PointCloud<PointT>::Ptr &cloud_source, pcl::PointCloud<PointT>::Ptr &output )  {      PointCloud<PointT>::Ptr src(new PointCloud<PointT>);      PointCloud<PointT>::Ptr tgt(new PointCloud<PointT>);      tgt = cloud_target;      src = cloud_source;      pcl::IterativeClosestPoint<PointT, PointT> icp;      icp.setMaxCorrespondenceDistance(0.1);      icp.setTransformationEpsilon(1e-10);      icp.setEuclideanFitnessEpsilon(0.01);      icp.setMaximumIterations (100);      icp.setInputSource (src);      icp.setInputTarget (tgt);      icp.align (*output);  //  std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;      output->resize(tgt->size()+output->size());      for (int i=0;i<tgt->size();i++)      {          output->push_back(tgt->points[i]);      }      cout<<"After registration using ICP:"<<output->size()<<endl;  }  

参考博客：http://blog.csdn.net/u010696366/article/details/8941938