ocTree（八叉树，点云的压缩，搜索，点云空间变化）

背景

背景：
● 通过雷达、激光扫描、立体摄像机等三维测量设备获取的点云数据，具有数据量大、分布不均匀等特点。作为三维领域中一个重要的数据来源，点云数据主要是表征目标表面的海量点集合，并不具备传统网格数据的集合拓扑信息。所以点云数据处理中最为核心的问题就是建立离散点间的拓扑关系，实现基于邻域关系的快速查找。
● 建立空间索引在点云数据处理中有着广泛的应用，常见的空间索引一般 是自顶而下逐级划分空间的各种空间索引结构
○ 比较有代表性的包括BSP树，KD树，KDB树，R树，四叉树，八叉树等索引结构
○ 而这些结构中，KD树和八叉树使用比较广泛

简单来说，空间可以被分为8个象限，想象一下假设空间中存在一个笛卡尔坐标系，则该坐标系将空间分为了8个象限，每个象限又可以按照这种方式再建立一个笛卡尔坐标系，再划分为8个象限，以此类推，空间中任何一块区域都可以被n个笛卡尔坐标划分。 其中每一个小立方体都是一个子节点，子节点可以继续被划分。如下图所示：

什么是八叉树

八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。
● 一般中心点作为节点的分叉中心。
● 八叉树若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。
● 分割一直要进行到节点所对应的立方体或是完全空白，或是完全为V占据，或是其大小已是预先定义的体素大小，并且对它与V之交作一定的“舍入”，使体素或认为是空白的，或认为是V占据的。

实现八叉树原理：

(1). 设定最大递归深度。
　　(2). 找出场景的最大尺寸，并以此尺寸建立第一个立方体。
　　(3). 依序将单位元元素丢入能被包含且没有子节点的立方体。
　　(4). 若没达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体。
　　(5). 若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。
　　(6). 重复3，直到达到最大递归深度。

八叉树的应用：

1、点云压缩

点云是由海量的数据集组成，这些数据集通过距离 颜色 法线 等信息来描述三维点，此外，点云还能以非常高的速度被创建出来，因此需要占用相当大的存储资源，一旦点云需要存储或者通过速率受限制的通信信道进行传输，提供针对这种数据的压缩方法就很有用，pcl提供了点云的压缩功能。它可以压缩所有类型的点云。

/*
 * @Description: 点云压缩：https://www.cnblogs.com/li-yao7758258/p/6436117.html
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-13 10:10:33
 * @LastEditTime: 2020-10-27 16:02:18
 * @FilePath: /pcl-learning/03octree/1point_cloud_compression/point_cloud_compression.cpp
 */


#include <pcl/point_cloud.h>                // 点云类型
#include <pcl/point_types.h>                //点数据类型
#include <pcl/io/openni_grabber.h>          //点云获取接口类
#include <pcl/visualization/cloud_viewer.h> //点云可视化类

#include <pcl/compression/octree_pointcloud_compression.h> //点云压缩类

#include <stdio.h>
#include <sstream>
#include <stdlib.h>

#ifdef WIN32
#define sleep(x) Sleep((x)*1000)
#endif

class SimpleOpenNIViewer
{
public:
    SimpleOpenNIViewer() : viewer(" Point Cloud Compression Example")
    {
    }
    /************************************************************************************************
  在OpenNIGrabber采集循环执行的回调函数cloud_cb_中，首先把获取的点云压缩到stringstream缓冲区，下一步就是解压缩，
  它对压缩了的二进制数据进行解码，存储在新的点云中解码了点云被发送到点云可视化对象中进行实时可视化
*************************************************************************************************/

    void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
    {
        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);

            // 可视化解压缩的点云
            viewer.showCloud(cloudOut);
        }
    }
    /**************************************************************************************************************
 在函数中创建PointCloudCompression类的对象来编码和解码，这些对象把压缩配置文件作为配置压缩算法的参数
 所提供的压缩配置文件为OpenNI兼容设备采集到的点云预先确定的通用参数集，本例中使用MED_RES_ONLINE_COMPRESSION_WITH_COLOR
 配置参数集，用于快速在线的压缩，压缩配置方法可以在文件/io/include/pcl/compression/compression_profiles.h中找到，
  在PointCloudCompression构造函数中使用MANUAL——CONFIGURATION属性就可以手动的配置压缩算法的全部参数
******************************************************************************************************************/
    void run()
    {

        bool showStatistics = true; //设置在标准设备上输出打印出压缩结果信息

        // 压缩选项详情在: /io/include/pcl/compression/compression_profiles.h
        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>();
        /***********************************************************************************************************
    下面的代码为OpenNI兼容设备实例化一个新的采样器，并且启动循环回调接口，每从设备获取一帧数据就回调函数一次，，这里的
    回调函数就是实现数据压缩和可视化解压缩结果。
   ************************************************************************************************************/
        //创建从OpenNI获取点云的抓取对象
        pcl::Grabber *interface = new pcl::OpenNIGrabber();

        // 建立回调函数
        boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &)> f = boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);

        //建立回调函数和回调信息的绑定
        boost::signals2::connection c = interface->registerCallback(f);

        // 开始接受点云的数据流
        interface->start();

        while (!viewer.wasStopped())
        {
            sleep(1);
        }

        interface->stop();

        // 删除压缩与解压缩的实例
        delete (PointCloudEncoder);
        delete (PointCloudDecoder);
    }

    pcl::visualization::CloudViewer viewer;

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

int main(int argc, char **argv)
{
    SimpleOpenNIViewer v; //创建一个新的SimpleOpenNIViewer  实例并调用他的run方法
    v.run();

    return (0);
}

2、使用八进制进行空间分区和搜索操作

octree是一种管理稀疏3D数据的树状结构，利用octree实现多个无序点云之间的空间变化检测，这些点云可能在尺寸。分辨率 密度，和点顺序等方面有所差异，通过递归的比较octree的树结构，可以鉴定出由octree产生的体素组成的区别所代表的空间变化，还要学习关于octree的“双缓冲”技术，以便实时的探测多个点云之间的空间组成的差异。

/*
 * @Description: 点云搜索  https://pcl.readthedocs.io/projects/tutorials/en/latest/octree.html#octree-search
 * @Author: HCQ
 * @Company(School): UCAS
 * @Email: 1756260160@qq.com
 * @Date: 2020-10-13 10:54:55
 * @LastEditTime: 2020-10-27 16:01:26
 * @FilePath: /pcl-learning/03octree/2Spatial Partitioning and Search Operations with Octrees/1octree_search/octree_search.cpp
 */
#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)); //用系统时间初始化随机种子与 srand (time (NULL))的区别

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

    // 创建点云数据
    cloud->width = 1000;
    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);
    }
    /****************************************************************************
   创建一个octree实例，用设置分辨率进行初始化，该octree用它的页节点存放点索引向量，
   分辨率参数描述最低一级octree的最小体素的尺寸，因此octree的深度是分辨率和点云空间维度
   的函数，如果知道点云的边界框，应该用defineBoundingbox方法把它分配给octree然后通过
   点云指针把所有点增加到ctree中。
   *****************************************************************************/
    float resolution = 128.0f;

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

    octree.setInputCloud(cloud);      //设置输入点云    这两句是最关键的建立PointCloud和octree之间的联系
    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);

    /*************************************************************************************
  一旦PointCloud和octree联系一起，就能进行搜索操作，这里使用的是“体素近邻搜索”，把查询点所在体素中
   其他点的索引作为查询结果返回，结果以点索引向量的形式保存，因此搜索点和搜索结果之间的距离取决于octree的分辨率参数
*****************************************************************************************/

    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 被设置为10 ，K近邻搜索  方法把搜索结果写到两个分开的向量，第一个pointIdxNKNSearch包含搜索结果
   （结果点的索引的向量）  第二个向量pointNKNSquaredDistance存储搜索点与近邻之间的距离的平方。
*************************************************************************************/

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

实现效果

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

/*
 * @Description:：无序点云数据集的空间变化检测： https://www.cnblogs.com/li-yao7758258/p/6441595.html
 * @Author: HCQ
 * @Company(School): UCAS
 * @Date: 2020-10-13 10:53:45
 * @LastEditors: HCQ
 * @LastEditTime: 2020-10-13 11:21:16
 */
#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点云填充点数据
    cloudA->width = 128;                                   //设置点云cloudA的点数
    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(); //从输入点云构建八叉树
                                      /***********************************************************************************
    点云cloudA是参考点云用其建立的八叉树对象描述它的空间分布，octreePointCloudChangeDetector
    类继承自Octree2BufBae类，Octree2BufBae类允许同时在内存中保存和管理两个octree，另外它应用了内存池
    该机制能够重新利用已经分配了的节点对象，因此减少了在生成点云八叉树对象时昂贵的内存分配和释放操作
    通过访问 octree.switchBuffers ()重置八叉树 octree对象的缓冲区，但把之前的octree数据仍然保留在内存中
   ************************************************************************************/
    // 交换八叉树的缓冲，但是CloudA对应的八叉树结构仍然在内存中
    octree.switchBuffers();
    //cloudB点云用于建立新的八叉树结构，与前一个cloudA对应的八叉树共享octree对象，同时在内存中驻留
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>); //实例化点云对象cloudB

    // 为cloudB创建点云
    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);
    }

    // 添加cloudB到八叉树中
    octree.setInputCloud(cloudB);
    octree.addPointsFromInputCloud();

    /**************************************************************************************************************
  为了检索获取存在于couodB的点集R，此R并没有cloudA中的元素，可以调用getPointIndicesFromNewVoxels方法，通过探测两个八叉树之间
  体素的不同，它返回cloudB 中新加点的索引的向量，通过索引向量可以获取R点集  很明显这样就探测了cloudB相对于cloudA变化的点集，但是只能探测
  到在cloudA上增加的点集，二不能探测减少的
****************************************************************************************************************/

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

    // 获取前一cloudA对应八叉树在cloudB对应在八叉树中没有的点集
    octree.getPointIndicesFromNewVoxels(newPointIdxVector);

    // 打印点集
    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;
}

输出