PCL:计算两个点云的最小距离(附源码)

最新推荐文章于 2024-04-28 20:34:10 发布

VIP文章 Cynthia.Chen

最新推荐文章于 2024-04-28 20:34:10 发布

阅读量530

点赞数

分类专栏： PCL C++编程文章标签：算法

本文链接：https://blog.csdn.net/weixin_38342946/article/details/129425455

版权

1.问题

空间中存在两组点云cloudA与cloudB,计算两个点云最小距离。

2.思路

第一步：根据cloudB构建kd-tree;
第二步：遍历cloudA中每一个点，通过kd-tree搜索cloudB中与其最近的点，和最近点距离；
第三步：找出所有距离中的最小值即为两个点云的距离。

3.C++源码

/**
*@brief computeMinDistanceBetweenCloud计算两个点云间的最小距离
*@para cloud_A : 输入点云A
*@para cloud_B : 输入点云B
*@para return : 返回点云A与点云B之间的最小距离
*/
float 
EdgeDetect::computeMinDistanceBetweenCloud(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_A,
	pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_B)
{
   
	//构造kd-tree
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud_B);

	int K = 1;								    //设置邻域点个数 K
	std::vector<int>pointIdxNKNSearch(K);		//存储查询点近邻索引
	std::vector<float>pointNKNSquaredDistance(K);<