点云配准 6 -pcl如何使用正态分布变换进行配准

Posted 2021-12-18 行码阁119

本节我们将介绍如何使用正态分布变换算法来确定两个大型点云（都超过100,000个点）之间的刚体变换。正态分布变换算法是一个配准算法，它应用于三维点的统计模型,使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。想了解更多信息，请看这篇论文：The Three-Dimensional Normal-Distributions Transform - an Efficient Representation for Registration,Surface Analysis, and Loop Detection

下载地址：share_noel/papers/The Three-Dimensional Normal-Distributions Transform — an Efficient Representation for Registration, Surface Analysis, and Loop Detection.pdf

优秀的博主，对这边论文也进行了讲解：

点云配准论文阅读笔记--3d-dnt博士论文_诺有缸的高飞鸟的博客-CSDN博客

一些自己的理解（对于二维）：参考：学习教程：点云匹配-正态分布变换NDT（Normal Distributions Transform）算法_哔哩哔哩_bilibili

 上面的点云是稀疏点云，如果单看点云的分布，分布的很开，没有规则，NDT算法就是利用上面的框框将上面的框内的稀疏点云合成一个点。

 比如说A方格内有很多点，那么怎么通过一个连续函数来表达点的分布了？概率密度函数，意思就是说方框内出现点云的概率是多少，如果说我在这个位置出现点云的概率大，就将其数值调大，所以每一个方框里面都有一个概率分布。方框中，蓝色对应的是我们采集到的点云，那么红色和黄色对应的点云的协方差矩阵，点云的平均值得到的。协方差矩阵有一个特性，可以描绘点云分布的特性，如果点云的点离矩阵内的平均值越远，其特征值越大，点云分布越散，就如B一样。根据点云密度函数，可以初略的知道点云分布的朝向和平整度。

NDT在变换中，还将信息进行了压缩。比如说普通的点云数据，一个点云，三个坐标（x,y,z）,其中x,y,z对应的浮点型数据，一个数据占8bite,三个就是24bite,几万个点，数据量大的惊人。

一维的正态分布就是一条曲线，二维的正态分布是一个平面，三维的正态分布，就有三个特征向量，特征向量大的，就可以将点云形状拉长。

 如果说点云有三个形状的话，就将其分为三个类。点云球状分布，线状分布，平面分布。比如说，激光点云打到一个平面上，垂直平面的特征向量就会比较小，反射回来，点云分布就会呈现平面状。当激光雷达扫到一个边缘，在两个维度上反射比较少，在分布上回呈现线性排列。球类反射回来就是扫到了一些没有规则的物体上。然后在进行细分。比如将球状分成好几类。尖锐于不尖锐进行细分。

 比如对于球状进行分类，当三个特征向量差别不大的时候，看起来像圆，差别大，看起来不像圆。于是上诉就设置了球状比列roundness,就是将两个大的特征向量值进行相除（te是一个经验值），如果i大一点的话，就畸形一点，然后按照其分成一个小类。

主要讨论的还是平面分布，即最主要讨论的是平面比（普遍存在）。左图三空间区域朝向分成了个9个小的角度，即灰色的，白色的，这些小的区域就是描绘面的法向量（小的特征值指的方向，就是面的法向量），这些朝向怎么来分了？就分成这9个小的角度。这个小角度朝北朝南，在一个数据集是可以统计出来的，但是如果在另外一个数据，我把右边这个点云转了一定的角度，再说朝北朝南就有问题

 在这场景里面我分成了很多小小阁小的体素，比如说朝一个方向数量比较多（朝北），要把这个方向，我把它拧到朝北去，如果统计出来朝向第二多的，我把它放在朝南这边去，对每一个数据都进行这样一个旋转和拧的操作，就能把旋转不变性给做出来。

 线性分布

这三个描述值说的就是线性分布，还有平面分布和球状分布，作用最大的还是平面的法向量，这个参数比较靠谱一点，其他的法向量于结果的影响不是特别大 。比如说，我们把两个直方图做出来之后，比如说两个scan,一个F一个G，两个直方图做出来之后，我要判断F和G差别大不大，差别不大就说明这是到过的场景。之间的距离就是通过上面公式计算的。第一列减去第一列。

这里一个方格一个方格的分的话，方格之间会有抖动，或者说是激变，或者说是界越，这样将导致一个scan，一个scan匹配的时候，方格之间会有一些误差。有一部分学者，对其进行了优化，一个方格，一个方格之间平移半个方格，就有重叠区域，概率密度函数将会有平滑的效果。八叉树的优化方法，压缩之后，数据量还是很大，点云密集的地方用小的方格进行规划，点云稀疏的地方，用大的方格进行优化

 

直接上代码：

/*
 * @Description:  使用正态分布变换进行配准
 * https://www.cnblogs.com/li-yao7758258/p/6554582.html
 * http://robot.czxy.com/docs/pcl/chapter03/registration/#ndt
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: [e-mail]
 * @Date: 2020-10-22 19:20:43
 * @LastEditTime: 2020-10-22 19:26:04
 * @FilePath: /pcl-learning/14registration配准/4正态分布变换配准(NDT)/normal_distributions_transform.cpp
 */
 /*
 使用正态分布变换进行配准的实验 。其中room_scan1.pcd  room_scan2.pcd这些点云包含同一房间360不同视角的扫描数据
 */
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

#include <pcl/registration/ndt.h>      //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  （使用体素网格过滤器处理的效果比较好）

#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int
main(int argc, char** argv)

	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("E:/LX/PLC/pcl-learning-master/14registration配准/4正态分布变换配准(NDT)/room_scan1.pcd", *target_cloud) == -1)
	
		PCL_ERROR("could not load file for target_cloud");
		return(-1);
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if(pcl::io::loadPCDFile<pcl::PointXYZ> ("E:/LX/PLC/pcl-learning-master/14registration配准/4正态分布变换配准(NDT)/room_scan2.pcd",*input_cloud)==-1)
	
		PCL_ERROR("could not laod file for source_cloud");
		return(-1);
	
	std::cout << "loaded: " << input_cloud->size() << " data points from room_scan2.pcd" << std::endl;
	std::cout << "loaded: " << target_cloud->size() << " data points from room_scan1.pcd" << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximater_voxel_filter;
	approximater_voxel_filter.setLeafSize(0.1,0.1,0.1);
	approximater_voxel_filter.setInputCloud(target_cloud);
	approximater_voxel_filter.filter(*filtered_cloud);
	std::cout << "filter::" << filtered_cloud->size() << "data points from target_cloud" << std::endl;
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
	ndt.setTransformationEpsilon(0.1);
	ndt.setStepSize(0.1);
	ndt.setResolution(1.0);
	ndt.setMaximumIterations(70);
	ndt.setInputTarget(filtered_cloud);
	ndt.setInputSource(input_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<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	ndt.align(*output_cloud, init_guess);
	//这个地方的output_cloud不能作为最终的源点云变换，因为上面对点云进行了滤波处理
	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<pcl::visualization::PCLVisualizer>
		viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer_final->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色

	int v1(0), v2(0);
	viewer_final->createViewPort(0, 0, 0.5, 1, v1);//第一个窗口位置、颜色
	viewer_final->setBackgroundColor(0, 0, 0, v1);
	viewer_final->createViewPort(0.5, 0, 1, 1, v2);//第二个窗口位置以及背景颜色
	viewer_final->setBackgroundColor(0, 0, 0, v2);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		input_color(input_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input color", v1);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		target_color_1(target_cloud, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color_1, "target_color_1", v1);

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

	// 对转换后的源点云着色 (green)可视化.
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
		output_color(output_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud",v2);
	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);

运行结果（左边是没有配准的图，右边是配准过后图）：

 仔细看一下，其最后还差点，没有配准

 在代码中，加入icp进行对比，通过对比可知，ndt速度的确块，icp设置的迭代次数为300，迭代后的结果如下（左：icp,右：ndt）：