pcl系列-octree的使用

记录一些pcl库官方文档的学习，之前的学习会陆续补上。

1.octree简介

octree是一中用于描述三位空间的树状数据结构。octree的每个节点表示一个正方体的体积元素，每个节点有八个子节点，将八个子节点所表示的体积元素加在一起就等于父节点的体积。octree是四叉树在三维空间上的扩展，二维上我们有四个象限，而三维上，我们有8个卦限。octree主要用于空间划分和最近邻搜索。

2.octree实现原理

(1). 设定最大递归深度
(2). 找出场景的最大尺寸，并以此尺寸建立第一个立方体
(3). 依序将单位元元素丢入能被包含且没有子节点的立方体
(4). 若没有达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体
(5). 若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。
(6). 重复3，直到达到最大递归深度。

3.pcl库octree的使用

#include <pcl/point_cloud.h>  //点云头文件
#include <pcl/octree/octree_search.h>  //八叉树头文件
 
#include <iostream>
#include <vector>
#include <ctime>
 
int
main(int argc, char **argv)
{
	//随机数种子，与srand(time(NULL))无区别：
	srand((unsigned int)time(NULL));  
 
	//创建点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); 
 
	//点云参数
	cloud->width = 1000;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
 
	//点随机赋值
	for(std::size_t i = 0; i < cloud->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);
	}
 
	/**********************************************************
	创建octree实例，用分辨率初始化，octree在其叶节点内保留了一个点索引向量
	分辨率参数描述最低一级octree的最小体素的长度，因此octree的深度是点云的
	分辨率和空间维度的函数。
	**********************************************************/
	float resolution = 128.0f;
 
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
 
	//设置输入点云
	octree.setInputCloud(cloud);
	//建立octree
	octree.addPointsFromInputCloud();
 
	//设置searchPoint
	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);
 
	/**********************************************************
	一旦PointCloud与octree相关联，我们就可以执行搜索操作，这里采用的方法是体素搜索法，
	将搜索点分配给相应的叶节点，并返回点索引向量。搜索电和搜索结果之间的距离取决于分辨
	率参数
	**********************************************************/
	std::vector<int> pointIdxVec;
 
	if (octree.voxelSearch(searchPoint, pointIdxVec))
	{
		std::cout << "Neighbors within voxel search at (" << searchPoint.x
			<< " " << searchPoint.y
			<< " " << searchPoint.z << ")"
			<< std::endl;
	}
 
	for (std::size_t i = 0; i < pointIdxVec.size(); ++i)
	{
		std::cout << " " << cloud->points[pointIdxVec[i]].x
			<< " " << (*cloud)[pointIdxVec[i]].y
			<< " " << cloud->points[pointIdxVec[i]].z
			<< std::endl;
	}
 
	/**********************************************************
	采用K最近邻搜索，K=10。将搜索结果写入两个单独向量，第一个pointIdxNKNSearch
	包含搜索结果，第二个向量保存搜索点和最近邻点之间的相应平方距离
	**********************************************************/
	int K = 10;
	std::vector<int> pointIdxNKNSearch;
	std::vector<float> pointNKNSquareDistance;
 
	std::cout << "K nearest Neighbor seacher at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with K = " << K 
		<< std::endl;
 
	if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquareDistance) > 0)
	{
		for (std::size_t i = 0; i < K; ++i)
		{
			std::cout << (*cloud)[pointIdxNKNSearch[i]].x
				<<  " " << (*cloud)[pointIdxNKNSearch[i]].y
				<<  " " << (*cloud)[pointIdxNKNSearch[i]].z
				<< " (squared distance is " << pointNKNSquareDistance[i] << ")"
				<< std::endl;
		}
	}
 
	/**********************************************************
	采用半径邻居的搜索，与KNN类似，搜索结果写入两个单独向量，点索引和平方距离
	**********************************************************/
	std::vector<int> pointIdxDadiusSearch;
	std::vector<float> pointRadiusSquareDistance;
 
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
 
	std::cout << " Neighbor within radius seacher at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with radius = " << radius
		<< std::endl;
 
	if (octree.radiusSearch(searchPoint, radius, pointIdxDadiusSearch, pointRadiusSquareDistance) > 0)
	{
		for (std::size_t i = 0; i < K; ++i)
		{
			std::cout << (*cloud)[pointIdxDadiusSearch[i]].x
				<< " " << (*cloud)[pointIdxDadiusSearch[i]].y
				<< " " << (*cloud)[pointIdxDadiusSearch[i]].z
				<< " (squared distance is " << pointRadiusSquareDistance[i] << ")"
				<< std::endl;
		}
	}
}