基于八叉树的空间划分及搜索操作-阿里云开发者社区

原理

建立空间索引在点云数据处理中有着广泛的应用，常见的空间索引一般 是自顶而下逐级划分空间的各种空间索引结构。
比较有代表性的包括

• BSP树
• KD树
• KDB树
• R树
• 四叉树
• 八叉树

这些结构中，KD树和八叉树使用比较广泛

八叉树定义： 八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。一般中心点作为节点的分叉中心。

八叉树算法思想

(1). 设定最大递归深度。

(2). 找出场景的最大尺寸，并以此尺寸建立第一个立方体。

(3). 依序将单位元元素丢入能被包含且没有子节点的立方体。

(4). 若没达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体。

(5). 若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。

(6). 重复3，直到达到最大递归深度。

下面实现用octree在点云数据中进行空间划分及近邻搜索
实现

• 体素内近邻搜索（Neighbors within VOxel Search）
• K近邻搜索（K Nearest Neighbor Search
• 半径内近邻搜索”（Neighbors within Radius Search）

Code

CmakeList.txt

# 声明要求的 cmake 最低版本
cmake_minimum_required(VERSION 2.8 )

# 声明工程名称
project(octree_search)

# 添加c++ 11 标准支持
set(CMAKE_CXX_FLAGS "-std=c++11")


# 寻找PCL的库
find_package(PCL REQUIRED COMPONENT common io visualization)


# 添加头文件
include_directories(${PCL_INCLUDE_DIRS})
add_definitions( ${PCL_DEFINITIONS} )


#添加一个可执行程序
add_executable(Octree_Search octree_search.cpp)

#链接PCL 库
target_link_libraries(Octree_Search ${PCL_LIBRARIES})

CPP

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <vector>
#include <ctime>

需要包含的头文件
cloud_viewer.h 这个文件如果不看 点云 的 话 可以不要

=======================================================

    //用系统时间初始化随机种子
    srand ((unsigned int) time (NULL));

    //声明一个点云指针 并分配空间
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    //定义点云大小  点云个数1000个，无序
    cloud->width =1000;
    cloud->height =1;
    cloud->points.resize (cloud->width * cloud->height);

    //循环给点云赋值 通过 随机数  点云的坐标 0-1024
    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);            
    }

通过系统数据创建随机变量 。 创建一个1000个点的点云，为每个点的x，y，z坐标赋值 0-1024

显然 正常 就是一个 立方体。 把点云显示出来就是这样

显示的话 加上如下 代码 在最后面

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    
    viewer.showCloud(cloud);

    while (!viewer.wasStopped ())
        {

        }

=======================================================

    /* 设置分辨率 描述的是最低一级 八叉树 的 最小体素 的 尺寸*/
    float resolution =128.0f;

    /*创建 一个 八叉树 实例*/
    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);

构建一个 搜索点 x、y、z 取值 0-1024 随机

=======================================================
上面构建了 八叉树 一个搜索点 下面就可以搜索了

体素 近邻 搜索

    // 创建一个 搜索后 保存 的 点的索引值 的 向量
    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;
        }
    }

函数 octree.voxelSearch (searchPoint, pointIdxVec)
就是执行 体素近邻搜索 函数

• 参数1 -----搜索的点
• 参数2 ----- 返回的索引值 向量

=======================================================

K 近邻 搜索

   //设置K的个数
    int K = 10;
    // 创建一个 搜索后 保存 的 点的索引值 的 向量
    std::vector<int>pointIdxNKNSearch;
    // 创建一个 搜索后 保存 的 点的距离平方值 的 向量
    std::vector<float>pointNKNSquaredDistance;

    //打印 K近邻 搜索 的点  和 K个数
    std::cout<<"K nearest neighbor search at ("<<searchPoint.x
    <<" "<<searchPoint.y
    <<" "<<searchPoint.z
    <<") with K="<< K <<std::endl;

    /*执行 K近邻 搜索  函数 参数 搜索点  K个数  搜索结果的点索引值存放向量  搜索结果点距离平方存放向量*/
    if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0)
    {
         //打印搜索到的点 的 位置 和距离
        for (size_t i=0; i<pointIdxNKNSearch.size (); ++i)
        {
            std::cout<<"    "<<   cloud->points[ pointIdxNKNSearch[i] ].x 
            <<" "<< cloud->points[pointIdxNKNSearch[i] ].y 
            <<" "<< cloud->points[pointIdxNKNSearch[i] ].z 
            <<" (squared distance: "<<pointNKNSquaredDistance[i] <<")"<<std::endl;
        }
    }

函数 octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)
执行 K近邻 搜索 函数

• 参数1 ----- 搜索点
• 参数2 ----- K个数
• 参数3 ----- 搜索结果的点索引值存放向量
• 参数4 ----- 搜索结果点距离平方存放向量

=======================================================

半径内 近邻 搜索

    // 创建一个 搜索后 保存 的 点的索引值 的 向量
    std::vector<int>pointIdxRadiusSearch;
    // 创建一个 搜索后 保存 的 点的距离平方值 的 向量
    std::vector<float>pointRadiusSquaredDistance;
    // 设置搜索的半径  随机0-256
    float radius =256.0f* rand () / (RAND_MAX +1.0f);

    //打印 半径搜索 的 点 的 位置  和  半径 
    std::cout<<"Neighbors within radius search at ("<<searchPoint.x
    <<" "<<searchPoint.y
    <<" "<<searchPoint.z
    <<") with radius="<< radius <<std::endl;

    /* 执行 半径 近邻 搜索  参数 搜索点  半径   搜索结果的点索引值存放向量  搜索结果点距离平方存放向量 */
    if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >0)
    {   
        //打印搜索到的点 的 位置 和距离
        for (size_t i=0; i<pointIdxRadiusSearch.size (); ++i)
        {
            std::cout<<"    "<<   cloud->points[ pointIdxRadiusSearch[i] ].x 
            <<" "<< cloud->points[pointIdxRadiusSearch[i] ].y 
            <<" "<< cloud->points[pointIdxRadiusSearch[i] ].z 
            <<" (squared distance: "<<pointRadiusSquaredDistance[i] <<")"<<std::endl;
        }
    }

函数 octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)
执行 半径 近邻 搜索 函数

• 参数1 ----- 搜索点
• 参数2 ----- 半径
• 参数3 ----- 搜索结果的点索引值存放向量
• 参数4 ----- 搜索结果点距离平方存放向量

=======================================================

Result