PCL：八叉树（Octree）实现点云体素内近邻搜索

Posted 2022-01-08 没事就要敲代码

1 八叉树 Octree

八叉树（Octree）结构是由 Hunter 博士于1978年首次提出的一种数据模型。八叉树结构通过对三维空间的几何实体进行体元剖分，每个体元具有相同的时间和空间复杂度，通过循环递归的划分方法对三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。在八叉树结构中如果被划分的体元具有相同的属性，则该体元构成一个叶节点；否则继续对该体元剖分成8个子立方体，依次递剖分，如下示意图所示。

八叉树是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个叶子节点，这八个叶子节点所表示的体积元素加在一起就等于父节点的体积。一般中心点作为节点的分中心。八叉树若不为空树的话，树中的任意节点的子节点恰好只会有八个或零个，子节点不会有0和8以外的数目。

八叉树的分辨率是指最低一级的叶子节点的尺寸。如果分辨率设置为0.01m，那么每个叶子节点就是一个1cm的小方块。

简单来说，八叉树的存储结构可用于对应元素的查找，依次对立方体进行划分，寻找查找元素对应的小立方体再次进行划分查找。

生成的八叉树的节点可分为三类：

• 灰节点： 它对应的立方体部分的为查找元素所占据；
• 白节点： 它对应的立方体没有查找元素的内容；
• 黑节点： 它对应的立方体全部为查找元素所占据。

参考链接：PCL中八叉树理论

2 体素内近邻搜索

体素内近邻搜索（Neighbors within Voxel Search），是指在某一点所在的体素中搜索该点的近邻点。搜索点和搜索结果间的最大距离取决于八叉树的分辨率。

3 代码实现

#define BOOST_TYPEOF_EMULATION
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree.h>

using namespace std;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main()

	//-------------------------- 加载点云 --------------------------
	cout << "->正在加载点云..." << endl;
	PointCloudT::Ptr cloud(new PointCloudT);
	if (pcl::io::loadPCDFile("1.pcd", *cloud) < 0)
	
		PCL_ERROR("\\a->点云文件不存在！\\n");
		system("pause");
		return -1;
	
	cout << "->加载点云点数：" << cloud->points.size() << endl;
	//========================== 加载点云 ==========================
	
	//------------------------- 构建Octree -------------------------
	float resolution = 0.2;				//Octree分辨率，即最低级叶子节点的大小
	pcl::octree::OctreePointCloudSearch<PointT> octree(resolution);	//初始化八叉树
	octree.setInputCloud(cloud);		//设置输入点云
	octree.addPointsFromInputCloud();	//构建八叉树
	//========================= 构建Octree =========================

	//------------------------ 体素内近邻搜索 -----------------------
	//设置搜索点
	pcl::PointXYZ searchPoint;			
	searchPoint = cloud->points[100];

	// 体素内近邻搜索
	PointCloudT cloud_voxel_neighbor;	//存放体素内近邻点点云
	vector<int> pointIdxVec;			//存放体素近邻搜索的结果向量
	if (octree.voxelSearch(searchPoint, pointIdxVec))
	
		cout << "->搜索点坐标：" << searchPoint << endl;
		cout << "->体素近邻点个数：" << pointIdxVec.size() << endl;
		cout << "->体素近邻点坐标：" << endl;
		for (size_t i = 0; i < pointIdxVec.size(); ++i)
		
			//cout << cloud->points[pointIdxVec[i]] << endl;
			cloud_voxel_neighbor.push_back(cloud->points[pointIdxVec[i]]);
		
		//提取体素内近邻点
		pcl::io::savePCDFileBinary("cloud_voxel_neighbor.pcd", cloud_voxel_neighbor);
		cout << "->体素内近邻点详情：\\n" << cloud_voxel_neighbor << endl;
	
	//======================== 体素内近邻搜索 =======================
	
	return 0;

输出结果：

->正在加载点云...
->加载点云点数：41049
->搜索点坐标：(-0.015396,-0.29139,-1.901)
->体素近邻点个数：642
->体素近邻点坐标：
->体素内近邻点详情：
points[]: 642
width: 642
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

4 结果展示

白色点为原始点云

红色点为当 $八叉树分辨率=0.2m$ 时，点云中一点（本次实验选择的第100个点）对应的近邻点。

5 注意

• 根据需求设置Octree分辨力
• 当搜索点 $searchPoint$ 为原始点云中一点时，其体素内近邻点包含搜索点本身；否则，不包含。

---

相关链接：

《PCL点云数据处理基础》目录