KDtree近邻点搜索

PCL中kdtree搜索，可以实现K近邻搜索与半径r近邻搜索两种，使用案例如下：

K个近邻点搜索：

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <time.h>
using namespace std;
void main()
{
	clock_t start01, finish01;
	clock_t start, finish;
	clock_t start02, finish02;
	start01 = clock();
	//创建所要所搜点云对象指针
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//创建搜索k近邻点的点云（K个点）指针
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_k_near_point(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;//创建3维kdtree结构
	kdtree.setInputCloud(cloud);//设置搜索空间，将读入的点云带入
	pcl::PointXYZ searchpoint;//待搜索的点，即以searchpoint为搜索点
	searchpoint.x = -0.037829701;
	searchpoint.y = 0.12794;
	searchpoint.z = 0.0044746702;
	int k = 50;//设置要搜索的近邻点个数
	cloud_k_near_point->width = k;
	cloud_k_near_point->height = 1;
	cloud_k_near_point->is_dense = false;
	cloud_k_near_point->resize(cloud_k_near_point->width*cloud_k_near_point->height);
	finish01 = clock();
	cout << "加载点云时间：" << (double)(finish01 - start01)/CLOCKS_PER_SEC <<"秒"<< endl;
	//开始计时
	start = clock();


	vector<int>pointIdxNKNSearch(k);//存放搜到的最近的点的地址
	vector<float>pointNKNSquareDistance(k);//存储近邻点对应的距离信息
	kdtree.nearestKSearch(searchpoint, k, pointIdxNKNSearch, pointNKNSquareDistance);
	for (int i = 0; i < k; i++)
		{
			cloud_k_near_point->points[i].x = cloud->points[pointIdxNKNSearch[i]].x;
			cloud_k_near_point->points[i].y = cloud->points[pointIdxNKNSearch[i]].y;
			cloud_k_near_point->points[i].z = cloud->points[pointIdxNKNSearch[i]].z;
		}
	cout << "待搜索点坐标为:" << endl;
	cout << "x:" << searchpoint.x << "\t" << "y:" << searchpoint.y << "\t" << searchpoint.z << endl;
	cout << "搜索得到的50个点坐标：" << endl;
	for (int i = 0; i < k; i++)
	{
		cout << i << "\t" << cloud_k_near_point->points[i].x << "\t" <<
			cloud_k_near_point->points[i].y << "\t" << cloud_k_near_point->points[i].z << endl;
	}
	//结束时间
	finish = clock();
	double time = (double)(finish - start)/CLOCKS_PER_SEC;
	cout << "在点云个数为" << cloud->width*cloud->height << "中，使用kdtree搜索" << k << "个近邻点所需时间为:" << time<<"秒" << endl;

	start02 = clock();
	//创建可视化窗口
	pcl::visualization::PCLVisualizer viewer("K nearest neighbor points show");
	viewer.setBackgroundColor(255, 255, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);//将原始点云设置成绿色对象 single_color
	viewer.addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample");//将single_color在viewer中进行显示
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color_01(cloud_k_near_point, 255, 0, 0);//将原始点云设置成红色显示 single_color
	viewer.addPointCloud<pcl::PointXYZ>(cloud_k_near_point, single_color_01, "sample01");//将single_color在viewer中进行显示
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample01");
	finish02 = clock();
	cout << "显示点云时间：" << (double)(finish02 - start02) / CLOCKS_PER_SEC << "秒"<<endl;
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(1);
	}
}

需要注意的是：点的编号搜索得到的近邻点，是从0开始编号开始的。即在vector[index]即可，无需添-1或者+1

搜索半径r内的近邻点：

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <time.h>
using namespace std;
void main()
{
	clock_t start01, finish01;
	clock_t start, finish;
	clock_t start02, finish02;
	start01 = clock();
	//创建所要所搜点云对象指针
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//创建搜索k近邻点的点云（K个点）指针
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_radius_near_point(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;//创建3维kdtree结构
	kdtree.setInputCloud(cloud);//设置搜索空间，将读入的点云带入
	pcl::PointXYZ searchpoint;//待搜索的点，即以searchpoint为搜索点
	searchpoint = cloud->points[0];
	finish01 = clock();
	cout << "加载点云时间：" << (double)(finish01 - start01) / CLOCKS_PER_SEC << "秒" << endl;
	//开始计时 搜索时间
	start = clock();
	vector<int>pointIdxRadiusSearch;//存放搜到的最近的点的地址
	vector<float>pointNKNSquareDistance;//存储近邻点对应的距离平方
	double radius = 0.01;
	//如果搜索半径radius搜索成功，则生成搜索半径内的点结果
	if (kdtree.radiusSearch(searchpoint, radius, pointIdxRadiusSearch, pointNKNSquareDistance) > 0)
	{
		cloud_radius_near_point->width = pointIdxRadiusSearch.size();//半径radius内点的数目
		cloud_radius_near_point->height = 1;
		cloud_radius_near_point->is_dense = false;
		cloud_radius_near_point->resize(cloud_radius_near_point->width*cloud_radius_near_point->height);

		for (int i = 0; i < pointIdxRadiusSearch.size() - 1; i++)
		{
			cloud_radius_near_point->points[i].x = cloud->points[pointIdxRadiusSearch[i]].x;
			cloud_radius_near_point->points[i].y = cloud->points[pointIdxRadiusSearch[i]].y;
			cloud_radius_near_point->points[i].z = cloud->points[pointIdxRadiusSearch[i]].z;
		}
	}
	cout << "待搜索点坐标为:" << endl;
	cout << "x:" << searchpoint.x << "\t" << "y:" << searchpoint.y << "\t" << "z:" << searchpoint.z << endl;
	cout << "在半径为" << radius << "内共有" << pointIdxRadiusSearch.size() << "个点" << endl;
	for (int i = 0; i < pointIdxRadiusSearch.size(); i++)
	{
		cout << i << "\t" << cloud_radius_near_point->points[i].x << "\t" <<
			cloud_radius_near_point->points[i].y << "\t" << cloud_radius_near_point->points[i].z << endl;
	}

	//结束时间
	finish = clock();
	double time = (double)(finish - start) / CLOCKS_PER_SEC;
	cout << "在点云个数为" << cloud->width*cloud->height << "中，使用kdtree搜索" << pointIdxRadiusSearch.size() << "个近邻点所需时间为:" << time << "秒" << endl;

	start02 = clock();
	//创建可视化窗口
	pcl::visualization::PCLVisualizer viewer("K nearest neighbor points show");
	viewer.setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);//将原始点云设置成绿色对象 single_color
	viewer.addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample");//将single_color在viewer中进行显示
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color_01(cloud_radius_near_point, 255, 0, 0);//将原始点云设置成红色显示 single_color
	viewer.addPointCloud<pcl::PointXYZ>(cloud_radius_near_point, single_color_01, "sample01");//将single_color在viewer中进行显示
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample01");
	finish02 = clock();
	cout << "显示点云时间：" << (double)(finish02 - start02) / CLOCKS_PER_SEC << "秒" << endl;
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(1);
	}
}