点云NDT配准方法介绍

本文介绍的是另一种比较好的配准算法，NDT配准。所谓NDT就是正态分布变换，作用与ICP一样用来估计两个点云之间的刚体变换。用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。

理论基础

三维配准中经常被提及的配准算法是ICP迭代的方法，这种方法一般般需要提供一个较好的初值，也就是需要粗配准，同时由于算法本身缺陷，最终迭代结果可能会陷入局部最优，导致配准失败，往往达不到我们想要的效果。本文介绍的是另一种比较好的配准算法，NDT配准。所谓NDT就是正态分布变换，作用与ICP一样用来估计两个点云之间的刚体变换。用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。这个配准算法耗时稳定，跟初值相关不大，初值误差大时，也能很好的纠正过来。

对比ICP配准方法需要提出不合适的点对，比如点对之间距离过大，包含了边界点对，每次迭代都要搜索最近点，计算代价较大。正态分布变换（NDT）算法是一种很有用途的点云配准方法，是一个一次性初始化工作，不需要消耗大量的代价计算最近邻搜索匹配点，并且概率密度函数在两幅图像采集之间的额时间可以离线计算出来，但仍在存在的问题很多，包括收敛域差、NDT代价函数的不连续性以及稀疏室外环境下不可靠的姿态估计等。

具体关于两种方法的对比可查看文献：

http://www.diva-portal.org/smash/get/diva2:276162/FULLTEXT02.pdf

那么针对无损检测算法在二维和三维场景下的不足，也会有研究者们提出了相应的解决方法。为了改进二维扫描配准的无损检测收敛域，提出了一种多尺度K均值无损检测（MSKM-NDT）算法，利用K均值聚类对二维点云进行分割，并对多尺度聚类进行扫描配准优化。与标准无损检测算法相比，k-均值聚类方法生成的高斯分布更少，从而可以评估所有高斯聚类的成本函数，从而保证算法的收敛性。由于K均值聚类不能很好地扩展到三维，提出了分段贪婪聚类无损检测（SGC-NDT）方法，作为一种改进和保证三维点云收敛的替代方法，该点云包含与环境地面相对应的点。SGC-NDT算法使用高斯过程回归模型分割接地点，并使用贪婪方法对非接地点进行聚类。区域增长聚类算法提取环境中的自然特征，生成高斯聚类，在无损检测框架内用于扫描配准。涉及到的相关文献如下：

Das A, Waslander SL. Scan Registration using Segmented Region Growing NDT. International Journal of Robotics Research. 2014.

Das A, Servos J, Waslander SL. 3D Scan Registration Using the Normal Distributions Transform with Ground Segmentation and Point Cloud Clustering. In 2013 IEEE International Conference on Robotics and Automation. Karlsruhe, Germany; 2013.

Das A, Waslander SL. Scan Registration with Multi-Scale K-Means Normal Distributions Transform. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. Villamoura, Portugal; 2012.

NDT算法的步骤

（1）该算法的第一步是将扫描占用的空间细分为单元格网格（2D图像中的正方形或3D中的立方体），基于单元内的点分布计算每个单元的PDF。每个单元格中的PDF可以解释为单元格内表面点x^的生成过程。将点云投票到各个格子中，计算每个格子的PDF，PDF可以当做表面的近似表达，协方差矩阵的特征向量和特征值可以表达表面信息（朝向、平整度） 格子内少于3个点，经常会协方差矩阵不存在逆矩阵，所以只计算点数大于5的cell，涉及到下采样方法。

正态分布给出了点云的分段平滑表示，具有连续导数。每个PDF都可以看作是局部表面的近似值，描述了表面的位置以及它的方向和平滑度。2D激光扫描及其相应的正态分布如图6.1所示。图6.2显示了矿井隧道扫描的3D正态分布。

由于目前的工作主要集中在正态分布上，让我们更仔细地研究单变量和多变量正态分布的特征。在一维情况下，正态分布的随机变量x具有一定的期望值μ，并且关于该值的不确定性用方差σ表示。

在多维的情况下，平均值和方差由平均向量 μ和协方差来描述矩阵Σ。协方差矩阵的对角元素表示方差每个变量，非对角线元素表示的是协方差变量。图6.3说明了一维，二维和三维的正态分布

（2）将第二幅扫描点云的每个点按转移矩阵T的变换。

(3)第二幅扫描点落于参考帧点云的哪个 格子，计算响应的概率分布函数

（4）当使用NDT进行扫描点配准时，目标是找到当前扫描点的位姿，以最大化当前扫描的点位于参考扫描表面上的可能性，该位姿是要优化的参数; 也就是说，当前扫描的点云估计的旋转和平移向量。求所有点的最优值，目标函数为

2D-NDT

对于2D配准，有三个要优化的转换参数。让p = [tx，ty，φ]T，其中 tx 和 ty 是平移参数，φ是旋转角度。使用逆时针旋转，2D变换功能是求：

算法步骤：

3D-NDT

NDT的2D和3D配准的主要区别在于空间变换函数T（ p， x）及其偏导数。3D NDT使用欧拉角，有六个转换要优化的参数：三个用于平移，三个用于旋转。可以使用六维参数矢量对姿势进行编码 p6 = [tx，ty，tz，φx，φy，φz]T

使用欧拉序列z-y-x，3D变换函数是

PCL官网例程

向上滑动阅览

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

作者：Being_young博客