PCL：半径R近邻搜索的实现

Posted 2021-09-13 没事就要敲代码

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了PCL：半径R近邻搜索的实现相关的知识，希望对你有一定的参考价值。

文章目录

1 半径R近邻

待补充…

2 实现代码

#include <iostream>
#include <pcl\\io\\pcd_io.h>
#include <pcl\\kdtree\\kdtree_flann.h>

using namespace std;

int main()
{
	//-------------------------------- 从硬盘中读取点云 -----------------------------
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); //存放读取点云的对象
	pcl::PCDReader reader;	//定义点云读取对象
	if (reader.read("test.pcd", *cloud) < 0)
	{
		PCL_ERROR("\\a->点云文件不存在！\\n");
		system("pause");
		return -1;
	}
	cout << "->加载了 " << cloud->points.size() << " 个数据点" << endl;
	//==============================================================================


	//---------------------------------- 半径R近邻搜索 ---------------------------------
	pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;		//创建kdtree对象
	kdtree.setInputCloud(cloud);				//设置搜索空间

	pcl::PointXYZ searchPoint;					//定义查询点
	searchPoint = cloud->points[0];
	cout << "->查询点坐标为：" << searchPoint << endl;

	float R = 0.1;								//设置搜索半径大小
	vector<int> pointIdxRadiusSearch;			//存储近邻索引
	vector<float> pointRadiusSquaredDistance;	//存储近邻对应的平方距离

	cout << "\\n->正在进行半径R邻域近邻搜索..." << endl << endl;
	if (kdtree.radiusSearch(searchPoint, R, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	{
		//打印所有近邻点坐标，方式2
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
		{
			cout << "第" << i + 1 << "个近邻点："
				<< cloud->points[pointIdxRadiusSearch[i]]
				<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;
		}
	}
	else
	{
		PCL_ERROR("未搜索到邻近点！\\a\\n");
	}
	//==============================================================================

	return 0;
}

3 输出结果

->加载了 1348 个数据点
->查询点坐标为：(2.04987,2.51423,0.147404)

->正在进行半径R邻域近邻搜索...

第1个近邻点：(2.04987,2.51423,0.147404) (squared distance: 0)
第2个近邻点：(2.05609,2.51304,0.146812) (squared distance: 4.04699e-05)
第3个近邻点：(2.02584,2.51832,0.182883) (squared distance: 0.00185291)
第4个近邻点：(2.02652,2.51803,0.193176) (squared distance: 0.00265429)
第5个近邻点：(2.10567,2.5034,0.150128) (squared distance: 0.00323892)
第6个近邻点：(2.1113,2.50233,0.148728) (squared distance: 0.00391698)
第7个近邻点：(2.03088,2.51653,0.235119) (squared distance: 0.0080595)
第8个近邻点：(2.03057,2.51653,0.239043) (squared distance: 0.00877514)

同样的，当查询点为搜索空间中一点式，最近邻点就是查询点本身。