PCL中的八叉树

 建立空间索引在点云数据处理中已被广泛应用，常见的空间索引一般是自定向下逐级划分空间的各种空间索引结构，比较有代表行的包括BSP树、KD树、KDB树、R树、R+树、CELL树、四叉树和八叉树等索引结构，而在3D点云数据中应用较广泛的是KD树和八叉树。PCL对八叉树的数据结构建立和索引方法进行了实现，以方便再次基础上进行其它点云数据处理操作。

一、八叉树

 1. 什么是八叉树

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

 2. 计算原理

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

 3. 数据结构

 八叉树的节点分为三类：
  灰色节点：它对应的立方体部分被目标占据（非叶节点）
  白色节点：它对应的立方体没有被目标占据（叶节点）
  黑色节点：它对应的立方体完全被目标占据（叶节点）
  对于非叶节点，数据结构以八元数法进行分区，分解为更小的子区块，每个区块有节点容量，当节点达到最大容量时节点停止分裂。

 3. 存储结构

  八叉树的存储结构一般分为三种：规则八叉树、线性八叉树、和一对八式。
规则八叉树：有介个字段，八个子叶节点，一个表征该节点是灰/白/黑的节点，这种表示方式处于贮存空间考量，一般不使用。
线性八叉树：将八叉树转化为线性表，采用内存紧凑的方式进行表示。

一对八式：使用指针表示，好处是顺序可以随机。

二、PCL中的八叉树

 1. 简介

 Octree结构是通过对三维空间的几何实体进行体元剖分，每个体元具有相同的时间和空间复杂度，通过循环递归的划分方法对大小为 $2n\ast 2n\ast 2n$的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。在八叉树结构中如果被划分的体元具有相同的属性，则该体元构成一个叶节点，否则继续对该体院剖分成8个子立方体，依次递归剖分，对于 $2n\ast 2n\ast 2n$大小的空间对象，最多剖分n次。
 PCL中的Octree模块利用十几个类实现了利用Octree数据结构对点云的高效管理和检索，以及相应的一些空间处理算法，例如压缩、空间变化检测等；Octree依赖于pcl_common模块。

 2. 事例代码

//
// Created by aiden on 21-11-24.
//

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

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

void octree_serch();
void octree_change_detection();
void print_point(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<int> &idxs, std::vector<float> &dis);

int main(int argc, char **argv){
//    octree_serch();
    octree_change_detection();
    return 0;
}

void print_point(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, std::vector<int> &idxs, std::vector<float> &dis){
    for (size_t i = 0; i < dis.size(); ++i){
        std::cout << "  point " << i + 1 << ", x:" << cloud->points[idxs[i] ].x
                  << ", y:" << cloud->points[idxs[i] ].y
                  << ", z:" << cloud->points[idxs[i] ].z
                  << ", distance:" << dis.at(i)
                  << std::endl;
    }
}

void octree_serch(){
    srand((unsigned int)time(nullptr));
    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.f * rand() / (RAND_MAX + 1.f);
        cloud->points[i].y = 1024.f * rand() / (RAND_MAX + 1.f);
        cloud->points[i].z = 1024.f * rand() / (RAND_MAX + 1.f);
    }
    float resolution = 128.f;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();
    pcl::PointXYZ search_point;
    search_point.x = 1024.f * rand() / (RAND_MAX + 1.f);
    search_point.y = 1024.f * rand() / (RAND_MAX + 1.f);
    search_point.z = 1024.f * rand() / (RAND_MAX + 1.f);
    std::cout << "Nearest neighbor search at ( x:" << search_point.x << ", y:"
              << search_point.y << ", z:" << search_point.z << ")" << std::endl;

    std::cout << "voxelSearch: " << std::endl;
    std::vector<int> point_idx_vec;
    if (octree.voxelSearch(search_point, point_idx_vec)){
    //if (octree.voxelSearch(cloud->points[0], point_idx_vec)){
        std::vector<float> v = {0.f};
        print_point(cloud, point_idx_vec, v);
    }

    std::cout << "nearestKSearch: " << std::endl;
    int k = 10;
    std::vector<int> point_idx_nkn_search;
    std::vector<float> point_idx_nkn_distance;
    if (0 < octree.nearestKSearch(search_point, k, point_idx_nkn_search, point_idx_nkn_distance)){
        print_point(cloud, point_idx_nkn_search, point_idx_nkn_distance);
    }

    std::cout << "radiusSearch: " << std::endl;
    std::vector<int> point_idx_radius_search;
    std::vector<float> point_idx_radius_distance;
    float radius = 256.f * rand() / (RAND_MAX + 1.f);
    if (0 < octree.radiusSearch(search_point, radius, point_idx_radius_search, point_idx_radius_distance)){
        print_point(cloud, point_idx_radius_search, point_idx_radius_distance);
    }
}

void octree_change_detection(){
    srand((unsigned int)time(nullptr));

    float resolution = 32.f;
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    cloud->width = 128;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);
    for (size_t i = 0; i < cloud->points.size(); ++i){
        cloud->points[i].x = 64.f * rand()/(RAND_MAX + 1.f);
        cloud->points[i].y = 64.f * rand()/(RAND_MAX + 1.f);
        cloud->points[i].z = 64.f * rand()/(RAND_MAX + 1.f);
    }

    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();

    octree.switchBuffers();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_2->width = 128;
    cloud_2->height = 1;
    cloud_2->points.resize(cloud_2->width * cloud_2->height);
    for (size_t i = 0; i < cloud_2->points.size(); ++i){
        cloud_2->points[i].x = 64.f * rand()/(RAND_MAX + 1.f);
        cloud_2->points[i].y = 64.f * rand()/(RAND_MAX + 1.f);
        cloud_2->points[i].z = 64.f * rand()/(RAND_MAX + 1.f);
    }

    octree.setInputCloud(cloud_2);
    octree.addPointsFromInputCloud();
    std::vector<int> new_point_idx_v;
    octree.getPointIndicesFromNewVoxels(new_point_idx_v);

    std::cout << "Output from getPointIndicesFromNewVoxels: " << std::endl;
    std::vector<float> tmp(new_point_idx_v.size());
    print_point(cloud_2, new_point_idx_v, tmp);
}