NDT算法的匹配流程

在进行点云匹配时，常用的方法有ICP配准算法以及NDT匹配算法，下面总结一下NDT的一些基本知识。

NDT算法原理：

　　　　NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布， 如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。

       因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数 据将匹配的最好。

NDT算法关键点：

    1、将二维空间划分为固定大小网格，每个网格至少包括3个点（一般5个）

    2、计算网格中点集的均值μ

    3、计算网格中点集的协方差矩阵Σ

    4、网格中的观测到点x的概率p(x)服从正态分布N( μ,Σ)。

NDT代码实现的基本思路总结~

1、加载目标点云，这里以PCD文件的形式进行加载

2、加载需要配准的点云，这里以PCD文件的形式进行加载

3、去除二者周边的点云，这里用最近距离以及最低距离两个阈值进行挑选，类似于点云的裁剪

4、将输入的扫描过滤到原始点云的10%，这里主要使用体素滤波器，主要进行点云的下采样工作

5、初始化正态分布NDT对象

6、设置NDT的依赖参数以及迭代终止条件

7、设置NDT的初始化矩阵参数

8、将降采样后的点云与初始化矩阵进行相乘

9、得到NDT的标准正态分布与分数

10、使用创建的变换对未过滤的点云进行变换

11、保存转换后的输入点云

12、初始化点云可视化界面

13、加载点云

实现代码如下：

#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>

void range_filter(pcl::PointCloud<pcl::PointXYZ> & input_cloud, pcl::PointCloud<pcl::PointXYZ> & output_cloud,const float & min_scan_range_, const float min_z)
    {
        double r;
        pcl::PointXYZ p ;
        for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = input_cloud.begin();
                 item != input_cloud.end(); item++)
        {
            p.x = (double) item->x;
            p.y = (double) item->y;
            p.z = (double) item->z;
            r = p.x * p.x + p.y * p.y;
            if (r > min_scan_range_&&p.z >min_z) //filter the point distance lager than min_scan_range and height lower than min_z
            {
                output_cloud.push_back(p);
            }
        }
    }
int
main (int argc, char** argv)
{
  //加载目标点云pcd
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("test1.pcd", *target_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file test1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
  //加载需要配准点云pcd
  pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("test2.pcd", *input_cloud) == -1)
  {
    PCL_ERROR ("Couldn't read file test2.pcd\n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
//range filter,去除周围点云

pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
range_filter(*input_cloud,*output_cloud1,4,0.1);
range_filter(*target_cloud,*output_cloud2,4,0.1);
   
  //将输入的扫描过滤到原始尺寸的大概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.09, 0.09, 0.09);
  approximate_voxel_filter.setInputCloud (output_cloud1);
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from test2.pcd" << std::endl;
  //初始化正态分布变换（NDT）
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
  //设置依赖尺度NDT参数
  //为终止条件设置最小转换差异
  
  ndt.setTransformationEpsilon (0.001);//定义了[x,y,z,roll,pitch,yaw]在配准中的最小递增量，一旦递增量小于此限制，配准终止
  //为More-Thuente线搜索设置最大步长，步长越大迭代越快，但也容易导致错误
  ndt.setStepSize (0.1);
  //设置NDT网格结构的分辨率（VoxelGridCovariance）
  ndt.setResolution (0.8);//ND体素的大小，单位为m,越小越准确，但占用内存越多
  //设置匹配迭代的最大次数
  ndt.setMaximumIterations (1000);
  // 设置要配准的点云
  ndt.setInputCloud (filtered_cloud);
  //设置点云配准目标
  ndt.setInputTarget (output_cloud2);
  //设置使用机器人测距法得到的初始对准估计结果
  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;//欧式适合度评分
  //使用创建的变换对未过滤的输入点云进行变换
  //ndt.getFinalTransformation (）即最终变换
  pcl::transformPointCloud (*output_cloud1, *output_cloud, ndt.getFinalTransformation ());
  //保存转换的输入点云
  pcl::io::savePCDFileASCII ("test_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 (output_cloud2, 255, 0, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud2, 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);
}