PCL：K近邻搜索的实现

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

tags:

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

文章目录

1 K近邻

待补充…

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;
	//==============================================================================


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

	pcl::PointXYZ searchPoint;				//定义查询点
	searchPoint.x = 0;
	searchPoint.y = 0;
	searchPoint.z = 0;

	int K = 9;								//设置邻域点个数 K
	vector<int>pointIdxNKNSearch(K);		//存储查询点近邻索引
	vector<float>pointNKNSquaredDistance(K);//存储近邻点对应平方距离

	cout << "\\n->正在进行K近邻搜索..." << endl << endl;
	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		//打印所有 K 近邻点坐标
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
		{
			cout << "第" << i + 1 << "个近邻点："
				<< cloud->points[pointIdxNKNSearch[i]].x << " "
				<< cloud->points[pointIdxNKNSearch[i]].y << " "
				<< cloud->points[pointIdxNKNSearch[i]].z
				<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << endl;
		}
	}
	else
	{
		PCL_ERROR("未搜索到邻近点！\\a\\n");
	}
	//==============================================================================

	return 0;
}

3 输出结果

按平方距离从小到大的顺序输出

->加载了 1348 个数据点

->正在进行K近邻搜索...

第1个近邻点：0.558115 2.79775 0.458206 (squared distance: 8.34885)
第2个近邻点：0.551515 2.79901 0.459101 (squared distance: 8.34941)
第3个近邻点：0.49194 2.81055 0.457907 (squared distance: 8.35085)
第4个近邻点：0.486503 2.81156 0.460216 (squared distance: 8.35336)
第5个近邻点：0.620649 2.78568 0.456991 (squared distance: 8.35407)
第6个近邻点：0.617656 2.78622 0.459704 (squared distance: 8.35584)
第7个近邻点：0.43334 2.82181 0.461748 (squared distance: 8.36363)
第8个近邻点：0.424862 2.82345 0.46224 (squared distance: 8.36602)
第9个近邻点：0.670547 2.776 0.459312 (squared distance: 8.36678)

4 讨论

上面的代码中，查询点定义为（0，0，0），并非搜索空间中的点；若查询点为搜索空间中一点，则最近邻点为查询点本身。

下面给出代码验证此结论。

代码：

#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;
	//==============================================================================


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

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

	int K = 9;								//设置邻域点个数 K
	vector<int>pointIdxNKNSearch(K);		//存储查询点近邻索引
	vector<float>pointNKNSquaredDistance(K);//存储近邻点对应平方距离

	cout << "\\n->正在进行K近邻搜索..." << endl << endl;
	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		//打印所有 K 近邻点坐标
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
		{
			cout << "第" << i + 1 << "个近邻点："
				<< cloud->points[pointIdxNKNSearch[i]].x << " "
				<< cloud->points[pointIdxNKNSearch[i]].y << " "
				<< cloud->points[pointIdxNKNSearch[i]].z
				<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << endl;
		}
	}
	else
	{
		PCL_ERROR("未搜索到邻近点！\\a\\n");
	}
	//==============================================================================

	return 0;
}

输出结果：

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

->正在进行K近邻搜索...

第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)
第9个近邻点：2.17018 2.49092 0.15083 (squared distance: 0.0150309)

可以发现，当查询点为搜索空间中一点式，最近邻点就是查询点本身。