扫描匹配算法的使用（二）

（续前）

五、使用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);