NDT算法配准代码学习，很详细，很多不懂，一句一句解释。

本人超级小白，先是对每一块代码进行学习，结尾是无注释代码。

一、这些是使用正态分布变换算法和用来过滤数据的过滤器对应的头文件

#include   //包含c++的标准输入输出头文件iostream

#include   //PCL的PCD格式文件的输入输出头文件

#include //PCL对各种格式的点的支持头文件

#include  

//NDT配准类相关头文件，点(.)后面可改成icp.h，就成为ICP配准类相关头文件。

#include

//滤波类头文件，ApproximateVoxelGrid体素采样引入头文件；也可以采用VoxelGrid体素采样引入头文件 #include 。对于体素下采样接口功能函数pcl提供了两种方式： pcl::ApproximateVoxelGrid和pcl::VoxelGrid。两种区别主要在于第一种是用每个体素栅格的中心点来近似该体素内的点，提升了速度，但是也损失了原始点云的局部形态精细度。（为什么进行点云滤波在文章结尾）

#include

//PCL的点云可视化的最常用的方法就是PCLVisualizer,它是基于VTK（Visualization Toolkit）的可视化类。引入头文件。

#include

//c++的boost库多线程(Thread)编程，不是很懂这个（boost库本文结尾可解释）

Int main(int argc, char** argv)

// argc和argv参数在用命令行编译程序时有用。int型的argc，为整型，用来统计程序运行时发送给main函数的命令行参数的个数，在VS中默认值为1。char**型的argv，为字符串数组。有看到通过这种argc、argv传入参数到vs，正好解决了我不知道怎么加入PCD文件，之前用直接复制PCD文件到解决方案，可是总是找不到此文件，一直使用的是文件路径代码加入PCD文件。

 

{

二、加载房间的第一次扫描

pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud);

//pcl::PointCloud::Ptr cloud(new pcl::PointCloud);这一句代码的作用是创建一个真实参数为PointXYZ的PointCloud类型的共享指针并初始化。PointCloud是一个模板类，这里使用PointXYZ作为它的真实参数，Ptr其实就是一个boost共享指针。创建点云指针，将这个点云指向它的副本目标点云，总的来说创建目标点云。

if(pcl::io::loadPCDFile("E://shuju//room_scan1.pcd",*target_cloud) == -1)

//读入PCD格式的文件，如果文件不存在则返回-1。指针指向的目标点云为E盘中room_scan1.pcd。

    {

        PCL_ERROR("Couldn't read file room_scan1.pcd \n");

//文件不存在时，返回错误，终止程序。

        return (-1);

    }

//::在c++中表示作用域和所属关系

    std::cout << "Loaded "     //输出一句话，std::是个名称空间标识符。

                  << target_cloud->size()

//用指针调用函数或者获取数据，对象指针/结构指针->成员变量/成员函数。size()返回的是字节数。

                  << " data points from room_scan1.pcd" //输出一句话

                  << std::endl;   //换行

三、加载从新视角得到的房间的第二次扫描

    pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud);

//创建点云指针，将这个点云指向它的副本输入点云。

if (pcl::io::loadPCDFile("E://shuju//room_scan2.pcd", *input_cloud) == -1)

//读入PCD格式的文件，如果文件不存在则返回-1。指针指向的输入点云为E盘中room_scan2.pcd

    {

        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文件得到共享指针，后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，得到第二组点云变换到第一组点云坐标系下的变换矩阵

四、对如此多的点做优化是非常耗时的，使用了voxel_filter对输入点云（input_cloud）进行过滤,减少其数据量到10%左右，而对target_cloud不做滤波处理.在NDT算法中，目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据

    pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud);

//创建点云指针，将这个点云指向它的副本被滤波的点云。创建了滤波的点云。

    pcl::ApproximateVoxelGrid approximate_voxel_filter;

//初始化ApproximateVoxelGrid滤波器

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

//执行滤波，保存过滤结果至filtered_cloud

    std::cout << "Filtered cloud contains "

                  << filtered_cloud->size()

//用指针调用函数或者获取数据，对象指针/结构指针->成员变量/成员函数。size()返回的是字节数。

                  << " data points from room_scan2.pcd"

                  << std::endl;

   

五、根据输入数据的尺度设置NDT相关参数, 主要涉及到以下四个参数：最小转换差异（TransformationEpsilon）、More-Thuente线搜索的最大步长（StepSize）、NDT网格结构的分辨率（Resolution）、匹配迭代的最大次数（MaximumIterations），NDT算法使用一个体素化数据结构和More-Thuente线搜索，因此需要缩放一些参数来适应数据集。

    pcl::NormalDistributionsTransform ndt;

//初始化正态分布变换（NDT）

    ndt.setTransformationEpsilon(0.01);

//即设置变换的（两个连续变换之间允许的最大差值），这是判断我们的优化过程是否已经收敛到最终解的阈值。设置前后两次迭代的转换矩阵的最大容差（epsilion）（文章结尾有解释）。用处不大的参数，最小精度，一旦增量变化低于此阈值，对齐就会终止。在变换中Epsilon参数分别从长度和弧度，定义了变换矢量[x, y, z, roll, pitch, yaw]的最小许可的递增量，一旦递增量减小到这个临界值以下，那么配准算法就将终止.

    ndt.setStepSize(0.1); 

//为More-Thuente线搜索设置最大步长(不懂，本文结尾)，即设置牛顿法优化的最大步长。需要调，允许的最大步长。搜索算法确定低于此最大值的最佳步长，在接近最佳解时缩小步长。调的经验：step的话可以先放大后变小。

    ndt.setResolution(1.0);

//设置NDT网格结构的分辨率（VoxelGridCovariance），即设置网格化时立方体的边长，网格大小设置在NDT中非常重要，太大会导致精度不高，太小导致内存过高，并且只有两幅点云相差不大的情况才能匹配。需要调，网格的体素分辨率。它需要足够大，每个体素包含与点有关的统计数据，平均值，协方差等，统计数据作为一组多元高斯分布用来模拟点云，并且允许我们计算和优化体素内任意位置点的存在概率。该参数是与尺度最相关的，每个体素至少包含6个点，但小到足以唯一地描述环境。调的经验：resolution在追求精度的情况下最好与点密度相等或者低一个数量级,这里是激光点云，所以设了1米

    ndt.setMaximumIterations(35);

  //设置匹配迭代的最大次数，即当迭代次数达到35次或者收敛到阈值时，停止优化。 一般来说在这个限制值之前，优化程序会在epsilon（最大容差）变换阀值下终止，添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长

  ndt.setInputCloud(filtered_cloud);

// 设置要配准的点云，输入点云（滤波后）

    ndt.setInputTarget(target_cloud);

//设置点云配准目标（目标点云）

   

六、设置使用机器人测距法得到的初始对准估计结果，我们对变换参数进行初始化(给一个估计值)，虽然算法不指定初值也可以运行，但是有了它，易于得到更好的结果。Eigen是一个矩阵计算的开源库，PCL中用了Eigen作为第三方做矩阵运算，矩阵变换包括旋转（rotation），平移（translation）和尺度（scale）变换。三维点云要表示变换矩阵，只需要旋转和平移。

    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());

    // AngleAxisd旋转向量rotation，以Z轴为旋转轴，angle=0.6931为旋转角度

    Eigen::Translation3f init_translation(1.79387, 0.720047, 0);

    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

// Eigen的API，Matrix4f 是4*4的float型矩阵,matrix为矩阵。init_guess 为ndt计算的初始换变换估计位置

   

七、计算需要的刚体变换（文章结尾解释）以便将输入的点云匹配到目标点云

    pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud);

    //创建输出点云

    ndt.align(*output_cloud, init_guess);

    //使用了初始变换矩阵，不使用初始变换ndt.align (*output_cloud)可以试试，仿真结果不同。进行ndt配准,计算变换矩阵。output_cloud: 存储input_cloud经过配准后的点云(由于input_cloud被极大的降采样了,因此这个数据没什么用) 。此处output_cloud不能作为最终的源点云变换，因为上面对源点云进行了滤波处理

    std::cout << "Normal Distributions Transform has converged:"

<< ndt.hasConverged() //英文翻译已收敛，此应该是收敛函数。

              << " score（分数越大，配准效果越差）： "

<< ndt.getFitnessScore()

//欧几里得适合度得分FitnessScore，该分数计算为从输出云到目标云中最近点的距离的平方。分数越大，准确率越低，配准效果也就越差

<< std::endl;

八、初始变换后，进行点云的变换并保存。

    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());

     //使用创建的变换对未过滤的输入点云（intput_cloud）进行变换

进行点云的变换主要用到的的函数是pcl::transformPointCloud，其原型pcl::transformPointCloud(const pcl::PointCloud< PointT > &  cloud_in,

pcl::PointCloud& cloud_out,

const Eigen::Matrix4f  & transform )

      cloud_in为源点云，cloud_out为变换后点云,transform为变换矩阵

    pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);

//以ascii格式写入文件，保存转换后的点云

九、可视化界面背景设定，着色两种点云。

boost::shared_ptrviewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));

// 初始化点云可视化界面

    viewer_final->setBackgroundColor(0, 0, 0);

//设置背景色为黑色。

    pcl::visualization::PointCloudColorHandlerCustom

        target_color(target_cloud, 255, 0, 0);

    viewer_final->addPointCloud(target_cloud, target_color, "target cloud");

      viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"target cloud");

//对目标点云着色（红色）并可视化。(255,0,0)与RGB有关。

    pcl::visualization::PointCloudColorHandlerCustom

        output_color(output_cloud, 0, 255, 0);

    viewer_final->addPointCloud(output_cloud, output_color, "output cloud");

    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1, "output cloud");

//对转换后的点云着色（绿色）并可视化

十、启动可视化

    viewer_final->addCoordinateSystem(1.0);

//显示xyz指示轴

    viewer_final->initCameraParameters();

    //初始化摄像头参数等

    while (!viewer_final->wasStopped())

    {

        viewer_final->spinOnce(100);

        boost::this_thread::sleep(boost::posix_time::microseconds(100000));

    }

    return (0);

}

//等待直到可视化窗口关闭。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
int
main(int argc, char** argv)
{
	pcl::PointCloud::Ptr target_cloud(new pcl::PointCloud);
	if (pcl::io::loadPCDFile("E://shuju//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;
	pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud);
	if (pcl::io::loadPCDFile("E://shuju//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;
	
	pcl::PointCloud::Ptr filtered_cloud(new pcl::PointCloud);
	pcl::ApproximateVoxelGrid 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 room_scan2.pcd" << std::endl;
	
	pcl::NormalDistributionsTransform ndt;
	ndt.setTransformationEpsilon(0.01);
	ndt.setStepSize(0.1);
	ndt.setResolution(1.0);
	ndt.setMaximumIterations(35);
	ndt.setInputCloud(filtered_cloud);
	ndt.setInputTarget(target_cloud);

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

	pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud);
	ndt.align(*output_cloud, init_guess);
	std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
		<< " score: " << ndt.getFitnessScore() << std::endl;
	pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
	pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);
	boost::shared_ptr
		viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer_final->setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom
		target_color(target_cloud, 255, 0, 0);
	viewer_final->addPointCloud(target_cloud, target_color, "target cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "target cloud");

	pcl::visualization::PointCloudColorHandlerCustom
		output_color(output_cloud, 0, 255, 0);
	viewer_final->addPointCloud(output_cloud, output_color, "output cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
		1, "output cloud");

	viewer_final->addCoordinateSystem(1.0);
	viewer_final->initCameraParameters();
。
	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return (0);
}

名词解释：

1、点云滤波作为常见的点云处理算法，一般是点云处理的第一步，对后续处理（配准、特征提取、曲面重建等）有重要作用。滤波有很多功能，比如去除噪声点、离散点、点云平滑以及数据压缩等。接下来几种需要点云滤波处理的情况：点云数据密度不规则需要平滑；离散点需要去除；大量数据需要下采样；噪声数据需要去除。

本文章中用到的是体素滤波器（下采样），体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能，但是会移动点的位置。 此外体素滤波器可以去除一定程度的噪音点及离群点，主要功能是用来进行降采样。它的原理是根据输入的点云，首先计算一个能够刚好包裹住该点云的立方体，然后根据设定的分辨率，将该大立方体分割成不同的小立方体。对于每一个小立方体内的点，计算他们的质心，并用该质心的坐标来近似该立方体内的若干点。ApproximateVoxelGrid的不同在于这种方法是利用每一个小立方体的中心来近似该立方体内的若干点。相比于 VoxelGrid，计算速度稍快，但也损失了原始点云局部形态的精细度。点云处理之点云滤波去噪_DayDayUp-CSDN博客_点云去噪和点云滤波  各个滤波器

2、配准算法的三个迭代终止条件设置：

1) setMaximumIterations（） 设置最大的迭代次数。迭代停止条件之一

2) setTransformationEpsilon（）为终止条件设置最小转换差异，设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0 。本文中NDT算法有使用。

3) setEuclideanFitnessEpsilon（） 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max ()

文中的ndt.setTransformationEpsilon(0.01);  ndt.setStepSize(0.1);  ndt.setResolution(1.0);以上参数在使用房间尺寸比例下运算比较好，但是对于处理更小物体，需要改参数。

3、刚体变换：变换前后两点间的距离依旧保持不变则被称为刚体变换（Rigid Transform）。 刚体变换可分解为平移变换、旋转变换和反转（镜像）变换。

4、More-Thuente线搜索：非约束最优化：步控制。如果不使用步控制进行根搜索，迭代在两个点之间交替进行却不收敛，下面是从无步控制到有步控制

                                   

一个良好的步控制算法可以避免出现重复的情况或从接近根或极小值附近的区域远离的情况。在一个线搜索方法中，模型函数给出一个步骤方向，然后沿着这个方向搜索以找到一个可以达到收敛的适当的点。

步长参数定义了More-Thuente线搜索允许的最大步长，这个线搜索算法确定了最大值以下的最佳步长，当你靠近最优解时该算法会缩短迭代步长，在最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。

Unconstrained Optimization: Step Control—Wolfram Language Documentation

5、在浏览文献中又看到一篇挺好的文章，关于点云配准相关的类：

无人驾驶算法学习（五）：激光里程计之帧间匹配算法_蒋成的博客-CSDN博客_激光里程计