PCL——快速邻域搜索

以下代码参考《点云库PCL从入门到精通》

1. 代码示例

• 用 k-d tree找到具体点在空间中的k近邻
• 找到指定的某一半径内的所有近邻
  

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>

using namespace std;
using namespace pcl;

int main(int argc, char**argv)
{
	// 用系统时间初始化随机种子
	srand(time(NULL));	
	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
	// 点云初始化
	cloud->width = 1000;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
	// 点云填充数据
	for (size_t i = 0; i< cloud->points.size(); ++i)
	{
		cloud->points[i].x = 1024.0f* rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024.0f* rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024.0f* rand() / (RAND_MAX + 1.0f);
	}

	// 创建k-d tree对象
	KdTreeFLANN<PointXYZ> kdtree;
	// 设置搜索的点云
	kdtree.setInputCloud(cloud);
	// 定义查询点并赋值
	PointXYZ searchPoint;
	searchPoint.x = 1024.0f* rand() / (RAND_MAX + 1.0f);
	searchPoint.y = 1024.0f* rand() / (RAND_MAX + 1.0f);
	searchPoint.z = 1024.0f* rand() / (RAND_MAX + 1.0f);

	// k近邻搜索 找最近的K个点，最终的点数有可能小于K
	int K = 10;
	vector<int> pointIdxNKNSearch(K);			//存储搜索到查询点近邻的索引
	vector<float> pointNKNSquaredDistance(K);	//存储对应近邻的平方距离

	cout << "K nearest neighbor search at (" 
		<< searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with K=" << K << endl;

	cout << endl;

	//n 是返回的点的数量
	//所有的返回点已经按照近邻平方距离排列好了，然后对应索引从小到大排列
	int n = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
	if (n > 0)
	{
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
		{
			cout << " " << cloud->points[pointIdxNKNSearch[i]].x
				 << " " << cloud->points[pointIdxNKNSearch[i]].y
				 << " " << cloud->points[pointIdxNKNSearch[i]].z
				 << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << endl;
		}
		cout << endl;
	}


	// 在半径r内搜索近邻 找半径内的所有点，点的数量无法确定，有可能一个都没有，也有可能有很多
	vector<int> pointIdxRadiusSearch;
	vector<float> pointRadiusSquaredDistance;
	float radius = 100;

	cout << "Neighbors within radius search at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with radius=" << radius << endl;
	cout << endl;

	//n 是返回的点的数量
	//所有的返回点已经按照近邻平方距离排列好了，然后对应索引从小到大排列
	n = kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);

	if (n >0)
	{
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
		{
			cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x
				<< " " << cloud->points[pointIdxRadiusSearch[i]].y
				<< " " << cloud->points[pointIdxRadiusSearch[i]].z
				<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << endl;
		}
	}

	system("Pause");
	return 0;
}