PCL学习（4）——octree

octree

基于octree的空间划分及搜索操作

  

代码如下：

#include<pcl\point_cloud.h>
#include<pcl\octree\octree.h>
#include<iostream>
#include<vector>
#include<ctime>
#include<pcl\io\pcd_io.h>
using namespace std;
int main()
{
	//pcl::PointCloud<pcl::PointXYZ> cloud;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//用Ptr指针创建点云对象
	if (pcl::io::loadPCDFile("E:\\test_PCL\\ConsoleApplication1\\ConsoleApplication1\\pipline.pcd",*cloud) == -1)//读取pcd点云图像
	{
		cout << "read failed" << endl;
	}
	float resolution = 128.0f;//定义分辨率，即octree最小体素的尺寸
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//创建octree对象
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	pcl::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);
	// 体素内近邻搜索
	vector<int>pointIdxVec;   //存放近邻点的索引
	if (octree.voxelSearch(searchpoint, pointIdxVec))//如果存在近邻点
	{
		for (int i = 0; i < pointIdxVec.size();++i)
		{ 
			cout << " " << cloud->points[pointIdxVec[i]].x
				<< " " << cloud->points[pointIdxVec[i]].y
				<< " " << cloud->points[pointIdxVec[i]].z <<endl;
		}

	}
	//K近邻搜索
	int K = 10;//定义近邻点数
	vector<int>pointIdxNKNSearch;//近邻点索引
	vector<float>pointNKNSquaredDistance;//对应近邻点与搜索点的距离的平方
	if(octree.nearestKSearch(searchpoint, 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;
	}
	//半径内近邻搜索
	vector<int>pointIdxRadiusSearch;
	vector<float>pointRadiusSquaredDistance;//对应近邻点与搜索点的距离的平方
	float radius = 256.0f* rand() / (RAND_MAX + 1.0f);//定义搜索半径
	std::cout << "Neighbors within radius search at (" << searchpoint.x
		<< " " << searchpoint.y
		<< " " << searchpoint.z
		<< ") with radius=" << radius << std::endl;
	if(octree.radiusSearch(searchpoint,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;
	}
}

上面使用的是：pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>)定义点云对象;也可以使用pcl::PointCloud<pcl::PointXYZ>  cloud进行定义，略有不同，除了cloud->points.x改为cloud.points.x，还有其他改动。代码如下：

	pcl::PointCloud<pcl::PointXYZ> cloud;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointer(new pcl::PointCloud<pcl::PointXYZ>);
	//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//用Ptr指针创建点云对象
//loadPCDFile（"",cloud）和loadPCDFile（"",*cloud），注意这个差别
	if (pcl::io::loadPCDFile("E:\\test_PCL\\ConsoleApplication1\\ConsoleApplication1\\pipline.pcd",cloud) == -1)//读取pcd点云图像
	{
		cout << "read failed" << endl;
	}
	float resolution = 128.0f;//定义分辨率，即octree最小体素的尺寸
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//创建octree对象



	cloudPointer = cloud.makeShared();//需要对类型进行转换



	octree.setInputCloud(cloudPointer);//只接受Ptr类型的点云对象
	octree.addPointsFromInputCloud();
	pcl::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);