PCL学习：octree进行快速查找

#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
int main()
{
	srand(time(NULL));
	pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	my_cloud->width = 1000;
	my_cloud->height = 1;
	my_cloud->points.resize(my_cloud->width * my_cloud->height);
	for(size_t i=0;i<my_cloud->points.size();++i)
	{
		my_cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
		my_cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
		my_cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
	}
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> my_octree(resolution);
	//这两句将octree与pointcloud联系了起来
	my_octree.setInputCloud(my_cloud);
	my_octree.addPointsFromInputCloud();
	pcl::PointXYZ searchPoint; //设置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);
	std::vector<int> pointId;
	if(my_octree.voxelSearch(searchPoint,pointId))
	{
		std::cout<<"查找的点的坐标为: "<<std::endl<<searchPoint.x<<" "<<searchPoint.y <<" "<<searchPoint.z << std::endl;
		std::cout<<"依据体素搜索的结果为： "<<std::endl;
		for(size_t i=0;i<pointId.size();++i)
		{
			std::cout<<my_cloud->points[ pointId[i]].x
				<<" "<<my_cloud->points[ pointId[i]].y
				<<" "<<my_cloud->points[ pointId[i]].z
				<<std::endl;
		}
	}
	int K=10;
	std::vector<int> pointId2;
	std::vector<float> pointKNNSquareDistance;
	//下面进行octree的k近邻搜索
	std::cout<<"octree的k近邻搜索结果: "<<std::endl;
	if(my_octree.nearestKSearch(searchPoint,K,pointId2,pointKNNSquareDistance))
		for(size_t i=0;i<pointId2.size();++i)
		{
			std::cout<<my_cloud->points[ pointId2[i]].x
				<<" "<<my_cloud->points[ pointId2[i]].y
				<<" "<<my_cloud->points[ pointId2[i]].z
				<<" "<<"the Squaredistance is "<<pointKNNSquareDistance[i]
				<<std::endl;
		}
	pcl::PointXYZ min;
	pcl::PointXYZ max;
	pcl::getMinMax3D(*my_cloud,min,max);
	double r = (max.x-min.x)/4;
	std::vector<int> pointId3;
	std::vector<float> pointRadiusSquareDistance;
	if(my_octree.radiusSearch(searchPoint,r,pointId3,pointRadiusSquareDistance))
		for(size_t i=0;i<pointId3.size();++i)
		{
			std::cout<<my_cloud->points[ pointId3[i]].x
				<<" "<<my_cloud->points[ pointId3[i]].y
				<<" "<<my_cloud->points[ pointId3[i]].z
				<<" "<<"the Squaredistance is "<<pointRadiusSquareDistance[i]
			<<std::endl;
		}
	system("pause");
	return 0 ;
}

关键的语句在于

	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> my_octree(resolution);
	//这两句将octree与pointcloud联系了起来
	my_octree.setInputCloud(my_cloud);
	my_octree.addPointsFromInputCloud();

resolution设置的是体素的分辨率
后面两句是将octree与Pointcloud联系了起来
至于后面的搜索操作，由于八叉树本身的特点，多了一个依据体素进行搜索的函数，其余的感觉和前面的kd-tree操作一致