pcl 点云配准 Registration 正态分布变换NDT配准 迭代最近点ICP配准 非线性ICP 配准

点云配准 Registration

在逆向工程，计算机视觉，文物数字化等领域中，由于点云的不完整，旋转错位，平移错位等，
使得要得到的完整的点云就需要对局部点云进行配准，为了得到被测物体的完整数据模型，
需要确定一个合适的坐标系，将从各个视角得到的点集合并到统一的坐标系下形成一个完整的点云，
然后就可以方便进行可视化的操作，这就是点云数据的配准。

实质就是把不同的坐标系中测得到的数据点云进行坐标系的变换，以得到整体的数据模型，
问题的关键是如何让得到坐标变换的参数R（旋转矩阵）和T（平移向量），
使得两视角下测得的三维数据经坐标变换后的距离最小，

目前配准算法按照过程可以分为整体配准和局部配准。
PCL中有单独的配准模块，实现了配准相关的基础数据结构，和经典的配准算法如ICP。 

给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，
使得两个点集能统一到同一坐标系统中，即配准过程

求得旋转和平移矩阵
P2 = R*P1  + T　　　　[R t]

点云配准的概念也可以类比于二维图像中的配准，

只不过二维图像配准获取得到的是x，y，alpha，beta等放射变化参数

三维点云配准可以模拟三维点云的移动和对其，也就是会获得一个旋转矩阵和一个平移向量，
通常表达为一个4×3的矩阵，其中3×3是旋转矩阵，1*3是平移向量。
严格说来是6个参数，因为旋转矩阵也可以通过罗格里德斯变换转变成1*3的旋转向量。

常用的点云配准算法有两种：

    1. 正态分布变换方法  NDT  正态分布变换进行配准（normal Distributions Transform） 
    2. 和著名的 迭代最近点 Iterative Closest Point （ICP） 点云配准，

此外还有许多其它算法 列举如下：

    ICP：稳健ICP、point to plane ICP、point to line ICP、MBICP、GICP
    NDT: NDT 3D、Multil-Layer NDT
    FPCS、KFPSC、SAC-IA
    Line Segment Matching、ICL

两个数据集的计算步骤：

  1. 识别最能代表两个数据集中的场景的兴趣点（interest points）（即关键点 keypoints）
  2. 在每个关键点处，计算特征描述符;
  3. 从特征描述符集合以及它们在两个数据集中的x,y,z位置，基于特征和位置之间的相似性来估计对应关系;
  4. 假设数据被认为包含噪声的，并不是所有的对应关系都是有效的，
     所以舍弃对配准过程产生负面影响的那些负影响对应关系;
  5. 利用剩余的正确的对应关系来估算刚体变换，完整配准。

迭代最近点 Iterative Closest Point （ICP）

ICP算法本质上是基于最小二乘法的最优配准方法。
该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。
算法的输入：参考点云和目标点云，停止迭代的标准。
算法的输出：旋转和平移矩阵，即转换矩阵。

使用点匹配时，使用点的XYZ的坐标作为特征值，针对有序点云和无序点云数据的不同的处理策略：
      1. 穷举配准（brute force matching）;
      2. kd树最近邻查询（FLANN）;
      3. 在有序点云数据的图像空间中查找;
      4. 在无序点云数据的索引空间中查找.
特征描述符匹配：
      1. 穷举配准（brute force matching）;
      2. kd树最近邻查询（FLANN）。

in cloud B for every point in cloud A .

错误对应关系的去除（correspondence rejection）:

    由于噪声的影响，通常并不是所有估计的对应关系都是正确的，
    由于错误的对应关系对于最终的刚体变换矩阵的估算会产生负面的影响，
    所以必须去除它们，可以采用随机采样一致性估计，或者其他方法剔除错误的对应关系，
    最终只使用一定比例的对应关系，这样既能提高变换矩阵的估计京都也可以提高配准点的速度。

变换矩阵的估算（transormation estimation）的步骤如下:

      1. 在对应关系的基础上评估一些错误的度量标准
      2. 在摄像机位姿（运动估算）和最小化错误度量标准下估算一个刚体变换(  rigid  transformation )
      3. 优化点的结构  (SVD奇异值分解 运动估计;使用Levenberg-Marquardt 优化 运动估计;)
      4. 使用刚体变换把源旋转/平移到与目标所在的同一坐标系下，用所有点，点的一个子集或者关键点运算一个内部的ICP循环
      5. 进行迭代，直到符合收敛性判断标准为止。


===================================================

迭代最近点算法（Iterative CLosest Point简称ICP算法）:

ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，
其中对应点对的个数，然后通过最小二乘法迭代计算最优的坐标变换，
即旋转矩阵R和平移矢量t，使得误差函数最小，

ICP处理流程分为四个主要的步骤：

    1. 对原始点云数据进行采样(关键点 keypoints(NARF, SIFT 、FAST、均匀采样 UniformSampling)、
       特征描述符　descriptions，NARF、 FPFH、BRIEF 、SIFT、ORB )
    2. 确定初始对应点集(匹配 matching )
    3. 去除错误对应点对(随机采样一致性估计 RANSAC )
    4. 坐标变换的求解

Feature based registration 配准

    1. SIFT 关键点 (pcl::SIFT…something)
    2. FPFH 特征描述符  (pcl::FPFHEstimation)  
    3. 估计对应关系 (pcl::CorrespondenceEstimation)
    4. 错误对应关系的去除( pcl::CorrespondenceRejectionXXX )  
    5. 坐标变换的求解

PCL类的相关的介绍:

对应关系基类　　   pcl::CorrespondenceGrouping< PointModelT, PointSceneT >
几何相似性对应　   pcl::GeometricConsistencyGrouping< PointModelT, PointSceneT >
相似性度量　　　   pcl::recognition::HoughSpace3D
多实例对应关系　   pcl::Hough3DGrouping< PointModelT, PointSceneT, PointModelRfT, PointSceneRfT >
CRH直方图　　　   pcl::CRHAlignment< PointT, nbins_ >
随机采样一致性估计 pcl::recognition::ObjRecRANSAC::Output
      pcl::recognition::ObjRecRANSAC::OrientedPointPair
      pcl::recognition::ObjRecRANSAC::HypothesisCreator
      pcl::recognition::ObjRecRANSAC

      pcl::recognition::ORROctree::Node::Data
      pcl::recognition::ORROctree::Node
      pcl::recognition::ORROctree
      pcl::recognition::RotationSpace

GITHUB

#include <iostream>                 //标准输入输出头文件
#include <pcl/io/pcd_io.h>          //I/O操作头文件
#include <pcl/point_types.h>        //点类型定义头文件
#include <pcl/registration/icp.h>   //ICP配准类相关头文件
#include <sstream> 

int
 main (int argc, char** argv)
{ 
  //创建两个pcl::PointCloud<pcl::PointXYZ>共享指针，并初始化它们
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  float x_trans = 0.7;
  if(argc>=2) {
   std::istringstream xss(argv[1]);
   xss >> x_trans;

  }

  // 随机填充点云
  cloud_in->width    = 5;               //设置点云宽度
  cloud_in->height   = 1;               //设置点云为无序点
  cloud_in->is_dense = false;
  cloud_in->points.resize (cloud_in->width * cloud_in->height);
  for (size_t i = 0; i < cloud_in->points.size (); ++i)//循环随机填充
  {
    cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"//打印处点云总数
      << std::endl;
// 打印坐标
  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " << //打印处实际坐标
      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
      cloud_in->points[i].z << std::endl;
  *cloud_out = *cloud_in;
  std::cout << "size:" << cloud_out->points.size() << std::endl;
//实现一个简单的点云刚体变换，以构造目标点云
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
    cloud_out->points[i].x = cloud_in->points[i].x + x_trans;//　x轴正方向偏移　0.7米
  std::cout << "Transformed " << cloud_in->points.size () << " data points:"
      << std::endl;
//打印构造出来的目标点云
  for (size_t i = 0; i < cloud_out->points.size (); ++i)   
      std::cout << "    " << cloud_out->points[i].x << " " <<
      cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;//创建IterativeClosestPoint的对象
  icp.setInputCloud(cloud_in);                 //cloud_in设置为点云的源点
  icp.setInputTarget(cloud_out);               //cloud_out设置为与cloud_in对应的匹配目标
  pcl::PointCloud<pcl::PointXYZ> Final;        //存储经过配准变换点云后的点云
  icp.align(Final);  
    
//打印经过配准变换点云后的点云
std::cout << "Assiend " << Final.points.size () << " data points:"
      << std::endl;
  for (size_t i = 0; i < Final.points.size (); ++i)   
      std::cout << "    " << Final.points[i].x << " " <<
      			     Final.points[i].y << " " << 
			     Final.points[i].z << std::endl;                      
  //打印配准相关输入信息
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << "Transformation: "<< "\n" << icp.getFinalTransformation() << std::endl;

 return (0);
}

正态分布变换进行配准（normal Distributions Transform）

介绍关于如何使用正态分布算法来确定两个大型点云之间的刚体变换，
正态分布变换算法是一个配准算法，它应用于三维点的统计模型，
使用标准最优化技术来确定两个点云间的最优匹配，
因为其在配准的过程中不利用对应点的特征计算和匹配，
所以时间比其他方法比较快.

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>//滤波类头文件  （使用体素网格过滤器处理的效果比较好）

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int
main (int argc, char** argv)
{
  // 加载房间的第一次扫描点云数据作为目标
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../room_scan1.pcd", *target_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;

  // 加载从新视角得到的第二次扫描点云数据作为源点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../room_scan2.pcd", *input_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;

//以上的代码加载了两个PCD文件得到共享指针，
//后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，
//得到第二组点云变换到第一组点云坐标系下的变换矩阵
// 将输入的扫描点云数据过滤到原始尺寸的10%以提高匹配的速度，
//只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理
//因为在NDT算法中在目标点云 对应的 体素网格数据结构的 统计计算不使用单个点，
//而是使用包含在每个体素单元格中的点的统计数据
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setInputCloud (input_cloud);// 第二次扫描点云数据作为源点云
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from room_scan2.pcd" << std::endl;

  // 初始化正态分布(NDT)对象
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // 根据输入数据的尺度设置NDT相关参数
  ndt.setTransformationEpsilon (0.01);// 为终止条件设置最小转换差异
  ndt.setStepSize (0.1);              // 为more-thuente线搜索设置最大步长
  ndt.setResolution (1.0);            // 设置NDT网格网格结构的分辨率（voxelgridcovariance）

  //以上参数在使用房间尺寸比例下运算比较好，但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体，需要对参数进行很大程度的缩小

  //设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon变换阀值下终止
  //添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
  ndt.setMaximumIterations (35);

  ndt.setInputSource (filtered_cloud);  //源点云
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);  //目标点云

  // 设置使用机器人测距法得到的粗略初始变换矩阵结果
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  // 计算需要的刚体变换以便将输入的源点云匹配到目标点云
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt.align (*output_cloud, init_guess);
   //这个地方的output_cloud不能作为最终的源点云变换，因为上面对点云进行了滤波处理
  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // 使用创建的变换对为过滤的输入点云进行变换
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // 保存转换后的源点云作为最终的变换输出
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // 初始化点云可视化对象
  boost::shared_ptr<pcl::visualization::PCLVisualizer>
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色
 
  // 对目标点云着色可视化 (red).
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  target_color (target_cloud, 255, 0, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "target cloud");

  // 对转换后的源点云着色 (green)可视化.
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  output_color (output_cloud, 0, 255, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "output cloud");

  // 启动可视化
  viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
  viewer_final->initCameraParameters ();   //初始化摄像头参数

  // 等待直到可视化窗口关闭
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}