(续前)
五、使用NDT对齐概述
在3D中,网格空间是由立方体而不是正方形组成的
PDF现在是三维的,因为PDF的方程可以推广到任何维度。q变成3x1,而中心向量的和变成3x3。
g现在是6x1, H现在是6x6
Δp的计算仍然与2D情况相同
使用NDT进行对齐
现在已经实现了ICP,在下一个练习,进行相同的操作实现NDT。PCL中NDT的建立与ICP的步骤非常相似,但有一些不同。 首先让我们看看sm2-main.cpp中的NDT头函数,并将其与之前使用的sm1-main.cpp中的ICP头函数进行比较。
//NDT
Eigen::Matrix4d NDT(pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt, PointCloudT::Ptr source, Pose startingPose, int iterations)
//ICP
Eigen::Matrix4d ICP(PointCloudT::Ptr target, PointCloudT::Ptr source, Pose startingPose, int iterations)
这两个函数的不同之处在于,来自ICP的目标参数现在在NDT中是一个ndt对象。具有点云图输入和集合空间分辨率的ndt对象只在开始时设置一次,然后传递给NDT函数使用。在sm2-main.cpp中,已经定义ndt对象。
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
然后,任务将是设置输入云、分辨率和NDT所需的任何额外参数设置。下面是如何设置ndt的一些主要参数。
#INPUT pcl::PointCloud::Ptr 输入点云指针
#RES double 单元格大小,~1m是测试初始值
#STEP double 最大步长,~1m是测试初始值
ndt.setInputTarget (##INPUT);
ndt.setResolution (##RES);
ndt.setStepSize (##STEP);
要了解更多关于设置ndt参数的细节,请查看这个链接:How to use Normal Distributions Transform — Point Cloud Library 1.12.1-dev documentation
用PCL实现NDT功能
完成NDT可以在几行代码中完成。
##ITERATIONS int 从函数头的ITERATIONS迭代值设置int
##SOURCE pcl::PointCloud::Ptr 从函数头的SOURCE值设置源
##TRANSFORM Eigen::Matrix4d intial pose 从函数头获得startingPose
helper.h中的函数transfrom3D可以用来将输入的Pose对象转换为一个矩阵。
##OUTPUT pcl::PointCloud<pcl::PointXYZ> 扫描对齐
ndt.setMaximumIterations (##ITERATIONS);
ndt.setInputSource (##SOURCE);
ndt.align (##OUTPUT, ##TRANSFORM);
ndt的最终矩阵变换可以通过调用来获取
ndt.getFinalTransformation ().cast<double>();
测试 ICP vs NDT
一旦实施了NDT,就可以使用ICP练习中的相同步骤进行测试,并对不同起始姿势的时间和误差进行基准测试。ICP测试的代码可以下载并复制到本测试中,以便同时测试ICP和NDT。sm1-main.cpp的完成部分可以复制并粘贴到sm2-main.cpp中。enum匹配选择器现在将有三个值,enum Registration{ Off, Icp, Ndt};
六、NDT对齐---参考代码
NDT函数
Eigen::Matrix4d NDT(pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt, PointCloudT::Ptr source, Pose startingPose, int iterations){
pcl::console::TicToc time;
time.tic ();
Eigen::Matrix4f init_guess = transform3D(startingPose.rotation.yaw, startingPose.rotation.pitch, startingPose.rotation.roll, startingPose.position.x, startingPose.position.y, startingPose.position.z).cast<float>();
// Setting max number of registration iterations.
ndt.setMaximumIterations (iterations);
ndt.setInputSource (source);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ndt (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*cloud_ndt, init_guess);
//cout << "Normal Distributions Transform has converged:" << ndt.hasConverged () << " score: " << ndt.getFitnessScore () << " time: " << time.toc() << " ms" << endl;
Eigen::Matrix4d transformation_matrix = ndt.getFinalTransformation ().cast<double>();
return transformation_matrix;
}
设置NDT的分辨率和点云目标(地图)
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon (.0001);
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize (1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution (1);
ndt.setInputTarget (mapCloud);
创建体素过滤器(the Voxel Filter)
// cloudFiltered = scanCloud; // Make sure you removed this line
pcl::VoxelGrid<PointT> vg;
vg.setInputCloud(scans[0]);
double filterRes = 0.5;
vg.setLeafSize(filterRes, filterRes, filterRes);
typename pcl::PointCloud<PointT>::Ptr cloudFiltered (new pcl::PointCloud<PointT>);
vg.filter(*cloudFiltered);
更改迭代Iterations次数
if(matching == Ndt)
// Change 0 to 3 (or other positive number)
transform = NDT(ndt, cloudFiltered, pose, 3);