简介
正态分布变换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.
