PCL学习笔记(三)——————octree八叉树

介绍

八叉树（octree）是一种树形数据结构，每个内部节点都正好有八个子节点。八叉树常用于分割三维空间，将其递归细分为八个象限。

原理

八叉树算法将空间递归地划分为八个子空间（称为八分体），每个子空间可以进一步细分，直到满足特定条件（如最小分辨率或点数量）。
Voxel，体积元素，简称体素，描述一个预设的最小单位的正方体。

示例

一、点云压缩

https://pointclouds.org/documentation/tutorials/compression.html#octree-compression

点云数据具有大数据量、高生成速率和多样性的特点，因此压缩技术对于点云的存储和传输至关重要。PCL 提供了强大的点云压缩功能，支持多种点云类型，并通过八叉树数据结构实现了高效的数据合并和处理。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/compression/octree_pointcloud_compression.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <cmath>   // 用于数学计算
#include <cstdlib> // 用于rand()和srand()
#include <ctime>   // 用于时间函数
#include <thread>

class SimplePointCloudCompressor
{
public:
    SimplePointCloudCompressor() : viewer("Point Cloud Compression Example") {}

    void generateTorusAndProcessCloud()
    {
        // 创建一个环形圆环点云
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
        float majorRadius = 1.0f;    // 主半径
        float minorRadius = 0.25f;   // 次半径
        int numPoints     = 1000000; // 点的数量

        srand(static_cast<unsigned>(time(0))); // 初始化随机数种子
        for (int i = 0; i < numPoints; ++i) {
            pcl::PointXYZRGBA point;

            // 使用球坐标系生成点
            float theta = 2 * M_PI * static_cast<float>(rand()) / RAND_MAX; // 环面角度
            float phi   = 2 * M_PI * static_cast<float>(rand()) / RAND_MAX; // 截面角度

            point.x = (majorRadius + minorRadius * cos(phi)) * cos(theta);
            point.y = (majorRadius + minorRadius * cos(phi)) * sin(theta);
            point.z = minorRadius * sin(phi);

            point.r = static_cast<uint8_t>((theta / (2 * M_PI)) * 255); // 根据位置设置颜色
            point.g = static_cast<uint8_t>((phi / (2 * M_PI)) * 255);
            point.b = 128;
            cloud->points.push_back(point);
        }
        cloud->width  = static_cast<uint32_t>(cloud->points.size());
        cloud->height = 1;

        if (!viewer.wasStopped()) {
            std::stringstream compressedData;
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZRGBA>());

            // 压缩点云
            PointCloudEncoder->encodePointCloud(cloud, compressedData);

            // 解压缩点云
            PointCloudDecoder->decodePointCloud(compressedData, cloudOut);

            // 添加原始点云到视口1
            viewer.createViewPort(0.0, 0.0, 0.5, 1.0, viewport1); // 视口1占据左侧一半
            viewer.setBackgroundColor(0, 0, 0, viewport1);
            viewer.addPointCloud<pcl::PointXYZRGBA>(cloud, "original_cloud", viewport1);

            // 添加解压缩后的点云到视口2
            viewer.createViewPort(0.5, 0.0, 1.0, 1.0, viewport2); // 视口2占据右侧一半
            viewer.setBackgroundColor(0.1, 0.1, 0.1, viewport2);
            viewer.addPointCloud<pcl::PointXYZRGBA>(cloudOut, "decompressed_cloud", viewport2);
            viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0,
                                                    "decompressed_cloud", viewport2); // 设置解压后点云为绿色

            while (!viewer.wasStopped()) {
                viewer.spinOnce(100);
                std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 控制更新频率
            }
        }
    }

    void run()
    {
        bool showStatistics                                = true;
        pcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

        // 实例化编码器和解码器
        PointCloudEncoder =
            new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>(compressionProfile, showStatistics);
        PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>();

        // 生成并处理点云
        generateTorusAndProcessCloud();

        // 清理资源
        delete PointCloudEncoder;
        delete PointCloudDecoder;
    }

    pcl::visualization::PCLVisualizer viewer;

    int viewport1, viewport2;

    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> *PointCloudEncoder;
    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> *PointCloudDecoder;
};

int main()
{
    SimplePointCloudCompressor compressor;
    compressor.run();
    return 0;
}

二、基于八叉树的空间划分与搜索操作

搜索方式：

• 体素近邻搜索
• k近邻搜索
• 半径范围近邻搜索

#include <pcl/point_cloud.h>   //点云头文件
#include <pcl/octree/octree.h> //八叉树头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>

#include <iostream>
#include <vector>
#include <ctime>

int main(int argc, char **argv)
{
    srand((unsigned int)time(NULL)); //用系统时间初始化随机种子与 srand (time (NULL))的区别

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 创建点云数据
    cloud->width  = 10000;
    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 = 128.0f;

    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); //初始化Octree

    octree.setInputCloud(cloud);      //设置输入点云
    octree.addPointsFromInputCloud(); //构建octree
    pcl::PointXYZ searchPoint;        //设置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)) //执行搜索，返回结果到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;
    }

    // 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 (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;
    }

    // 半径内近邻搜索
    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 (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;
    }
}

三、无组织点云数据的空间变化检测

https://pointclouds.org/documentation/tutorials/octree_change.html#octree-change-detection

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

#include <iostream>
#include <vector>
#include <ctime>

int main(int argc, char **argv)
{
    srand((unsigned int)time(NULL)); //用系统时间初始化随机种子

    float resolution = 32.0f; // 八叉树的分辨率，即体素的大小

    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution); // 初始化空间变化检测对象

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(
        new pcl::PointCloud<pcl::PointXYZ>); //创建点云实例cloudA生成的点云数据用于建立八叉树octree对象

    cloudA->width  = 128;
    cloudA->height = 1;
    cloudA->points.resize(cloudA->width * cloudA->height);

    for (size_t i = 0; i < cloudA->points.size(); ++i) {
        cloudA->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloudA->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloudA->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
    }

    octree.setInputCloud(cloudA);     //设置输入点云
    octree.addPointsFromInputCloud(); //从输入点云构建八叉树

    octree.switchBuffers(); // 交换八叉树的缓冲，但是CloudA对应的八叉树结构仍然在内存中
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);

    cloudB->width  = 128;
    cloudB->height = 1;
    cloudB->points.resize(cloudB->width * cloudB->height);

    for (size_t i = 0; i < cloudB->points.size(); ++i) {
        cloudB->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloudB->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloudB->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
    }

    octree.setInputCloud(cloudB);
    octree.addPointsFromInputCloud();

    std::vector<int> newPointIdxVector; //存储新添加的索引的向量

    octree.getPointIndicesFromNewVoxels(newPointIdxVector); //获取cloudB相较于cloudA变化的点集

    std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
    for (size_t i = 0; i < newPointIdxVector.size(); ++i)
        std::cout << i << "# Index:" << newPointIdxVector[i] << "  Point:" << cloudB->points[newPointIdxVector[i]].x
                  << " " << cloudB->points[newPointIdxVector[i]].y << " " << cloudB->points[newPointIdxVector[i]].z
                  << std::endl;
}

pcl_octree_viewer使用

• a -> 增加显示深度（减小体素大小）
• z -> 降低显示深度 （增加体素大小）
• v -> 隐藏或显示octree立方体
• b -> 隐藏或显示体素中心店
• n -> 隐藏或显示原始点云
• q -> 退出

参考

https://robot.czxy.com/docs/pcl/chapter01/decomposition/#_4
https://github.com/HuangCongQing/pcl-learning/blob/master/03octree/1point_cloud_compression/point_cloud_compression.cpp
https://pointclouds.org/documentation/group__octree.html