PCL：八叉树Octree实现点云K近邻搜索

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

1 八叉树 Octree

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

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

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

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

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

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

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

2 K近邻搜索

K近邻搜索（K Nearest Neighbor Search），是指搜索距离某一点最近的K个近邻点。

3 代码实现

说明：搜索点云中第100个点的K近邻点，K=600。

代码：

#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 = 2;				//分辨率，即最低级叶子节点的大小
	pcl::octree::OctreePointCloudSearch<PointT> octree(resolution);	//初始化八叉树
	octree.setInputCloud(cloud);		//设置输入点云
	octree.addPointsFromInputCloud();	//构建八叉树
	//========================= 构建Octree =========================
	
	//------------------------ K近邻搜索 -----------------------
	PointCloudT cloud_k_neighbors;				//存放K近邻点
	PointT searchPoint = cloud->points[100];	//设置搜索点
	int K = 600;								//K近邻的大小
	vector<int> pointIdxNKNSearch;				//存放K近邻的索引
	vector<float> pointNKNSquaredDistance;		//存放K近邻的平方距离
	if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	
		cout << "->搜索点坐标：" << searchPoint << endl;
		cout << "->K近邻点个数：" << pointIdxNKNSearch.size() << endl;
		cout << "->K近邻点坐标和平方距离：" << endl;
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
		
			//cout << cloud->points[pointIdxNKNSearch[i]]<< "  平方距离: " << pointNKNSquaredDistance[i] << endl;
			cloud_k_neighbors.push_back(cloud->points[pointIdxNKNSearch[i]]);
		
		cout << "->耗时：" << time.toc() / 1000 << " s" << endl;
		//保存K近邻点
		pcl::io::savePCDFileBinary("cloud_k_neighbors.pcd", cloud_k_neighbors);
		cout << "->K近邻点详情：\\n" << cloud_k_neighbors << endl;
	
	//========================= K近邻搜索 =======================
	
	return 0;

输出结果：

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

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

分辨率	2m	0.2m	0.02m
耗时	0.145s	0.125s	0.228s

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

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

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

4 结果展示

白色点为原始点云

红色点为搜索点对应的K近邻点，$K=600$

5 注意

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

---

相关链接：

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