PCL教程-点云配准之正态分布变换算法（NDT)

本是古典何须时尚 2022-09-12 06:50 34阅读 0赞

原文链接：[How to use Normal Distributions Transform][]

在本篇教程中，我们将学习使用正态分布变换算法（Normal Distributions Transform ，NDT）来确定两个超大点云（都超过100000点）之间的刚性变换。NDT算法是一种配准算法， 它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，更多关于正态分布变换算法的详细的信息，请看 Martin Magnusson 博士的博士毕业论文：

> “The Three-Dimensional Normal Distributions Transform – an Efficient Representation for Registration, Surface Analysis, and Loop Detection.”

PCD文件下载：[room\_scan1.pcd][room_scan1.pcd] 和[room\_scan2.pcd][room_scan2.pcd]

# 程序源码 #

#include <iostream>
    #include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    #include <pcl/registration/ndt.h>
    #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;
      //将输入的扫描过滤到原始尺寸的大概 10% 以提高匹配的速度。
      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);
      //为More-Thuente线搜索设置最大步长
      ndt.setStepSize (0.1);
      //设置NDT网格结构的分辨率（VoxelGridCovariance）
      ndt.setResolution (1.0);
      //设置匹配迭代的最大次数
      ndt.setMaximumIterations (50);
      // 设置要配准的点云
      ndt.setInputCloud (filtered_cloud);
      //设置点云配准目标
      ndt.setInputTarget (target_cloud);
      //设置使用机器人测距法得到的初始对准估计结果
      Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
      Eigen::Translation3f init_translation (1.79387, 0, 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);
      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);
      //对目标点云着色（红色）并可视化
      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");
      //对转换后的目标点云着色（绿色）并可视化
      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);
      viewer_final->initCameraParameters ();
      //等待直到可视化窗口关闭。
      while (!viewer_final->wasStopped ())
      {
        viewer_final->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
      }
      return (0);
    }

# 代码解释 #

一个是NDT算法的头文件，一个是体素滤波器的头文件。滤波器用于下采样，不一定非要使用体素滤波，其他滤波器也可以使用，但是体素滤波器效果较好。

#include <pcl/registration/ndt.h>
    #include <pcl/filters/approximate_voxel_grid.h>

以下代码将过滤输入点云到约10%的原始大小，以提高配准速度。这里用任何其他均匀过滤器都可以，目标点云target\_cloud不需要进行滤波处理，因为NDT算法在目标点云对应的体素Voxel网格数据结构计算时，不使用单个点，而是使用体素的点。即已做了降采样处理。

//将输入的扫描过滤到原始尺寸的大概 10% 以提高匹配的速度。
      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 算法使用一个**体素化数据结构**和**More-Thuente 线搜索**，因此需要缩放一些参数来适应数据集。

以下参数看起来在我们使用的房间尺寸比例下运行地很好，但是它们如果需要处理例如一个咖啡杯的扫描之类更小物体，需要对参数进行很大程度的缩小。

*  在变换中 Epsilon 参数分别从长度和弧度，定义了变换矢量\[ x, y, z,roll,pitch, yaw\]的最小许可的递增量，一旦递增量减小到这个临界值以下 ，那么配准算法就将终止。
 *  步长StepSize参数定义了 More-Thuente 线搜索允许的最大步长，这个线搜索算法确定了最大值以下的最佳步长，当靠近最优解时该算法会缩短迭代步长，在更大的最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。

//根据输入数据的尺度设置NDT相关参数
    ndt.setTransformationEpsilon(0.01); // 为终止条件设置最小转换差异
    // Setting maximum step size for More-Thuente line search.
    ndt.setStepSize(0.1);   // 为More-Thuente线搜索设置最大步长
    //Setting Resolution of NDT grid structure (VoxelGridCovariance).
    ndt.setResolution(1.0); // 设置NDT网格结构的分辨率 VoxelGridCovariance

这个MaximumIterations参数控制了优化程序运行的最大迭代次数，一 般来说，在达到这个限制值之前优化程序就会在 epsilon 变换阈值下终止。添加此最大迭代次数限制能够增加程序鲁棒性,阻止了它在错误的方向运行太久。

//设置匹配迭代的最大次数
    ndt.setMaximumIterations (35);

这里，我们把点云赋给 NDT 配准对象，目标点云的坐标系是被匹配的输入点云的参考坐标系，匹配完成后输入点云将被变换到与目标点云统一坐标系下，当加载目标点云后，NDT 算法的内部数据结构被初始化。

// 设置过滤后的输入源点云（第二次扫描数据）
    ndt.setInputSource (filtered_cloud);
    // 设置目标点云（第一次扫描数据），作为对其的目标。
    ndt.setInputTarget (target_cloud);

在这部分的代码块中我们创建了一个点云配准变换矩阵的初始估计，**虽然算法运行并不需要这样的一个初始变换矩阵，但是有了它易于得到更好的结果**，尤其是当参考坐标系之间有较大差异时（本例即是）， 在机器人应用程序（例如用于生成此数据集的应用程序）中，通常使用里程表数据生成初始转换。

最后，我们准备对齐点云。生成的转换后的输入云存储在输出云中。然后，我们显示对齐的结果以及欧几里得适合度得分FitnessScore，该分数**计算为从输出云到目标云中最近点的距离的平方**。分数越大，准确率越低，配准效果也就越差。

//设置使用机器人测距法得到的初始对准估计结果
      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);
      //ndt.align (*output_cloud);
      std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
    			<< " score(分数越大，配准效果越差): " << ndt.getFitnessScore () << std::endl;

# 实验结果 #

当使用初始变换矩阵时：

ndt.align (*output_cloud, init_guess);

配准效果较好：

![20210923102443277.png][]

![20210923102702289.png][]

> Loaded 112586 data points from room\_scan1.pcd  
> Loaded 112624 data points from room\_scan2.pcd  
> Filtered cloud contains 12433 data points from room\_scan2.pcd  
> Normal Distributions Transform has converged:1 score(分数越大，准确率越低): 0.713633

如果不使用初始变换矩阵：

ndt.align (*output_cloud);

![20210923103046190.png][]

可以发现两者的配准效果相差很大。

[How to use Normal Distributions Transform]: https://pcl.readthedocs.io/projects/tutorials/en/latest/normal_distributions_transform.html#normal-distributions-transform
[room_scan1.pcd]: https://raw.github.com/PointCloudLibrary/data/master/tutorials/room_scan1.pcd
[room_scan2.pcd]: https://raw.github.com/PointCloudLibrary/data/master/tutorials/room_scan2.pcd
[20210923102443277.png]: /images/20220828/71abd6317ca147ea943aac4aeaa4df08.png
[20210923102702289.png]: /images/20220828/1067e582d9b846ed8617472590688b0e.png
[20210923103046190.png]: /images/20220828/7dcb0dcc6e1346788a35360966d42944.png