pcl_kdtree

int main(int argc, char *argv[])
{
	QCoreApplication a(argc, argv);

	using PointT = pcl::PointXYZ;
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);

	//点云生成
	cloud->width = 1000;
	cloud->height = 10;
	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);
	}

	//using showView = pcl::visualization::PCLVisualizer;
	//showView viewer("PCL");
	//viewer.setBackgroundColor(0, 0, 0);
	//viewer.addPointCloud(cloud);
	//while (!viewer.wasStopped())
	//	viewer.spinOnce();


	pcl::KdTreeFLANN<PointT> kdtree;
	kdtree.setInputCloud(cloud);

	PointT searchPt; //定义查询点并赋值随机值
	searchPt.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
	searchPt.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
	searchPt.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

	int K = 10;
	std::vector<int>pointIdxNKNSearch(K);
	std::vector<float>pointNKNSquaredDistance(K);
	/*
	int nearsestKSearch(const PointT &P_q, int k, std::vecotr<int> &k_indices, std::vector<flost> &ksqr_distances)
	1. p_q: 需要搜寻的点
	2. k： K邻域的个数
	3. k_indices: 搜索完的邻域点对应的索引
	4. ksqr_distance: 搜索完每个邻域点与查询点之间的欧式距离
	作用: 进行K邻域搜索
	*/
	if (kdtree.nearestKSearch(searchPt, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
			std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x
			<< " " << cloud->points[pointIdxNKNSearch[i]].y
			<< " " << cloud->points[pointIdxNKNSearch[i]].z
			<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
	}


	// 在半径r内搜索近邻
	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

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

	/*
	int radiusSearch(const PointT &p_q, double radius, std::vecotr<int> &k_indices, std::vector<flost> &ksqr_distances)
	1. p_q: 需要搜寻的点
	2. radius： 需要查询的半径大小
	3. k_indices: 搜索完的邻域点对应的索引
	4. ksqr_distance: 搜索完每个邻域点与查询点之间的欧式距离
	作用： 进行R半径内的邻域搜索
	*/
	if (kdtree.radiusSearch(searchPt, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	{
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
			std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x
			<< " " << cloud->points[pointIdxRadiusSearch[i]].y
			<< " " << cloud->points[pointIdxRadiusSearch[i]].z
			<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
	}
	return a.exec();
}