激光雷达点云配准NDT算法

点云配准主流的有ICP和NDT算法，都是为了通过刚性变化实现两帧点云的对准。主要目的是在一个优化目标下，求出刚性变化的旋转矩阵R和平移矩阵T。关于ICP算法的推导，可以参考前段时间更新的博客：

激光雷达点云配准算法_ego_vehicle的博客-CSDN博客https://blog.csdn.net/qq_53576285/article/details/129679830?spm=1001.2014.3001.5501

一、NDT基于概率统计进行点云配准

1.方差、协方差和协方差矩阵

方差是在概率论和统计方差衡量随机变量或一组数据离散程度的度量。

协方差在概率论和统计学中用于衡量两个变量的总体误差，方差是协方差的一种特殊情况，及两个变量相同的情况下。

协方差矩阵：对于多维随机变量计算各维度的两两协方差，构成一个协方差矩阵。

 2.高斯分布

高斯分布就是我们熟知的正态分布。下面是一维高斯分布的表达式：

 在NDT算法中我们需要使用到高维的高斯分布：

标准差是就是一个协方差矩阵，体现了点与点之间的相关性。

 计算高维的高斯分布可以得到一个概率分布，这个概率分布是点云点集之间的联合概率分布。这个概率分布简化处理后可以成为点云配准的目标函数。

 3.优化迭代的方法

向量的求导：

多元函数的泰勒展开

根据上面的展开，二元函数的泰勒展开可以简写成：

牛顿法利用了二阶偏导数，考虑了梯度变化的趋势，可以更快搜索到极小值。

牛顿法的迭代过程大致如下：

对于n维问题进行求导运算：

迭代更新，用梯度下降的思想更新极小值：

使用二阶偏导的牛顿法，来更新旋转矩阵R和平移矩阵T 。第二部分回全面的介绍一下NDT点云配准的过程。

二、NDT算法的过程与推导

算法的核心思想：

NDT算法的基本思想是先根据目标点云（Target）来构建多维变量的正态分布，如果变换参数能使 得两幅激光点云数据匹配的很好，那么源点云在目标点云中的联合概率密度将会很大。因此，同样可以使用优化的方法求出使得联合概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

算法的流程框图可以写成一下形式：

1）确定目标点云和源点云

2）对目标点云进行栅格划分，并计算各个栅格的均值、协方差、构建高斯分布

3）根据预测对点云进行变换，并计算联合概率密度

目标函数可以简化为：

4）通过牛顿法，计算雅可比和海森堡矩阵，得到优化增量：

 目标函数的优化过程可以参考第一节第三部分迭代优化进行。

5）不断迭代求最优

三、NDT的点云配准代码

# include <iostream>
# include <thread>
# include <pcl/io/pcd_io.h>  //pcl的pcd格式文件的输入输出头文件
# include <pcl/point_types.h>
# include <pcl/registration/ndt.h>  //使用ndt配准头文件
# include <pcl/filters/approximate_voxel_grid.h> //滤波头文件
# include <pcl/visualization/pcl_visualizer.h> //pcl的点云可视化头文件，基于VTK

using namespace std::chrono_literals;

std::string dataPath = "../data/";

//argc和argv参数在用命令行编译程序时有用
int main(int argc, char **argv)
{
    // 读取目标点云pcd文件（Ptr是一个boost共享指针）
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ>(dataPath+"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;

    // 读取源点云pcd文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ>(dataPath+"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;
    /*
        以上代码加载了两个pcd文件得到共享指针，后续配准是完成源点云到目标点云的坐标系的变换矩阵的估计，得到源点云变换到目标点云坐标系下的变换矩阵
    */

   // 将源点云数据过滤到原始尺寸的10%以提高匹配速度，只对源点云进行滤波，而目标点云不需要滤波处理
   // 因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据
    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); //设置每个体素单元格的大小，长宽高，用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;

    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.001); //为终止条件设置最小转换差异，设置前后两次迭代的转换矩阵的最大容差（epsilon），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止
    ndt.setStepSize(0.05); //设置牛顿法优化的最大步长
    ndt.setResolution(1.0); //设置NDT网格结构的分辨率（VoxelGridCovariance），即设置网格化时立方体的边长
    ndt.setMaximumIterations(50); //设置匹配迭代的最大次数，即当迭代次数达到50或者收敛到阈值时，停止优化
    // ndt.setInputCloud(filtered_cloud);
    ndt.setInputSource(filtered_cloud);
    ndt.setInputTarget(target_cloud);

    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ()); //AngelAxisf旋转向量rotation，以z轴为旋转轴，angle=0.6931为旋转角度
    Eigen::Translation3f init_translation(1.79387, 0, 0);
    //Eigen::Translation3d init_translation(1.79387, 0.720047, 0)
    Eigen::Matrix4f init_guss = (init_translation * init_rotation).matrix(); //Eigen的API，Matrixx4f是4*4的float矩阵，matrix为矩阵。init_guss为ndt计算的初始变换估计位置

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, init_guss); //进行ndt配准，计算变换矩阵，output_cloud用来储存input_cloud经过配准后的点云

    //欧几里得适合度得分FitnessScore，该分数计算为从输出云到目标云中最近点的距离的平方。分数越大，准确率越低，配准效果越差
    std::cout << "Normal Distributions Transform has converged: " << ndt.hasConverged() << " score(分数越大，配准效果越差): " << ndt.getFitnessScore() << std::endl;

    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation()); //使用创建的变换对未过滤的源点云（input_cloud）进行变换

    // pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud); //以ascii格式写入文件，保存转换后的点云

    pcl::visualization::PCLVisualizer::Ptr viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer")); //初始化点云可视化界面
    viewer_final -> setBackgroundColor(0, 0, 0); //设置背景色为黑色

    //对目标点云着色（红色）并可视化
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    viewer_final -> addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target color");
    viewer_final -> setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");

    //对转换后的点云着色（绿色）并可视化
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(target_cloud, 0, 255, 0);
    viewer_final -> addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output color");
    viewer_final -> setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");

    viewer_final -> addCoordinateSystem(1.0, "global"); //显示xyz指示轴
    viewer_final -> initCameraParameters(); //初始化摄像头参数等

    //等待直到可视化窗口关闭
    while(!viewer_final -> wasStopped())
    {
        viewer_final -> spinOnce(100);
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); //睡眠100毫秒（0.1秒）
    }

    return 0;
}