PCL KD树的使用

1.kdtree原理介绍：

参考：详解KDTree

2.K近邻搜索

#include <iostream>
#include <vector>
#include <pcl/kdtree/kdtree_flann.h>  //kdtree近邻搜索
#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/point_types.h>  //点类型相关定义
#include <pcl/visualization/pcl_visualizer.h>//可视化相关定义

using namespace std;

int main()
{
	//读取点云数据
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("Road - Cloud.pcd", *cloud) == -1)
	{
		PCL_ERROR("Cloudn't read file!");
		return -1;
	}

	//建立kd-tree
	pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;  //建立kdtree对象
	kdtree.setInputCloud(cloud); //设置需要建立kdtree的点云指针

	//K近邻搜索
	pcl::PointXYZRGB searchPoint = cloud->points[1000]; //设置查找点
	cloud->points[1000].r = 0;//查询点着色（绿色）
	cloud->points[1000].g = 255;
	cloud->points[1000].b = 0;
	int K = 100;  //设置需要查找的近邻点个数
	vector<int> pointIdxNKNSearch(K);  //保存每个近邻点的索引
	vector<float> pointNKNSquaredDistance(K); //保存每个近邻点与查找点之间的欧式距离平方

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

	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		for (size_t i = 1; i < pointIdxNKNSearch.size(); ++i){ //i=1时不包含被查询点本身，i=0时包含被查询点
			cloud->points[pointIdxNKNSearch[i]].r = 255;//查询点邻域内的点着色
			cloud->points[pointIdxNKNSearch[i]].g = 0;
			cloud->points[pointIdxNKNSearch[i]].b = 0;
		}
	}
	cout << "K = 100近邻点个数：" << pointIdxNKNSearch.size() << endl;
    //可视化
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); // 设置点云大小
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	}

	return 0;
}

3.KD树半径搜索

#include <iostream>
#include <vector>
#include <pcl/kdtree/kdtree_flann.h>  //kdtree近邻搜索
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h>   
#include <pcl/visualization/pcl_visualizer.h>//可视化相关定义

using namespace std;

int main()
{
	//读取点云
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("Road - Cloud.pcd", *cloud) == -1)
	{
		PCL_ERROR("Cloudn't read file!");
		return -1;
	}

	//建立kd-tree
	pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;  //建立kdtree对象
	kdtree.setInputCloud(cloud); //设置需要建立kdtree的点云指针
    //radius半径搜索
	pcl::PointXYZRGB searchPoint = cloud->points[1000];  //设置查找点
	cloud->points[1000].r = 0;
	cloud->points[1000].g = 255;
	cloud->points[1000].b = 0;
	vector<int> pointIdxRadiusSearch;  //保存每个近邻点的索引
	vector<float> pointRadiusSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
	float radius = 3;  //设置查找半径范围
	cout << "Neighbors within radius search at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with radius=" << radius << endl;

	if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	{
		for (size_t i = 1; i < pointIdxRadiusSearch.size(); ++i){
			cloud->points[pointIdxRadiusSearch[i]].r = 255;
			cloud->points[pointIdxRadiusSearch[i]].g = 0;
			cloud->points[pointIdxRadiusSearch[i]].b = 0;
		}
	}
	cout << "半径3近邻点个数： " << pointIdxRadiusSearch.size() << endl;
	//可视化
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
	viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); // 设置点云大小
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	}

	return 0;
}

4.结果展示

绿色为搜索点，红色为搜索点邻域内的点

5.其他

PCL 中 KdTree 的使用心得
建立点云的kdtree索引
官网链接：pcl::search::KdTree