PCL使用正态分布变换NDT(Normal Distributions Transform)进行配准

综述

正态分布变换算法是一个配准算法，他应用于三维点的统计模型，使用标准最优化技术来确定联合和点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。

应用

无人车的定位问题。实际上就是要找到无人车在地图上的当前位置（并且精度在厘米级别），其中一类方法是slam，他能实现导航定位而不需要地图，然而，真正实现无人车的SLAM是非常困难的，作为交通工具，远距离的行驶会使得实时构建地图的偏差变大。所以在已经拥有高精度地图的前提下去做无人车定位是更加简单，更加现实的。NDT（正太分布变换）就是一类利用高精度地图和实时的激光雷达数据进行高精度定位的技术。摘抄自大神博客：https://blog.csdn.net/adamshan/article/details/79230612

为了知道我们在高精度地图中的确切位置，我们比较lidar扫描得到的点云和我们的地图点云，其中一个问题在于：lidar扫描得到的点云可能和地图的点云存在细微的区别，这里的偏差可能来自于测量误差，也有可能这个“场景”发生了一下变化（比如说行人，车辆）。NDT配准就是用于解决这些细微的偏差问题。

NDT并没有比较两个点云点与点之间的差距，而是先将参考点云(target_cloud)（即高精度地图）转换为多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

API流程（个人理解）

给定input_cloud和target_cloud，其中先对input_cloud进行下采样得到filtered_cloud，然后计算filtered_cloud和target_cloud之间的变换矩阵T，根据该矩阵将input_cloud进行变换得到output_cloud。

NDT代码

  //将输入的扫描过滤到原始尺寸的大概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);//下采样之后的点云保存在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 (35);
  // 设置要配准的点云
  ndt.setInputSource (filtered_cloud);//第二次扫描的点云进行体素滤波的结果.设置输入点云要用setInputSource而不是setInputCloud
  //设置点云配准目标
  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);
  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);

完整代码

#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> ("cloud1.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> ("cloud2.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);//下采样之后的点云保存在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 (35);
  // 设置要配准的点云
  ndt.setInputSource (filtered_cloud);//第二次扫描的点云进行体素滤波的结果.设置输入点云要用setInputSource而不是setInputCloud
  //设置点云配准目标
  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);
  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"));
  //第一个窗口显示input_cloud,第二个窗口显示target_cloud,第三个窗口显示配准之后的结果
  int v1(0),v2(0),v3(0);
  viewer_final->createViewPort(0,0,0.5,1,v1);//第一个窗口位置、颜色
  viewer_final->setBackgroundColor (0,0,0,v1);
  viewer_final->createViewPort(0.5,0,1,1,v2);//第二个窗口位置以及背景颜色
  viewer_final->setBackgroundColor (0,0,0,v2);
 // viewer_final->createViewPort (0.66,0,1,1,v3);//第三个窗口位置、颜色
 // viewer_final->setBackgroundColor (0, 0, 0,v3);
  //对待一个窗口显示
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> input_color(input_cloud,0,255,0);
  viewer_final->addPointCloud<pcl::PointXYZ>(input_cloud,input_color,"input color",v1);




  //对目标点云着色（红色）并可视化
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  target_color (target_cloud, 255, 0, 0);
  //对第二个视口显示
  viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud0",v1);
  viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud",v2 );
  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",v2);
  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);
}

可视化

左侧为input_cloud和target_cloud，右侧为output_cloud和target_cloud