3 PCL-octree

3.1  区别

kdtree只有k近领域搜索和半径搜索

octree可以体素内近邻搜索、k近领域搜索和半径搜索

3.2  定义

八叉树结构通过对三维空间的几何实体进行体元剖分，每个体元具有相同的时间和空间复杂度，通过循环递归的划分方法对大小为 (2n∗2n∗2n)的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。在八叉树结构中如果被划分的体元具有相同的属性，则该体元构成一个叶节点；否则继续对该体元剖分成8个子立方体，依次递剖分，对于 (2n∗2n∗2n)大小的空间对象，最多剖分 n次，如下图所示。

3.3  代码

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>

#include <iostream>
#include <vector>
#include <ctime>
using namespace pcl;
using namespace std;

int main(int argc, char** argv)
{
    srand((unsigned int)time(NULL));

    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);//cloud指针

    // 创建点云数据，随机构造点云
    cloud->width = 1000;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);
    for (std::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 = 128.0f;
// 分辨率,最小体素尺寸，不再单独往下分了：可以看作是八叉树的深度受分辨率和空间维度的影响

    octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // 设置分辨率初始化
    // 设置输入点云
    octree.setInputCloud(cloud);
    // 定义Octree边界框(可选操作)
    //计算输入点云的边界框
   // octree.defineBoundingBox();
    //手动定义点云的边界框
    //octree.defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
    // 输入点云添加到Octree，执行构建八叉树，上面只是把点云输进去，没实际执行
    octree.addPointsFromInputCloud();

    // 创建一个随机点
    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);

    // Neighbors within voxel search
    std::vector<int> pointIdxVec;// 存储索引，存储索引，对cloud点云的索引

    // 体素近邻搜索：把和查询点在同一体素中的点的索引返回
    if (octree.voxelSearch(searchPoint, pointIdxVec))
    {
        std::cout << "Neighbors within voxel search at (" << searchPoint.x
            << " " << searchPoint.y
            << " " << searchPoint.z << ")"
            << std::endl;

        for (std::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;
    }

    // K nearest neighbor search
    // k近邻搜索
    int K = 10;

    std::vector<int> pointIdxNKNSearch; // 搜索结果的索引
    std::vector<float> pointNKNSquaredDistance;// 对应的距离值

    std::cout << "K nearest neighbor search at (" << searchPoint.x
        << " " << searchPoint.y
        << " " << searchPoint.z
        << ") with K=" << K << std::endl;

    if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        for (std::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;
    }

    // Neighbors within radius search
    // 半径近邻搜索
    std::vector<int> pointIdxRadiusSearch;// 搜索结果的索引
    std::vector<float> pointRadiusSquaredDistance;// 对应的距离

    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 (std::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.deleteTree();
}

3.4  结果

索引的结果是128分辨率时的（565.625  88.8125  342.062）这个点作为一个小体素块时里面包含的另外535.。。。和515.。。。的两个点在它体内

3.5  注意

如果需要高效内存管理，空间检测，需要使用八叉树双缓冲技术实现（Octree2BufBase类），该类可以同时保存两个类似的八叉树对象

3.6  八叉树的五种类型