扫描匹配算法的使用（一）

我们之前学习了如何创建ICP和NDT算法，现在使用它们来对齐点云。首先将着手使用ICP和NDT对齐点云，然后可以创建一些自己的点云，并熟悉CARLA模拟器。

一、使用ICP对齐概述

在3D中，物体有6个自由度即<x, y, z>平移和<roll, pitch, yaw>旋转。变换矩阵为:

对于3D的ICP, U、V、R的矩阵现在是3x3而不是2x2，平移t的矩阵现在是3x1而不是2x1。

3D的ICP仍然与2D的情况非常相似。

翻转Roll，俯仰Pitch和偏航Yaw方程

二、使用ICP进行对齐

在本练习中，您将使用PCL的ICP函数将激光雷达输入扫描与点云图对齐。这个练习也将作为一个测试平台，可以测量ICP的收敛速度和实际扫描姿态的误差。从下面的图片中我们可以看到开始的姿势(绿色框带红色扫描)比实际的姿势(红色框)落后2.6米。ICP的目标是提供将偏移量最小化的转换，并使绿框与红框重叠。

点云图用蓝色表示，输入扫描用红色表示。真实姿势是红框，估计姿势是绿框。 

输入扫描体素过滤Voxel Filtering for Input Scan

首先第一个任务是过滤输入扫描，ICP的收敛速度取决于输入扫描的大小，这是因为输入扫描中的每个点都需要在映射点云中搜索一个相关的最近点。当需要实时定位移动的对象时，ICP返回转换的速度越快越好。体素过滤器通过将3D空间分解为规则间隔的立方体网格(每个单元只包含一个点)以降低输入扫描的采样。第一个任务是构造一个PCL体素过滤器，将其输入设置为测量扫描。然后设置网格的单元格大小，它应该设置得足够大，以便ICP能够尽可能快;但也应该设置得足够小，以便在扫描中仍然有良好的清晰度以与地图对齐。
PCL体素过滤器只需要几行代码就可以创建。
PCL依赖项已经包含在sm1-main.cpp文件顶部声明的包含中。

#include <pcl/filters/voxel_grid.h>

#INPUT pcl::PointCloud<pcl::PointXYZ>::Ptr 输入点云指针

#OUTPUT pcl::PointCloud<pcl::PointXYZ>::Ptr 输出点云指针

#RES double 网格的单元格大小，~1m是测试初始值

pcl::VoxelGrid<PointT> vg;
vg.setInputCloud( #INPUT );
vg.setLeafSize(#RES, #RES, #RES);
vg.filter( #OUTPUT);

 要了解更多细节，请查看这里的链接中的示例：Downsampling a PointCloud using a VoxelGrid filter — Point Cloud Library 1.12.1-dev documentation

 用PCL实现ICP功能

下一个任务将是填写sm1-main.cpp中的ICP函数。目前这个函数只返回一个单位矩阵。任何矩阵乘以单位矩阵都是一样的。通过以下步骤可以完成ICP功能。源的表示法是输入扫描，目标是试图对齐的扫描对象，在本例中是点云地图。

Eigen::Matrix4d ICP(PointCloudT::Ptr target, PointCloudT::Ptr source, Pose startingPose, int iterations)

1. 将源转换为startingPose

2. 创建PCL icp对象

3.设置icp对象的值

4. 在icp对象上调用align

5. 如果icp收敛得到icp对象输出变换，并通过startingPose调整它，返回调整后的变换

6. 如果icp没有收敛，则记录消息，并返回原始的单位矩阵

在PCL中使用ICP的一个例子可以在这里找到:How to use iterative closest point — Point Cloud Library 1.12.1-dev documentation，它涵盖了上面的步骤。

可以通过使用pcl::transformPointCloud并提供一个4 x4转换矩阵来完成点云上的转换。例如

#INPUT pcl::PointCloud 输入点云

#OUTPUT pcl::PointCloud 输出点云

#TRANSFORM Eigen::Matrix4d 变换矩阵

pcl::transformPointCloud (#INPUT, #OUTPUT,  #TRANSFORM );

 同样，helper.h的函数transfrom3D，可以用来将输入的Pose对象转换为一个矩阵。

 设置ICP超参数（Hyper-parameters）

ICP的主要超参数如下所示。除了这些设置之外，还可以更改其他参数并进行实验，以查看哪些值会产生最佳结果。有关ICP参数的完整列表，请参阅此链接。

##ITERATIONS int 从函数头设置迭代值iterations

##SOURCE pcl::PointCloud::Ptr 从函数头设置source值

##TARGET pcl::PointCloud::Ptr 从函数头设置target值

##DIS double 如果这个值太小，则无法进行对应

icp.setMaximumIterations (##ITERATIONS);
icp.setInputSource (##SOURCE);
icp.setInputTarget (##TARGET);
icp.setMaxCorrespondenceDistance (##DIS);

 三、测试ICP

一旦完成了输入扫描的过滤和ICP功能，就可以通过编译和运行，然后按键盘上的“i”来测试算法。当成功地将输入扫描与地图对齐，并重叠绿色和红色框时，还有其他控件用于调整和做进一步的测试。

四、ICP对齐---参考代码

ICP函数

Eigen::Matrix4d ICP(PointCloudT::Ptr target, PointCloudT::Ptr source, Pose startingPose, int iterations){

    // Defining a rotation matrix and translation vector
      Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();

      // align source with starting pose
      Eigen::Matrix4d initTransform = transform3D(startingPose.rotation.yaw, startingPose.rotation.pitch, startingPose.rotation.roll, startingPose.position.x, startingPose.position.y, startingPose.position.z);
      PointCloudT::Ptr transformSource (new PointCloudT); 
      pcl::transformPointCloud (*source, *transformSource, initTransform);

      /*
      if( count == 0)
          renderPointCloud(viewer, transformSource, "transform_scan_"+to_string(count), Color(1,0,1)); // render corrected scan
      */

    pcl::console::TicToc time;
      time.tic ();
      pcl::IterativeClosestPoint<PointT, PointT> icp;
      icp.setMaximumIterations (iterations);
      icp.setInputSource (transformSource);
      icp.setInputTarget (target);
    icp.setMaxCorrespondenceDistance (2);
    //icp.setTransformationEpsilon(0.001);
    //icp.setEuclideanFitnessEpsilon(.05);
    //icp.setRANSACOutlierRejectionThreshold (10);

      PointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP output point cloud
      icp.align (*cloud_icp);
      //std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;

      if (icp.hasConverged ())
      {
          //std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
          transformation_matrix = icp.getFinalTransformation ().cast<double>();
          transformation_matrix =  transformation_matrix * initTransform;
          //print4x4Matrix(transformation_matrix);


          /*
          PointCloudT::Ptr corrected_scan (new PointCloudT);
          pcl::transformPointCloud (*source, *corrected_scan, transformation_matrix);
          if( count == 1)
              renderPointCloud(viewer, corrected_scan, "corrected_scan_"+to_string(count), Color(0,1,1)); // render corrected scan
        */
          return transformation_matrix;
      }
    else
          cout << "WARNING: ICP did not converge" << endl;
      return transformation_matrix;

}

创建体素过滤器（the Voxel Filter）

// cloudFiltered = scanCloud; // Make sure you removed this line

pcl::VoxelGrid<PointT> vg;
vg.setInputCloud(scans[0]);
double filterRes = 0.5;
vg.setLeafSize(filterRes, filterRes, filterRes);
typename pcl::PointCloud<PointT>::Ptr cloudFiltered (new pcl::PointCloud<PointT>);
vg.filter(*cloudFiltered);

更改迭代Iterations次数

if(matching == Icp)
    // Change 0 to 3 (or other positive number)
    transform = ICP(mapCloud, cloudFiltered, pose, 3);