基于NDT的前端里程计代码解析

本文主要对Lidar_Localization文件中的三个源文件，front_end_node.cpp、front_end.h/cpp进行详细代码解读。
代码整体部分为下图：

其中，NDT的里程计主函数是src中front_end_node.cpp函数，里面有对前端激光雷达点云里程计进行框架流程描述，具体的函数是在front_end.cpp中进行实现。
头文件include中publisher分为发布点云数据cloud和里程计数据odometry两部分，主要包含Pubish()函数。sensor_data中包含三个传感器数据：激光雷达点云cloud、GNSS、IMU。subscriber中同样包含对于三种传感器数据的接收，主要通过回调函数将数据填充到new_XXX_data中。

front_end_node.cpp中前端激光雷达点云里程计主要分为：点云下采样滤波、点云匹配、位姿估计、关键帧选取等步骤。具体流程为：
1）第一帧点云数据设置为地图
2）提取关键帧点云，拼接成地图，保证稀疏点云数据。
3）除了全局地图，还需要在当前帧附近形成滑动窗局部地图，减小计算量
4）匹配之前需要滤波，对点云稀疏化。

常规的的NDT建图、配准

NDT是一种适用于三维点云的配准算法，可以在PCL中调用正态分布变换NDT(Normal Distributions Transform)进行配准。ICP算法作为点对点的匹配点云算法，当出现小部分差异的环境变化时，使用NDT算法可以很好的消除这种不确定性。NDT没有像ICP一样计算两个点云之间点对点的差距，而是先将参考点云（高精度地图）转换为多维变量的正态分布，利用牛顿法等优化算法根据变换点在参考系中的概率密度之和最大判断匹配。具体公式推导可以参考AdamShan的博客

NDT并没有比较两个点云点与点之间的差距，而是先将参考点云（即高精度地图）转换为多维变量的正态分布（使用正态分布来表示原本离散的点云），如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

NDT基本思想为把空间划分为立方体，点云划分到网格中，每个立方体中存储的是这个立方体被占据的概率密度。

PCL库中常见类及函数介绍

1. setInputCloud (cloud_source) 设置原始点云 
2. setInputTarget (cloud_target) 设置目标点云 
3. setMaxCorrespondenceDistance（） 设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。 
4. 三个迭代终止条件设置： 
1) setMaximumIterations（） 设置最大的迭代次数。迭代停止条件之一 
2) setTransformationEpsilon（） 设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0 
3) setEuclideanFitnessEpsilon（） 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max () 
迭代满足上述任一条件，终止迭代。
5.getFinalTransformation () 获取最终的配准的转化矩阵，即原始点云到目标点云的刚体变换，返回Matrix4数据类型，该数据类型采用了另一个专门用于矩阵计算的开源c++库eigen。 
6. align (PointCloudSource &output) 进行ICP配准，输出变换后点云
7. hasConverged（） 获取收敛状态，注意，只要迭代过程符合上述三个终止条件之一，该函数返回true。 
8. min_number_correspondences_  最小匹配点对数量，默认值为3，即由空间中的非共线的三点就能确定刚体变换，建议将该值设置大点，否则容易出现错误匹配。 
9. 最终迭代终止的原因可从convergence_criteria_变量中获取
注意：getFitnessScore（）用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。

常规NDT的位姿估计

#include <iostream>
#include <pcl/io/pcd_io.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>

// 读取点云pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path){
   
    // Loading first scan.
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path, *cloud) == -1)
    {
   
        PCL_ERROR ("Couldn't read the pcd file\n");
        return nullptr;
    }
    return cloud;
}

void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud){
   
    // 1、初始化点云可视化
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
            viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);

    ///2、对目标点云和匹配后的点云上色
    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");

    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");

    // 3、开始可视化，坐标系和相机参数
    viewer_final->addCoordinateSystem (1.0, "global");
    viewer_final->initCameraParameters ();

    ///4、Wait until visualizer window is closed.
    while (!viewer_final->wasStopped ())
    {
   
        viewer_final->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

int main(int argc, char **argv) {
   
    // 1、读取pcd文件（source和target）
    auto target_cloud = read_cloud_point(argv[1]);
    std::cout << "Loaded " << target_cloud->size () << " data points from cloud1.pcd" << std::endl;

    auto input_cloud = read_cloud_point(argv[2]);
    std::cout << "Loaded " << input_cloud->size () << " data points from cloud2.pcd" << std::endl;

    // 2、过滤输入点云（减少数据量到10%）
    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 cloud2.pcd" << std::endl;
    
    // 3、初始化NDT并设置参数
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.01);// 优化过程是否收敛到的阈值
    ndt.setStepSize(0.1);// 设置牛顿法优化的最大步长
    ndt.setResolution(1.0);// 设置网格化时立方体的边长
    ndt.setMaximumIterations(35); // 优化的迭代次数

    ndt.setInputSource(filtered_cloud); // 待匹配点云
    ndt.setInputTarget(target_cloud); // 目标点云

    // 4、初始化变换参数
    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();

    // 5、开始配准优化并保存配准后的点云图，保存下来
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>