正态分布变换NDT算法原理及其在点云配准中的应用

简介

正态分布变换Normal Distributions Transform(NDT)方法最早由Biber于2003年提出。该方法最早用于解决SLAM问题中，激光扫描数据的匹配问题。该方法的核心思路是通过建立数据基于概率密度的表示形式，构建一个对匹配的连续的评估函数，并且连续可微。这样，匹配问题就转换成了对一个连续函数的极值优化问题。下面，我们就来展开介绍下NDT算法的一些理论知识与技术细节。

一. NDT原理

离散的数据匹配，包括图像，点云等数据，很难基于匹配的连续的评估函数，使之难以匹配到一个可微的优化框架中求解。为了解决该问题，我们希望能够为离散的数据形式建立一个相对连续的表示形式，并得到连续可导的评价函数。基于此，NDT被提出。其具体步骤为：

1). 将离散数据划分在不同的区域中；

2). 在每一个区域，求中点：

3). 计算子区域中基于每一个点到中点差值的协方差矩阵：

4). 得到针对于离散数据的NDT表示形式N，具体表示为：

可以看到，NDT的形式是一个基于概率分布的表示形式。下图给出一个直观的示例，来实现对NDT的可视化。

图1.1 NDT可视化 

之前我们提到，之所以建立这样一种形式，是为了得到一个针对匹配评估的一个连续的函数形式，以方便建立优化。基于NDT的表示形式，这个评估函数就能够被构建：

Score代表我们希望得到的评估函数，p是自变量，也是匹配希望获得的变换矩阵。与ICP一致，p由旋转与平移组成：

文献[1]针对的是二维的情况，但是整个变换是可以直接推广到三维上的。参数说明如下：

 整个NDT算法的步骤可以精简为6步：

1). 对一个点云P1或图像建立其NDT表示形式；

2). 初始化参数p（赋0或某一个间隔数据）；

3). 将另外一个准备匹配的点云P2或图像的点按照p变换；

4). 计算Score (xi为P2的一个点，xi‘为xi经过p变换后的位置),如果满足退出条件，则退出；

5). 使用牛顿法建立对p的更新；

6). Loop step 3。

这里再简单提一嘴牛顿法。牛顿法的原理是通过二阶泰勒展开，得到一个针对极值的偏微分方程，以获得对自变量的更新步长，如下：

对x求导取0，就能够推出对应的x值的表达。那么函数的更新步长就能够被获得。注意，这里的变量是一元的，而我们希望更新的p是多元的，因此求导就是一个偏微分方程的形式。这里就要用到Hessian矩阵，但是形式是与上面的公式保持一致的。 

原文是给出了比较详细的推导的，这里我就不再展开了，直接给出推出的结果：

我们计算获得了H和g，即二阶和一阶导，进而可以得到对p的更新步长，实现上面NDT算法第五步对p的更新。到此，我们介绍了整个的NDT算法实现原理。

我在这里列出一些基于NDT的推广工作[2-7]，包括向三维点云的配准问题推广的实现，有兴趣的同学可以下载查看。

二. 基于PCL的NDT实现

PCL库已经实现了NDT算法，如果我们希望使用NDT算法用于点云配准应用，可以直接调用PCL的功能函数即可。

链接：How to use Normal Distributions Transform — Point Cloud Library 1.12.1-dev documentation

我把代码贴在这里，方便直接阅读。

#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 ()
{
  // 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);
}

Reference

[1] P. Biber, W. Straßer. The normal distributions transform: A new approach to laser scan  matching [C]. International Conference on Intelligent Robots and Systems 2003, 3: 2743-2748.

[2] M. Magnusson. The three-dimensional normal-distributions transform: an efficient represen-tation for registration, surface analysis, and loop detection[D]. 2009.

[3] T. Stoyanov, M. Magnusson, H. Andreasson, et al. Path planning in 3D environments using the normal distributions transform[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2010: 3263-3268.

[4] J. Saarinen, H. Andreasson, T. Stoyanov, et al. Normal distributions transform Monte-Carlo localization (NDT-MCL)[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013: 382-389.

[5] JP. Saarinen, H. Andreasson, T. Stoyanov, et al. 3D normal distributions transform occupancy maps: An efficient representation for mapping in dynamic environments[J]. The International Journal of Robotics Research, 2013, 32(14): 1627-1644.

[6] H. Hong, BH. Lee. Probabilistic normal distributions transform representation for accurate 3D point cloud registration[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017: 3333-3338.

[7] H. Sobreira, CM. Costa, I. Sousa, et al. Map-matching algorithms for robot self-localization: a comparison between perfect match, iterative closest point and normal distributions transform[J]. Journal of Intelligent & Robotic Systems, 2019, 93(3): 533-546.