简介

正态分布变换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 visualizerpcl::visualization::PCLVisualizer::Ptrviewer_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 visualizerviewer_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.

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

NDT（正态分布变换）算法学习
NDT(正态分布变换)算法学习近期阅读NICP. Dense Normal Based Point Cloud Registration论文,其中的点云配准算法:ICP.NDT.GICP.NICP较 ...
博士论文阅读_仿生群智能优化算法及在点云配准中的应用研究
1.应用领域计算机视觉:高效的仿生群智能优化策略应用于解决复杂的三维点云配准问题. 概念: (1)点云:是一个数据集,数据集中的每个点代表一组X.Y.Z几何坐标和一个强度值,这个强度值根据物体表面反 ...
A* 算法原理以及在二维环境地图中的应用 -- Python 代码实现
上节学习了 Dijkstra 路径规划规划算法,虽然能够找到最短的路径,但是其遍历的搜索过程具有盲目性,因此效率比较低,计算量非常大.而实际中电子地图的结点数量是非常庞大的,Dijkstra 算法在有 ...
Sentinel滑动时间窗限流算法原理及源码解析（中）
文章目录 MetricBucket MetricEvent数据统计的维度 WindowWrap样本窗口实例范型T为MetricBucket windowLengthInMs 样本窗口长度 windo ...
多视图点云配准算法综述
作者:杨佳琪,张世坤,范世超等转载自:华中科技大学学报(自然科学版) 编辑:东岸因为@一点人工一点智能原文:多视图点云配准算法综述摘要:以多视图点云配准为研究对象,对近二十余年的多视图点 ...
激光SLAM之NDT算法（1）算法原理
/在激光SLAM之NDT算法(2)-建图中我会给出实测可用的建图代码,并予以解释代码结构,这里就先讲讲原理吧!!!/ 无人车激光SLAM系统简单可以分为建图和定位两部分,无人车的定位问题,实际上就是要 ...
一文详解NDT算法实现
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨paopaoslam 来源丨泡泡机器人SLAM .PCL源码编辑丨玉玺.lionheart ...
点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
多机器人编队人工势场法协同避障算法原理及实现
多机器人编队(二)多机器人编队人工势场法协同避障算法原理及实现避障算法原理避障算法仿真多机器人协同编队需要将理论和实践紧密地结合起来,其应用包括编队队形生成.保持.变换和路径规划与避障等等都是基 ...
一些论文中使用的统计检验（持续更新）+一些算法原理
写作目的由于偶尔会被问应该在什么情况下使用什么检验,然后有时答不上来或者分析的不是很准确,故而在阅读文献的时候特意记录一下文献中的统计方法. 如果需要在R里看源码的话,可参考该博文https://m ...

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

简介

一. NDT原理

二. 基于PCL的NDT实现

Reference

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

最新文章

热门文章