PCL NDT精配准（Registration_NDT）

原创已于 2024-09-20 09:29:17 修改 · 647 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #人工智能 #机器学习 #数据结构 #3d

于 2024-09-07 16:22:32 首次发布

PCL 专栏收录该内容

96 篇文章

订阅专栏

PCL专栏目录及须知

NDT同ICP一样，也是用于粗配准后的精配准。

1.NDT原理

将目标点云按照固定的分辨率划分成3d网格，并计算出每个3d网格点云的分布情况。然后计算源点云在目标点云3d网格分布的概率，以确定两点云之间的匹配关系。

（1）将目标点云按照输入的分辨率划分到3d网格中，并计算每个网格的均值与协方差。

（2）计算源点云中的每个点落在目标点云的3d网格中的哪个网格里。

（3）通过公式计算NDT代价值，确定是否收敛。

代价值公式：

（4）迭代计算最优解。

2.关键函数

（1）为More-Thuente线搜索设置最大步长

registration.setStepSize(0.1);														// 为More-Thuente线搜索设置最大步长

（2）设置NDT网格结构的分辨率

registration.setResolution(1.0f);													// 设置NDT网格结构的分辨率

（3）设置匹配迭代的最大次数

registration.setMaximumIterations(50);												// 设置匹配迭代的最大次数

（4）为终止条件设置最小转换差异

registration.setTransformationEpsilon(0.01);										// 为终止条件设置最小转换差异

3.完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>


int main()
{
	/****************NDT配准********************/
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);			// 源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云
	pcl::io::loadPCDFile("D:/code/csdn/data/lidar0.pcd", *source);
	pcl::io::loadPCDFile("D:/code/csdn/data/lidar1.pcd", *target);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云转换后的结果

	// NDT配准
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> registration;
	registration.setStepSize(0.1);														// 为More-Thuente线搜索设置最大步长
	registration.setResolution(1.0f);													// 设置NDT网格结构的分辨率
	registration.setMaximumIterations(50);												// 设置匹配迭代的最大次数
	registration.setInputSource(source);												// 设置要配准的点云
	registration.setInputTarget(target);												// 设置点云配准目标
	registration.setTransformationEpsilon(0.01);										// 为终止条件设置最小转换差异
	registration.align(*result);

	std::cout << "点对匹配分数：" << registration.getFitnessScore() << std::endl;
	std::cout << "刚体变换矩阵：" << registration.getFinalTransformation() << std::endl;

	/****************展示********************/
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("NDT"));
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0);		// 目标点云
	viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>result_color(result, 0, 255, 0);		// 配准结果点云
	viewer->addPointCloud<pcl::PointXYZ>(result, result_color, "result cloud");

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return (0);
}