点云配准NDT (P2D)算法详解

点云配准NDT (P2D)算法详解

最近了解了一些关于点云配准算法NDT的相关文章，进行总结一下。

NDT算法的关键是其利用正态分布对参考点云进行了重新表示，使用点云在一个模型特定位置的似然值而不是直接使用点云进行匹配。

先验知识（正态分布）

一维正态分布表达式：

D维正态分布表达式（本文中D为3）：

其中，均值向量μ和方差矩阵Σ通过下式计算：

在一维的情况，正交分布随机变量X有一个期望值μ，并且对于μ的不确定性用Σ来表示。上式中的多变量概率函数在D=1的情况下降低为一维。均值和方差通过一个均值向量μ和方差矩阵Σ表示。方差矩阵对角线元素代表每个变量的方差，非对角线元素代表变量之间的协方差。下图解释了一维、二维、三维正交分布的正交变换概率密度函数。
特征向量描述了分布的主要成分，即，一组对应于变量协方差矩阵Σ的主方向的正交向量。而一个三维正交变换的不同形状，取决于协方差矩阵Σ特征值的关系。箭头展示了分布的特征向量方向，通过对应的特征值调整大小。
（即：三个特征值的大小接近为球或点，一个特征值远大于另外两个特征值为线状，两个特征值远大于一个特征值为平面）

正交变换可以被作为表面紧凑表示的一个方法。这一变换把点云投影为一个光滑曲面表示，描述为一个局部概率密度函数的集合，描述一部分曲面的形状。

如下图中所示，左侧为一个三维点云，右侧为对应的NDT表示：

整体过程

1) 建立NDT

把二维/三维点云在空间上划分为格网或立方体，将点云中的点归于对应的网格并统计每个cell中的点数，若点数 >5则有效（避免点数过小导致的共面或协方差矩阵奇异不能求逆的问题）。利用空间坐标计算均值向量μ和协方差矩阵Σ。（公式中的符号进行相应的变化）


则每一个网格中的概率密度函数为：

2) 目标函数

当使用NDT来进行配准时，其目标是找到当前扫描中点在参考扫描表面的最大似然。要进行优化的参数有：当前扫描位置估计的旋转和平移，可以被编码为向量P。当前扫描通过点云X表示。假设有一个空间变换函数 T(P,X) 能够在空间中通过p移动点x。对扫描点给定一些概率密度函数P(x),则最优的变换p应该为最大似然函数那一个。即最大化下式：

等效变换上述目标函数为 负log对数如下所示：

下图中，左侧（a）为正态分布（红色）与混合分布（正态、均匀分布，绿色）的图像，右侧（b）为两者相应的-log对数。由（b）中可知，原始的正态分布相对于X没有约束，在x的值变大时函数值也会受到很大影响，而混合模型则会对x的值进行约束，具有更好的鲁棒性。（受cell内噪声点的影响更小）。

因此，我们选择右侧（b）中的绿色曲线对应的函数作为优化的目标函数，如下所示：

式中，C1，C2为对应的常数，调整一个网格内的P(x)函数的积分值为1。p0是预期的噪声比例。如上图所示，使用此函数能够对噪声的影响进行约束。

因此，目标函数转换为如下形式：

但是，虽然由图中可以看出上式是连续的（能够进行求导），但是其并没有一个简单的一阶/二阶导数，所以可以通过转换为高斯函数的形式：

设置x分别为0，σ，∞时，通过下式表述 d1，d2，d3：

所以，一个扫描点在对应NDT score函数上的影响为：（去除了d3是因为其为一个常数，优化过程中可以忽略）。

给定一个点集X = {x1, x2, …，xn}，一个变换p，以及一个空间变换函数 T(P,X) 在空间中通过p变换X，则其对应的 score 函数为：

3) 牛顿法优化

上一节中我们建立了 score 函数，则现在问题变为找到一个最优的p，使得 score 函数值最小。文章中使用了传统的牛顿法来处理这一优化过程。即迭代的求解

这一函数。其中，H为海森矩阵，g为S§的梯度向量，每次迭代求解出的增量△p加到当前的转换向量P中，即：

梯度向量g中的元素为：

海森矩阵H中的元素为：

4) 总体流程

好了，上述基本就描述了NDT算法的实现步骤，其总体流程如下：

5) PCL实现

最后附上pcl实现代码（运行环境WIN 10 + PCL1.8.0 + VS2013），不知道为什么程序的效率有点低，大概需要30s（3000 ms）左右才能结束，而其他人都是以ms为时间单位的。大家感兴趣可以尝试跑一下。测试数据可以通过这里下载。

/*
NDT算法点云配准
xiaochen wang
2021/06
*/
#include <iostream>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <time.h>

using namespace std;

pcl::PointCloud<pcl::PointXYZ>::Ptr read_point_clouds(string const &file_path); // 读取点云数据（pcd）
void visulizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud);// 点云可视化

int main()
{
	
	// 读取点云数据（目标点集，待配准点集），显示点的个数
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = read_point_clouds("./data/cloud1.pcd");
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud = read_point_clouds("./data/cloud2.pcd");
	cout << "Load " << target_cloud->size() << " data points from target cloud1." << endl;
	cout << "Load " << input_cloud->size() << " data points from target cloud2." << endl;
	visulizer(target_cloud, input_cloud); //初始读入数据可视化,关闭后执行下一步

	// 对cloud2进行下采样，降低点云数量，提高运行效率
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_point(new pcl::PointCloud<pcl::PointXYZ>); //建立一个filtered_point，存储滤波（下采样）后的点云数据
	pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter; // 建立一个approximate_voxel_filter
	approximate_voxel_filter.setLeafSize(0.4, 0.4, 0.4); //设置体素大小
	approximate_voxel_filter.setInputCloud(input_cloud);
	approximate_voxel_filter.filter(*filtered_point); //滤波（降采样）后的点云数据
	cout << "Filtered cloud contains " << filtered_point->size() << " data points from input cloud." << endl;

	// 初始化NDT变换参数
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; // 建立一个ndt
	ndt.setTransformationEpsilon(0.01); //ε（两个连续变换之间允许的最大差值），判断优化过程是否已经收敛。对[x, y, z, roll, pitch, yaw]以m和rad为单位的增量允许最小值,低于这个值，则停止
	ndt.setStepSize(0.1); //0.1牛顿法优化的最大步长，more-thuente线搜索设置最大步长
	ndt.setResolution(1.0); //格网化时的大小
	ndt.setMaximumIterations(35); //优化迭代的次数，当达到35次迭代，或者收敛到阈值ε时，停止迭代（防止在错误的方向运算太多时间），超出迭代次数则停止

	ndt.setInputSource(filtered_point); //待配准点集
	ndt.setInputTarget(target_cloud); //目标参考点集

	// 初始化变换关系p
	Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ()); // 欧拉角，绕Z轴旋转
	Eigen::Translation3f init_translation(1.79387, 0.720047, 0); //初始化平移参数
	Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix(); //初始化变换关系p（* 运算符在这里是两个变换的串联）
	
	cout << "The initial transformation matrix:" << endl;
	cout << init_guess << endl << endl;

	
	cout << "NDT processing..." << endl;
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>); //存储变换后的点云数据
	DWORD start_time = GetTickCount();
	ndt.align(*output_cloud, init_guess); // init_guess为变换初值
	DWORD end_time = GetTickCount();
	cout << "Normal distribution transformation has converged: " << ndt.hasConverged() << " score: " << ndt.getFitnessScore() << endl;

	Eigen::Matrix4f optimized_trans = ndt.getFinalTransformation();
	cout << "The optimized transformation matrix:" << endl;
	cout << optimized_trans << endl << endl;

	// 对滤波前的数据进行变换，保存配准后的点云数据，输出为pcd格式
	pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation()); //使用计算的变换参数，对采样前的输入数据进行变换，输出到output_cloud
	pcl::io::savePCDFileASCII("./data/output_point_clouds.pcd", *output_cloud); // 输出变换后的点云数据

	cout << "Point cloud registration completed! "  << endl;
	
	
	cout << "The run time is: " << (end_time - start_time) << " ms!" << endl;
	
	//点云结果可视化
	visulizer(target_cloud, output_cloud);

	getchar();
	return 0;
}

// 读取点云数据（pcd）
pcl::PointCloud<pcl::PointXYZ>::Ptr read_point_clouds(string const &file_path)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud(new pcl::PointCloud<pcl::PointXYZ>);

	if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *my_cloud) == -1) //判断是否能够成功读入pcd数据
	{
		cout << "Couldn't read the pcd file: " << file_path << endl;
		return nullptr;
	}

	return my_cloud;
}

// 点云可视化
void visulizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud)
{
	// 初始化3D Viewer
	boost::shared_ptr<pcl::visualization::PCLVisualizer> 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_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");

	//开始可视化
	viewer_final->addCoordinateSystem(1.0, "global");
	viewer_final->initCameraParameters();

	//此while循环保持窗口一直处于打开状态，并且按照规定时间刷新窗口，直到关闭可视化窗口。
	//wasStopped()判断显示窗口是否已经被关闭，spinOnce()叫消息回调函数，作用其实是设置更新屏幕的时间
	//this_thread::sleep()在线程中调用sleep()
	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}