【点云配准算法】【NDT】
0 前言
- 这篇文章的目的是为了记录对点云配准算法的学习，之前学习过ICP、PnP等，后面看到NDT算法，故记录
1 NDT（正态分布变换算法）
1.1 NDT算法介绍
-
正态分布变换算法，又名为 **NDT （ Normal Distributions Transform ）**算法 。该算法是一个一次性初始化的工作。和 ICP 算法相比， NDT 算法的配准效果同 ICP算法相似，其改进实质等同于将 ICP 算法栅格化，是另一种传统的点云配准算法。该算法实质是通过计算点云与点云之间的姿态转换，来确定最优匹配。而判断点云与点云之间是否为最优匹配的方法是基于标准最优化技术。
-
NDT 算法一般过程如下：
已知有两幅点云，分别为源点云 P 和目标点云 Q 。
（ 1 ）将源点云 P 所在空间划分为一个单元一个单元的网格（即三维空间在二维空间上的投影）。
（2）根据所划分单元网格内点的分布情况，计算该单元网格的正太分布 PDF参数。
（3）根据转移矩阵，将目标点云 Q 内的点进行变换；
（4）统计源点云 P 所在空间划分网格内目标点云点的个数，根据点的分布情况计算对应的概率分布函数；
（5）求解所有点的最优值，也即求解目标点云与源点云之间的刚体变换。 -
总结：
（1）不同于 ICP 算法，NDT 算法在配准过程中需要删除距离不正确的点对。但是由于每一次的迭代过程计算代价较高，需要搜索所有的临近点。
（2）在配准时，NDT 算法能够直接进行配准和计算，而不需要通过对应点的特征。所以在计算邻近搜索匹配点时不会消耗过多代价，概率密度函数计算较为简单，极大的提高了算法的效率 。因此从时间复杂度的角度上来看，NDT 算法优于 ICP 算法。
1.2 NDT算法在PCL库的使用
1.2.1 数据的体素滤波处理
- 体素格滤波器VoxelGrid 下采样 在网格内减少点数量保证重心位置不变
pcl::VoxelGrid
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Source(new pcl::PointCloud<pcl::PointXYZ>());
pcl::VoxelGrid<pcl::PointXYZ> cloud_filter_;//创建滤波对象
cloud_filter_.setLeafSize(1.3, 1.3, 1.3);/设置滤波时创建的体素体积为1.3m的立方体
cloud_filter_.setInputCloud(cloud_Source);//设置需要过滤的点云(指针) 给滤波对象
cloud_filter_.filter(*filtered_cloud_ptr);//执行滤波处理，存储输出
1.2.2 进行NDT处理
//初始化NDT对象
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
//根据输入数据的尺寸设置相关参数
ndt.setTransformationEpsilon(0.0001);//为终止条件设置最小转换差异
ndt.setStepSize(0.1); //为more-thuente线搜索设置最大步长
ndt.setResolution(0.01); //设置NDT网格网格结构的分辨率（voxelgridcovariance）
ndt.setMaximumIterations(100);//设置最大迭代次数
ndt.setInputSource(filtered_cloud_ptr);//源点云
//cloud_Target 同样也是 pcl::PointCloud<pcl::PointXYZ>::Ptr 格式
ndt.setInputTarget(cloud_Target);//目标点云
// 设置使用机器人测距法得到的粗略初始变换矩阵结果
Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(1.79387, 0.72047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
// 计算需要的刚体变换以便将输入的源点云匹配到目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 第一个是源点云匹配过去之后的点云信息
// 第二个参数是 预先估计的位姿初值
ndt.align(*output_cloud, init_guess);
//获取匹配的位姿
//Ttarget_source
Eigen::Matrix4f pose = ndt.getFinalTransformation();
//获得匹配的的分
float score = ndt.getFitnessScore();