PCL点云库入门——PCL库中点云数据拓扑关系之Octree（八叉树）

1、八叉树（Octree）理论

1.1、八叉树空间划分

        若以长方体空间来表示一个三维模型，在X轴、Y轴和Z轴三个方向上各进行一次分割，便能获得8个更小的长方体子空间。每个子空间能够容纳相应的点云数据。若子空间内的点云数据量较大，分割过程可以继续进行。分割的具体规则可以由用户自定义，例如，当一个子空间中的点云数据量超过8个时，就继续进行分割。所有这些子空间可以形成一个树状结构，从整体上看，每个节点都分叉出八个子节点，如图1所示，这种树状结构被称为八叉树数据结构。

图1 

        八叉树法建立点云数据之间拓扑就是在八叉树数据结构下来实现的，八叉树数据结构由Hunter博士在1978年提出。由于八叉树数据结构能够很好的表达三维物体模型，在三维图形图像邻域得到了广泛的应用。在八叉树数据结构中，八叉树的根节点表示整个物体包含的三维空间，相当于一边长为2n的立方体，经过n次划分之后，点云数据落在子节点边长为2^i的小立方体中。八叉树划分准则常有两种：一种以最小立方体的边长准则；另外一种是以划分立方体中所包含的点云数据量准则，两种条件各有利弊。在八叉树的每个最终的子节点对应于所要包含最简单的目标或者预先设定的最小值，通常称为八叉树的分辨率，在表示复杂模型时，精细程度就会受到这个分辨率的制约。八叉树划分的点云模型如2图所示。

图2 

1.2、线性八叉树编码实现点云数据拓扑原理

        八叉树数据结构的实现和编码方式很多，目前主流的方式有线性八叉树、深度优先编码、规则八叉树和三维行程编码等。本节采用线性八叉树数据的编码结构，来构建点云数据之间的拓扑访问。用线性八叉树数据结构编码时需要按照如图3所示将根节点按照先X轴、然后Y轴，最后Z轴的方式进行编码，以便对不同点云数据之间的访问，被划分的立方体的编码与它所在的位置有关。

图3

线性八叉树以最小立方体边长值作为终止阈值，建立点云数据拓扑的实现过程如下。计算出点云数据中的临界值（Xmin，Ymin，Zmin）和（Xmax，Ymax，Zmax），确定包围盒的尺寸为：

………………（1）

再结合式子(1)求得值为dave：，式中：N点云的总量。将dave作为八叉树划分终止的阈值 。

        对上面得到的包围盒进行递归的划分，划分后每一个叶子节点都对应一个Morton地址码,在线性八叉树中Morton码表示的是从跟节点到叶子节点的路径值。在线性八叉树中任意一个节点Vq(xq,yq,zq)的路径Pq，用二进制表示，为式子（2）

         在同一个划分层次中，各节之间的领域关系是Pm∈{0…，7}，Pm+1表示节点Pm父亲节点与其邻域之间的关系，因为树的最大深度为n,因此m∈{0…，n-1}。由式子(2)可得，叶子节点到根节点之间通过P0到Pn-1来建立关联，则节点Vq(xq,yq,zq)的Morton编码Mq用八进制表示为，式子（3）

         从上面(3)式子可知，三维点云数据与Morton编码之间没有直接关系，因此必须建立它们之间索引关联，才能对点云数据进行操作处理。用点云的坐标求出在八叉树节点中的索引值为：式子（4）

式中： 表示向上取整，(Px, Py, Pz)表示点云数据任意一点pi的坐标值，dave最小立方体的边长。为了与八叉树的Morton编码建立关键，需要将上面的索引值转换为二进制的表示形式：式子（5）

根据3图，节点之间的编码与点云数据坐标值二进制之间满足下面的式子： 式子（6）

        将式(6)带入(3)式中，则可以得到点云数据在八叉树节点中的Morton编码，进而建立了点云数据的拓扑关系。反过来，知道了节点的Morton编码，也可以求出点云数据的空间索引号。 

2、PCL点云库中的八叉树（Octree） 

        PCL库中的Octree模组提供了多种Octree组件，这些组件能够支持点云数据的高效处理和检索。例如，OctreeSearch类允许用户快速查询点云中的近邻点，而OctreeBase类则为其他Octree组件提供了基础功能。此外，Octree类结合了OctreeBase和OctreeSearch的功能，为点云数据的存储和检索提供了完整的解决方案。通过这些组件，开发者可以轻松实现点云数据的分割、特征提取和空间查询等操作，极大地提高了处理大规模点云数据的效率和准确性。此外，PCL库中的Octree模组还引入了动态Octree的概念，这种数据结构能够根据点云数据的动态变化进行自适应调整。动态Octree通过监测点云数据的添加和删除操作，自动调整其内部节点结构，从而确保数据的高效存储和快速访问。

        Octree模组还支持多线程处理，能够充分利用现代多核处理器的计算能力。通过并行处理点云数据的分割、索引和检索等操作，Octree模组能够显著提高处理速度，降低处理时间。此外，PCL库中的Octree模组还提供了丰富的可视化工具，帮助开发者直观地理解点云数据的空间分布和Octree结构。这些工具可以生成点云数据的三维可视化图形，并允许用户通过交互方式调整Octree的参数和视图设置。

        本节将首先介绍用于构建点云数据拓扑关系的OctreePointCloud<T>类以及用于K领域搜索的OctreePointCloudSearch<T>类。其他组件将在后续章节中进行详细讲解。

2.1、OctreePointCloud<T>类

        OctreePointCloud类是PCL库中用于处理点云数据的核心类之一。它利用Octree数据结构来组织和管理点云数据，使得数据的查询和检索更加高效。该类支持点云数据的插入和删除操作，并且能够根据点云数据的变化动态地调整Octree的结构。开发者可以通过OctreePointCloud类轻松实现对点云数据的快速遍历和空间查询，这对于需要实时处理大量点云数据的应用场景尤为重要。此外，OctreePointCloud类还提供了多种接口，主要的函数如下表1所示。

2.2、 OctreePointCloudSearch<T>类

       OctreePointCloudSearch<T>类实现了基于八叉树的空间点云搜索算，是 OctreePointCloud类的子类。该算法通过递归地将空间划分为更小的区域，有效地减少了搜索范围，提高了查询效率。其他主要的函数如表2.

3、PCL库中八叉树代（OCtree）码示例  

        新建文件PCLOctreemain.cpp,内容如下：

/*****************************************************************//**
* \file   PCLOctreemain.cpp
* \brief
*
* \author YZS
* \date   December 2024
*********************************************************************/
#include<iostream>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/auto_io.h>
#include <pcl/octree/octree.h>
#include <pcl/visualization/pcl_visualizer.h>
using namespace std;
void PCLOctreeUse()
{
    // 随机种子初始化
    srand(time(NULL));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // 生成点云数据3000个
    cloud->width = 3000;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.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);
    }
  
    // 八叉树点云分辨率
    float resolution = 64.0f;
    //创建OctreePointCloudSearch 对象
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
   //设置输入点云
    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);

    //体素搜索
    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 (size_t i = 0; i < pointIdxVec.size(); ++i)
        {
       
        std::cout << "    " << cloud->points[pointIdxVec[i]].x
            << " " << cloud->points[pointIdxVec[i]].y
            << " " << cloud->points[pointIdxVec[i]].z << std::endl;
        }
    }
    // 方式1：K个最近邻域搜索
    int K = 100;//表示搜索100个临近点
    std::vector<int>KNSearch(K);//  保存搜索到的临近点的索引
    std::vector<float> KNSquaredDistance(K);//保存对应临近点的距离的平方
    std::cout << "K nearest neighbor search at (" << searchPoint.x
        << " " << searchPoint.y
        << " " << searchPoint.z
        << ") with K=" << K << std::endl;
    //保存K个领域的点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudK(new  pcl::PointCloud<pcl::PointXYZ>);
    if (octree.nearestKSearch(searchPoint, K, KNSearch, KNSquaredDistance) > 0)
    {
        for (size_t i = 0; i < KNSearch.size(); ++i) {
            std::cout << "    " << cloud->points[KNSearch[i]].x
                << " " << cloud->points[KNSearch[i]].y
                << " " << cloud->points[KNSearch[i]].z
                << " (squared distance: " << KNSquaredDistance[i] << ")" << std::endl;
            pcl::PointXYZ pt;
            pt.getVector3fMap() = cloud->points[KNSearch[i]].getVector3fMap();
            cloudK->points.emplace_back(pt);
        }
    }
    // 方式2：R半径邻域搜索
    std::vector<int> radiusSearch;
    std::vector<float>radiusSquaredDistance;
    // R半径值
    float radius = 300.0f;
    std::cout << "Neighbors within radius search at (" << searchPoint.x
        << " " << searchPoint.y
        << " " << searchPoint.z
        << ") with radius=" << radius << std::endl;
    //保存R半径领域的点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudR(new  pcl::PointCloud<pcl::PointXYZ>);
    if (octree.radiusSearch(searchPoint, radius, radiusSearch, radiusSquaredDistance) > 0)
    {
        for (size_t i = 0; i < radiusSearch.size(); ++i) {
            std::cout << "    " << cloud->points[radiusSearch[i]].x
                << " " << cloud->points[radiusSearch[i]].y
                << " " << cloud->points[radiusSearch[i]].z
                << " (squared distance: " << radiusSquaredDistance[i] << ")" << std::endl;
            pcl::PointXYZ pt;
            pt.getVector3fMap() = cloud->points[radiusSearch[i]].getVector3fMap();
            cloudR->points.emplace_back(pt);
        }
    }
    //K个邻域的结果可视化--双窗口
// PCLVisualizer对象
    pcl::visualization::PCLVisualizer viewer("OctreeVIS");
    //创建左右窗口的ID v1和v2
    int v1(0);
    int v2(1);
    //设置V1窗口尺寸和背景颜色
    viewer.createViewPort(0.0, 0.0, 0.5, 1, v1);
    viewer.setBackgroundColor(0, 0, 0, v1);
    //设置V2窗口尺寸和背景颜色
    viewer.createViewPort(0.5, 0.0, 1, 1, v2);
    viewer.setBackgroundColor(0.1, 0.1, 0.1, v2);
    //设置cloud1的渲染颜色，点云的ID和指定可视化窗口v1
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>   cloud1_color(cloud, 255, 255, 255);
    viewer.addPointCloud(cloud, cloud1_color, "cloud1", v1);
    //设置cloud2的渲染颜色，点云的ID和指定可视化窗口v2
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>   cloud2_color(cloud, 250, 255, 255);
    viewer.addPointCloud(cloud, cloud2_color, "cloud2", v2);
    //添加cloudK到可视化窗口v1中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>   cloudk_color(cloudK, 255, 0, 0);
    viewer.addPointCloud(cloudK, cloudk_color, "cloudk", v1);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloudk");
    //添加cloudR到可视化窗口v2中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>   cloudr_color(cloudK, 0, 255, 0);
    viewer.addPointCloud(cloudR, cloudr_color, "cloudr", v2);
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloudr");
    // 可视化循环主体
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
}
int main(int argc, char* argv[])
{
    PCLOctreeUse();
    std::cout << "Hello World!" << std::endl;
    std::system("pause");
    return 0;
}

        结果：

4、PCL库中KDtree与Octree的搜索耗时比较

        对比代码如下：

/*****************************************************************//**
* \file   PCLKDtreeVSOctreemain.cpp
* \brief
*
* \author YZS
* \date   December 2024
*********************************************************************/
#include<iostream>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/auto_io.h>
#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/console/time.h>
#include <pcl/visualization/pcl_visualizer.h>


void KdTreeVSOctree()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	
	std::string file_name="E:/PCLlearnData/8/fragment.pcd";
	pcl::io::load(file_name, *cloud);

	float radius = 0.02;
	int k = 9;
	std::cout << "Radius=" << radius << std::endl << "K=" << k << std::endl;
	
	pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree(new  pcl::KdTreeFLANN<pcl::PointXYZ>);
	kdtree->setInputCloud(cloud);

	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(radius);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	pcl::console::TicToc t1;
	t1.tic();
	for (auto& i : *cloud)
	{
		std::vector<int>  indices(0, 0);
		octree.voxelSearch(i, indices);
	}
	std::cout << "octree体素搜索耗时" << t1.toc() << "ms" << std::endl;
	std::cout << std::endl;
	std::cout << "====================================" << std::endl;
	std::cout << "====================================" << std::endl;
	std::cout << std::endl;

	t1.tic();
	for (auto& i : *cloud)
	{
		std::vector<int>  indices(0, 0);
		std::vector<float> dists(0, 0.0);
		octree.radiusSearch(i, radius, indices, dists);
	}

	std::cout << "octree半径近邻搜索耗时" << t1.toc() << "ms" << std::endl;


	t1.tic();
	for (auto& i : *cloud)
	{
		std::vector<int> kindices(0, 0);
		std::vector<float>kdists(0, 0.0);
		kdtree->radiusSearch(i, radius, kindices, kdists);
	}
	std::cout << "kdtree半径搜索耗时" << t1.toc() << "ms" << std::endl;
	std::cout << std::endl;
	std::cout << "====================================" << std::endl;
	std::cout << "====================================" << std::endl;
	std::cout << std::endl;
	t1.tic();
	for (auto& i : *cloud)
	{
		std::vector<int>  oindices(0, 0);
		std::vector<float> odists(0, 0.0);
		octree.nearestKSearch(i, k, oindices, odists);
		
	}
	std::cout << "octreeK近邻搜索耗时" << t1.toc() << "ms" << std::endl;
	
	t1.tic();
	for (auto& i : *cloud)
	{
		std::vector<int>  oindices(0, 0);
		std::vector<float> odists(0, 0.0);
		kdtree->nearestKSearch(i, k, oindices, odists);
	}
	std::cout << "kdtreeK近邻搜索耗时" << t1.toc() << "ms" << std::endl;
}

int main(int argc, char* argv[])
{
	KdTreeVSOctree();
	std::cout << "Hello World!" << std::endl;
	std::system("pause");
	return 0;
}

输出结果：

从上面结果可知：

1)、 Octree体素搜索相对其他近邻搜索方法都快

2）、KDtree的半搜索和K近邻搜索耗均优于Octree，所以使用优先用KDtree

3）、搜索的半径越大或者K越大，越耗时

         至此完成第八节PCL库中点云数据拓扑关系之Octree树简单学习，下一节我们将进入《PCL库中点云滤波算法》的学习。