KD-Tree与Octo-Tree

下面记录一下SLAM中常用的两种存储点云的数据结构：KD-Tree和Octo-Tree。

KD-Tree

KD-Tree的原理详解可以参考以下文章：

欧式聚类（KD-Tree）详解，保姆级教程 - 知乎

Octo-Tree 

Octo-Tree的原理详解可以参考以下文章：

https://www.cnblogs.com/gaoxiang12/p/5041142.html

数据结构专题(四) | Dynamic Octree: 卡耐基梅隆大学Super Odometry方案动态八叉树结构 - 知乎

Octo-map

以下面代码为例，补充一下Octo-map的一些知识：

#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//octomap
#include <octomap/octomap.h>
using namespace std;

int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2octomap <input_file> <output_file>"<<endl;
        return -1;
    }

    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    //声明octomap变量
    cout<<"copy data into octomap..."<<endl;
    // 创建八叉树对象，参数为分辨率，这里设成了0.05
    octomap::OcTree tree( 0.05 );

    for (auto p:cloud.points)
    {
        // 将点云里的点插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }

    // 更新octomap
    tree.updateInnerOccupancy();
    // 存储octomap
    tree.writeBinary( output_file );
    cout<<"done."<<endl;

    return 0;
}

八叉树地图的构建过程是从根节点到叶子节点的构建过程，根节点的空间通常可以被视为一个大方块，它代表了整个地图的空间范围。这个大方块的中心通常被定义为根节点的原点，而大方块的边界则定义了地图空间的范围。

在八叉树地图中，根节点的坐标通常是相对于地图的原点来定义的。根节点的坐标不一定从坐标原点（0,0,0）开始，而是相对于地图的整体坐标系来确定的。

octomap::OcTree tree( 0.05 );

上面代码创建了一个八叉树地图，地图的分辨率是0.05，即一个叶子节点的体素大小为0.05m³。该八叉树地图创建的时候没有指定八叉树地图的大小（即根节点的大小）。

    for (auto p:cloud.points)
    {
        // 将点云里的点插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }

八叉树地图的根节点的空间大小通常是根据需要动态调整的，根据插入的数据点自动扩展或收缩。因此，在初始时并没有明确定义的根节点空间大小，地图的大小会根据插入的数据自适应调整。

当插入的地图点超出了八叉树地图的当前尺寸范围时，八叉树地图会进行自动扩展以容纳新的数据点。当需要扩展空间时，八叉树地图会检查当前根节点是否可以容纳新的数据。如果根节点的范围不足以容纳新的数据，则会创建一个新的根节点，并将现有的根节点作为其子节点之一。新的根节点会扩展地图的边界，以适应新的数据范围，并确保地图的连续性和一致性。