博客

NDT介绍

正态分布变换（NDT）是一种可以用在三维点云配准的算法。因为不需要对应特征点的特征计算和匹配，理论上时间要比其他方法快。

NDT算法的基本思想是先跟据参考数据来构建多维变量的正态分布，如果变换点和目标点云（参考数据）能够匹配很好，概率密度会很大。所以采用优化的方法求取使得概率密度最大的变换参数。

算法基本步骤

1.将参考点云所占空间化成指定大小的网格或体素，并计算每个网络的多维正态分布参数。计算每个网格的多维正态分布。

2.初始化变换参数p。

3.对于要匹配的点云。通过变换T将其转换到参考点云的网格中。

4.根据正态分布参考计算每个转换点的概率密度。

5.NDT配准得分通过对每个网格计算出的概率密度相加得到。

6.根据牛顿优化算法对目标-score进行优化，寻找变换参数p使得score的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵

7.跳回第三步，直到收敛为止。

代码（

摘自PCL官网教程代码

）

代码使用的点云下载链接：


room_scan1.pcd



room_scan1.pcd

#include <iostream>
#include <thread>

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

using namespace std::chrono_literals;

int
main (int argc, char** argv)
{
  // Loading first scan of room.
  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;

  // Loading second scan of room from new perspective.
  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;

  // Filtering input scan to roughly 10% of original size to increase speed of registration.
  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;

  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.01);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (0.1);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);

  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);

  // Setting point cloud to be aligned.
  ndt.setInputSource (filtered_cloud);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);

  // Set initial alignment estimate found using robot odometry.
  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 ();

  // Calculating required rigid transform to align the input cloud to the target cloud.
  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;

  // Transforming unfiltered, input cloud using found transform.
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // Saving transformed input cloud.
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // Initializing point cloud visualizer
  pcl::visualization::PCLVisualizer::Ptr
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);

  // Coloring and visualizing target cloud (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");

  // Coloring and visualizing transformed input 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");

  // Starting visualizer
  viewer_final->addCoordinateSystem (1.0, "global");
  viewer_final->initCameraParameters ();

  // Wait until visualizer window is closed.
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    std::this_thread::sleep_for(100ms);
  }

  return (0);
}

代码解释：

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

首先需要添加NDT算法和降采样需要的头文件。这里降采样采用 approximate voxel filter法，降采样算法也可以选取其他的方法。

// Loading first scan of room.
  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;

  // Loading second scan of room from new perspective.
  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点云文件。

// Filtering input scan to roughly 10% of original size to increase speed of registration.
  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算法不需要用到单独的点，而是用到在每一个体素当中包含的点的统计信息。

// Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.01);
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (0.1);
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);
  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35);

我们引入NDT算法，并且设定和点云规模有关的参数，这和算法本身的voxelized数据结构有关。 上面的参数适合我们例子中的情况，对于小的物体我们需要缩小参数。

Transformation Epsilon parameter决定了变换向量[x, y, z, roll, pitch, yaw]最小允许的增量（以米为单位）。当增量变化低于这个阈值，配准过程结束。Step Size parameter 规定了MT线性搜索（More-Thuente line search）方法允许的最大步长。算法会采用在规定值之下最适合的步长，在接近优化目标时减小步长。过大的最大值能减少迭代次数，但也面临着 overshooting 和在不希望的局部最小值停止的风险。Resolution parameter规定NDT算法数据结构体素的大小。每个体素数据包括点云统计数据，平均值，协方差等。这些统计数据用来构建多元高斯分布并且用来计算和优化点在体素区域的概率密度分布。这个参数和点云规模息息相关。每个体素要至少填充6个点，但又得小一些，满足能特异描述环境的功能。

最后一个参数规定最大迭代次数，在绝大多数情况，迭代应该因为Transformation Epsilon终止，而并非达到迭代的最大值。这个参数防止程序运行时间太长。

// Setting point cloud to be aligned.
  ndt.setInputSource (filtered_cloud);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);

然后，我们把点云传进算法。输入点云将通过变换转换到目标点云位置上。

  // Set initial alignment estimate found using robot odometry.
  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 ();

我们接下定义点云之间粗匹配的变换矩阵。虽然算法可以不需要这个参数，但它可以让我们得到更好的结果，在点云差距很大时效果更加明显。

// Calculating required rigid transform to align the input cloud to the target cloud.
  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;

算法的最后一步，我们配准点云。最后的输入点云配准的结果输出到output cloud。我们最后输出图像和Euclidean fitness score（由输出和目标点云距离平方和组成）。

// Transforming unfiltered, input cloud using found transform.
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // Saving transformed input cloud.
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

在配准后，输出结果是经过降采样后输入点云配准后的结果。为了得到原始点云的变换结果，需要对原始点云进行变换，然后保存到room_scan2_transformed.pcd。

// Initializing point cloud visualizer
  pcl::visualization::PCLVisualizer::Ptr
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);

  // Coloring and visualizing target cloud (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");

  // Coloring and visualizing transformed input 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");

  // Starting visualizer
  viewer_final->addCoordinateSystem (1.0, "global");
  viewer_final->initCameraParameters ();

  // Wait until visualizer window is closed.
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    std::this_thread::sleep_for(100ms);
  }

这段代码不是算法必须的，只是方便查看，通过图形化界面展示变换后的结果。

编译运行代码

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(normal_distributions_transform)

find_package(PCL 1.5 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})


add_executable(normal_distributions_transform normal_distributions_transform.cpp)
target_link_libraries (normal_distributions_transform ${PCL_LIBRARIES})

如果报错可以尝试加上一行

set(CMAKE_CXX_FLAGS "-std=c++14 -o3")

因为其中一些函数采用c++14标准。

之后运行程序就可以得到结果了，哈哈哈。

补充:调参技巧

关于代码中几个重要的参数的调参，可以参考

PCL入门-配准好用之NDT


ndt.setTransformationEpsilon (0.01); // 用处不大的参数。最小精度。一旦增量变化低于此阈值，对齐就会终止。

ndt.setMaximumIterations (35); // 用处不大的参数。最大迭代次数。

ndt.setStepSize (0.1); // 需要调。允许的最大步长。搜索算法确定低于此最大值的最佳步长，在接近最佳解时缩小步长。调的经验：step的话可以先放大后变小，当然只是经验公式

ndt.setResolution (1.0); // 需要调。网格的体素分辨率。它需要足够大，每个体素至少包含6个点，但小到足以唯一地描述环境。调的经验：resolution在追求精度的情况下最好与点密度相等或者低一个数量级,这里是激光点云，所以我设了1米。

参考：



NDT（Normal Distributions Transform）算法原理与公式推导



pcl官网



读书笔记-使用正态分布变换（NDT）进行点云配准