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

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

1 八叉树 Octree

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

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

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

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

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

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

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

2 半径内近邻搜索

半径内近邻搜索（Neighbors within Radius Search），是指搜索点云中一点在 $球体半径R$ 内的所有近邻点。

3 代码实现

说明：搜索点云中第100个点在球体半径 $R=0.2m$ 内的近邻点。

代码：

#define BOOST_TYPEOF_EMULATION
#include <pcl/io/pcd_io.h>
#include <pcl/octree/octree.h>
#include <pcl/console/time.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;
	//========================== 加载点云 ==========================
	
	pcl::console::TicToc time;
	time.tic();		//开始计时
	//------------------------- 构建Octree -------------------------
	float resolution = 0.2;				//分辨率，即最低级叶子节点的大小
	pcl::octree::OctreePointCloudSearch<PointT> octree(resolution);	//初始化八叉树
	octree.setInputCloud(cloud);		//设置输入点云
	octree.addPointsFromInputCloud();	//构建八叉树
	//========================= 构建Octree =========================
	
	//------------------------ 半径内近邻搜索 -----------------------
	PointCloudT cloud_r_neighbors;					//存放半径内近邻点
	PointT searchPoint = cloud->points[100];		//设置搜索点
	float radius = 0.2;								//邻域半径
	std::vector<int>pointIdxRadiusSearch;			//存放R近邻的索引
	std::vector<float>pointRadiusSquaredDistance;	//存放R近邻的平方距离

	if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
	
		cout << "->搜索点坐标：" << searchPoint << endl;
		cout << "->R近邻点个数：" << pointIdxRadiusSearch.size() << endl;
		cout << "->R近邻点坐标和平方距离：" << endl;
		for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
		
			//cout << cloud->points[pointIdxRadiusSearch[i]]<< "  平方距离: " << pointRadiusSquaredDistance[i] << endl;
			cloud_r_neighbors.push_back(cloud->points[pointIdxRadiusSearch[i]]);
		
		cout << "->耗时：" << time.toc() / 1000 << " s" << endl;
		//保存R近邻点
		pcl::io::savePCDFileBinary("cloud_r_neighbors.pcd", cloud_r_neighbors);
		cout << "->R近邻点详情：\\n" << cloud_r_neighbors << endl;
	
	//======================== 半径内近邻搜索 =======================

	return 0;

输出结果：

->正在加载点云...
->加载点云点数：41049
->搜索点坐标：(-0.015396,-0.29139,-1.901)
->R近邻点个数：1000
->R近邻点坐标和平方距离：
->耗时：0.125 s
->R近邻点详情：
points[]: 1000
width: 1000
height: 1
is_dense: 1
sensor origin (xyz): [0, 0, 0] / orientation (xyzw): [0, 0, 0, 1]

不同分辨率耗时情况对比（多次实验取平均值）

分辨率	2m	0.2m	0.02m
耗时	0.168 s	0.125 s	0.233 s

根据实验结果，可以初步得出：八叉树的分辨率越高，搜索速度不一定越快。

由于实验点云点数较少，感兴趣的小伙伴可以自行探索。

因此，避免需要设置分辨率的情况，推荐使用 kd-tree进行半径R内近邻搜索

4 结果展示

白色点为原始点云

红色点为原始点云中第100个点对应的半径内邻域点，$R=0.2m$

5 注意

• 邻域点按其到搜索点的平方距离，从小到大进行排序，距离最小的点为搜索点本身（如果搜索点为原始点云中的一点）。
• 当搜索点 $searchPoint$ 为原始点云中一点时，其半径内近邻点包含搜索点本身；否则，不包含。

---

相关链接：

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