octree八叉树数据结构原理与实现

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

1、八叉树的描述

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

八叉树结构示意图

　　八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。一般中心点作为节点的分叉中心。八叉树若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。

　　八叉树叶子节点代表了分辨率最高的情况。例如分辨率设成0.01m，那么每个叶子就是一个1cm见方的小方块。

　　那么，这要用来做什么？想象一个立方体，我们最少可以切成多少个相同等分的小立方体？答案就是8个。再想象我们有一个房间，房间里某个角落藏着一枚金币，我们想很快的把金币找出来，聪明的你会怎么做？我们可以把房间当成一个立方体，先切成八个小立方体，然后排除掉没有放任何东西的小立方体，再把有可能藏金币的小立方体继续切八等份….如此下去，平均在Log8(房间内的所有物品数)的时间内就可找到金币。因此，Octree就是用在3D空间中的场景管理，可以很快地知道物体在3D场景中的位置，或侦测与其它物体是否有碰撞以及是否在可视范围内。

2、实现Octree的原理

(1). 设定最大递归深度

(2). 找出场景的最大尺寸，并以此尺寸建立第一个立方体

(3). 依序将单位元元素丢入能被包含且没有子节点的立方体

(4). 若没有达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体

(5). 若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。

(6). 重复3，直到达到最大递归深度。

3、Octree的存储结构

本例中Octree的存贮结构用一个有（若干+八）个字段的记录来表示树中的每个结点。其中若干字段用来描述该结点的特性(本例中的特性为：节点的值和节点坐标)，其余的八个字段用来作为存放指向其八个子结点的指针。此外，还有线性存储和1托8式存储。

4、BSP Tree和Octree对比

a) BSP Tree将场景分割为1个面，而Octree分割为3个面。

b) BSP Tree每个节点最多有2个子结点，而Octree最多有8个子结点

5、八叉树和k-d树比较

因此BSP Tree可以用在不论几唯的场景中，而Octree则常用于三维场景

八叉树算法的算法实现简单，但大数据量点云数据下，其使用比较困难的是最小粒度（叶节点）的确定，粒度较大时，有的节点数据量可能仍比较大，后续查询效率仍比较低，反之，粒度较小，八叉树的深度增加，需要的内存空间也比较大（每个非叶子节点需要八个指针），效率也降低。而等分的划分依据，使得在数据重心有偏斜的情况下，受划分深度限制，其效率不是太高。

k-d在邻域查找上比较有优势，但在大数据量的情况下，若划分粒度较小时，建树的开销也较大，但比八叉树灵活些。在小数据量的情况下，其搜索效率比较高，但在数据量增大的情况下，其效率会有一定的下降，一般是线性上升的规律。

也有将八叉树和k-d树结合起来的应用，应用八叉树进行大粒度的划分和查找，而后使用k-d树进行细分，效率会有一定的提升，但其搜索效率变化也与数据量的变化有一个线性关系。

6、八叉树的C++实现 

#include <iostream>  
 
using namespace std;  
//定义八叉树节点类  
template<class T>  
struct OctreeNode  
{  
    T data; //节点数据  
    T xmin,xmax; //节点坐标，即六面体个顶点的坐标  
    T ymin,ymax;  
    T zmin,zmax;  
    OctreeNode <T>*top_left_front,*top_left_back; //该节点的个子结点  
    OctreeNode <T>*top_right_front,*top_right_back;  
    OctreeNode <T>*bottom_left_front,*bottom_left_back;  
    OctreeNode <T>*bottom_right_front,*bottom_right_back;  
    OctreeNode //节点类  
        (T nodeValue = T(),  
        T xminValue = T(),T xmaxValue = T(),  
        T yminValue = T(),T ymaxValue = T(),  
        T zminValue = T(),T zmaxValue = T(),  
        OctreeNode<T>*top_left_front_Node = NULL,  
        OctreeNode<T>*top_left_back_Node = NULL,  
        OctreeNode<T>*top_right_front_Node = NULL,  
        OctreeNode<T>*top_right_back_Node = NULL,  
        OctreeNode<T>*bottom_left_front_Node = NULL,  
        OctreeNode<T>*bottom_left_back_Node = NULL,  
        OctreeNode<T>*bottom_right_front_Node = NULL,  
        OctreeNode<T>*bottom_right_back_Node = NULL )  
        :data(nodeValue),  
        xmin(xminValue),xmax(xmaxValue),  
        ymin(yminValue),ymax(ymaxValue),  
        zmin(zminValue),zmax(zmaxValue),  
        top_left_front(top_left_front_Node),  
        top_left_back(top_left_back_Node),  
        top_right_front(top_right_front_Node),  
        top_right_back(top_right_back_Node),  
        bottom_left_front(bottom_left_front_Node),  
        bottom_left_back(bottom_left_back_Node),  
        bottom_right_front(bottom_right_front_Node),  
        bottom_right_back(bottom_right_back_Node){}  
};  
//创建八叉树  
template <class T>  
void createOctree(OctreeNode<T> * &root,int maxdepth,double xmin,double xmax,double ymin,double ymax,double zmin,double zmax)  
{  
    //cout<<"处理中，请稍候……"<<endl;  
    maxdepth=maxdepth-1; //每递归一次就将最大递归深度-1  
    if(maxdepth>=0)  
    {  
        root=new OctreeNode<T>();  
        cout<<"请输入节点值:";  
        //root->data =9;//为节点赋值，可以存储节点信息，如物体可见性。由于是简单实现八叉树功能，简单赋值为9。  
        cin>>root->data;  //为节点赋值  
        root->xmin=xmin; //为节点坐标赋值  
        root->xmax=xmax;  
        root->ymin=ymin;  
        root->ymax=ymax;  
        root->zmin=zmin;  
        root->zmax=zmax;  
        double xm=(xmax-xmin)/2;//计算节点个维度上的半边长  
        double ym=(ymax-ymin)/2;  
        double zm=(ymax-ymin)/2;  
        //递归创建子树，根据每一个节点所处（是几号节点）的位置决定其子结点的坐标。  
        createOctree(root->top_left_front,maxdepth,xmin,xmax-xm,ymax-ym,ymax,zmax-zm,zmax);  
        createOctree(root->top_left_back,maxdepth,xmin,xmax-xm,ymin,ymax-ym,zmax-zm,zmax);  
        createOctree(root->top_right_front,maxdepth,xmax-xm,xmax,ymax-ym,ymax,zmax-zm,zmax);  
        createOctree(root->top_right_back,maxdepth,xmax-xm,xmax,ymin,ymax-ym,zmax-zm,zmax);  
        createOctree(root->bottom_left_front,maxdepth,xmin,xmax-xm,ymax-ym,ymax,zmin,zmax-zm);  
        createOctree(root->bottom_left_back,maxdepth,xmin,xmax-xm,ymin,ymax-ym,zmin,zmax-zm);  
        createOctree(root->bottom_right_front,maxdepth,xmax-xm,xmax,ymax-ym,ymax,zmin,zmax-zm);  
        createOctree(root->bottom_right_back,maxdepth,xmax-xm,xmax,ymin,ymax-ym,zmin,zmax-zm);  
    }  
}  
int i=1;  
//先序遍历八叉树  
template <class T>  
void preOrder( OctreeNode<T> * & p)  
{  
    if(p)  
    {  
        cout<<i<<".当前节点的值为："<<p->data<<"
坐标为：";  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        i+=1;  
        cout<<endl;  
        preOrder(p->top_left_front);  
        preOrder(p->top_left_back);  
        preOrder(p->top_right_front);  
        preOrder(p->top_right_back);  
        preOrder(p->bottom_left_front);  
        preOrder(p->bottom_left_back);  
        preOrder(p->bottom_right_front);  
        preOrder(p->bottom_right_back);  
        cout<<endl;  
    }  
}  
//求八叉树的深度  
template<class T>  
int depth(OctreeNode<T> *& p)  
{  
    if(p == NULL)  
        return -1;  
    int h =depth(p->top_left_front);  
    return h+1;  
}  
//计算单位长度，为查找点做准备  
int cal(int num)  
{  
    int result=1;  
    if(1==num)  
        result=1;  
    else  
    {  
        for(int i=1;i<num;i++)  
            result=2*result;  
    }  
    return result;  
}  
//查找点  
int maxdepth=0;  
int times=0;  
static double xmin=0,xmax=0,ymin=0,ymax=0,zmin=0,zmax=0;  
int tmaxdepth=0;  
double txm=1,tym=1,tzm=1;  
template<class T>  
void find(OctreeNode<T> *& p,double x,double y,double z)  
{  
    double xm=(p->xmax-p->xmin)/2;  
    double ym=(p->ymax-p->ymin)/2;  
    double zm=(p->ymax-p->ymin)/2;  
    times++;  
    if(x>xmax || x<xmin|| y>ymax || y<ymin || z>zmax || z<zmin)  
    {  
        cout<<"该点不在场景中！"<<endl;  
        return;  
    }  
    if(x<=p->xmin+txm&& x>=p->xmax-txm && y<=p->ymin+tym &&y>=p->ymax-tym && z<=p->zmin+tzm &&z>=p->zmax-tzm )  
    {  
        cout<<endl<<"找到该点！"<<"该点位于"<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<"节点内！"<<endl;  
        cout<<"共经过"<<times<<"次递归！"<<endl;  
    }  
    else if(x<(p->xmax-xm) && y<(p->ymax-ym) &&z<(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->bottom_left_back,x,y,z);  
    }  
    else if(x<(p->xmax-xm) && y<(p->ymax-ym) &&z>(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->top_left_back,x,y,z);  
    }  
    else if(x>(p->xmax-xm) && y<(p->ymax-ym) &&z<(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->bottom_right_back,x,y,z);  
    }  
    else if(x>(p->xmax-xm) && y<(p->ymax-ym) &&z>(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->top_right_back,x,y,z);  
    }  
    else if(x<(p->xmax-xm) && y>(p->ymax-ym) &&z<(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->bottom_left_front,x,y,z);  
    }  
    else if(x<(p->xmax-xm) && y>(p->ymax-ym) &&z>(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->top_left_front,x,y,z);  
    }  
    else if(x>(p->xmax-xm) && y>(p->ymax-ym) &&z<(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->bottom_right_front,x,y,z);  
    }  
    else if(x>(p->xmax-xm) && y>(p->ymax-ym) &&z>(p->zmax-zm))  
    {  
        cout<<"当前经过节点坐标："<<endl;  
        cout<<"xmin: "<<p->xmin<<" xmax: "<<p->xmax;  
        cout<<"ymin: "<<p->ymin<<" ymax: "<<p->ymax;  
        cout<<"zmin: "<<p->zmin<<" zmax: "<<p->zmax;  
        cout<<endl;  
        find(p->top_right_front,x,y,z);  
    }  
}  
//main函数  
int main ()  
{  
    OctreeNode<double> *rootNode = NULL;  
    int choiced = 0;  
    while(true)  
    {  
        system("cls");  
        cout<<"请选择操作：
";  
        cout<<"1.创建八叉树 2.先序遍历八叉树
";  
        cout<<"3.查看树深度 4.查找节点   
";  
        cout<<"0.退出

";  
        cin>>choiced;  
        if(choiced == 0)  
            return 0;  
        else if(choiced == 1)  
        {  
            system("cls");  
            cout<<"请输入最大递归深度："<<endl;  
            cin>>maxdepth;  
            cout<<"请输入外包盒坐标，顺序如下：xmin,xmax,ymin,ymax,zmin,zmax"<<endl;  
            cin>>xmin>>xmax>>ymin>>ymax>>zmin>>zmax;  
            if(maxdepth>=0|| xmax>xmin || ymax>ymin || zmax>zmin || xmin>0 || ymin>0||zmin>0)  
            {  
                tmaxdepth=cal(maxdepth);  
                txm=(xmax-xmin)/tmaxdepth;  
                tym=(ymax-ymin)/tmaxdepth;  
                tzm=(zmax-zmin)/tmaxdepth;  
                createOctree(rootNode,maxdepth,xmin,xmax,ymin,ymax,zmin,zmax);  
            }  
            else  
            {  
                cout<<"输入错误！";  
                return 0;  
            }  
        }  
        else if(choiced == 2)  
        {  
            system("cls");  
            cout<<"先序遍历八叉树结果：/n";  
            i=1;  
            preOrder(rootNode);  
            cout<<endl;  
            system("pause");  
        }  
        else if(choiced == 3)  
        {  
            system("cls");  
            int dep =depth(rootNode);  
            cout<<"此八叉树的深度为"<<dep+1<<endl;  
            system("pause");  
        }  
        else if(choiced == 4)  
        {  
            system("cls");  
            cout<<"请输入您希望查找的点的坐标，顺序如下：x,y,z
";  
            double x,y,z;  
            cin>>x>>y>>z;  
            times=0;  
            cout<<endl<<"开始搜寻该点……"<<endl;  
            find(rootNode,x,y,z);  
            system("pause");  
        }  
        else  
        {  
            system("cls");  
            cout<<"

错误选择！
";  
            system("pause");  
        }  
    }  
}

 PCL中octree应用

1、八叉树在PCL中的压缩例子

// 用于显示数据
pcl::visualization::CloudViewer viewer;
// 用于压缩、解压点云数据
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;
// 用于接收到设备数据时的回调响应
void  cloud_callback(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)；

//  interface->start ();之后，设备获取一帧数据，就回调一次次函数，不断刷新压缩后的点云
void  cloud_callback(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);
        }
    }

int main()
{
   // 压缩选项详情在: /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, true);
    PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();
     //创建从OpenNI获取点云的抓取对象
    pcl::Grabber* interface = new pcl::OpenNIGrabber ();
    // 建立回调函数
    boost::function<void
    (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&SimpleOpenNIViewer::cloud_callback, this, _1);
    //建立回调函数和回调信息的绑定
    boost::signals2::connection c = interface->registerCallback (f);
    // 开始接受点云的数据流
    interface->start ();
    while (!viewer.wasStopped ())
    {
      sleep (1);
    }

    interface->stop ();
    // 删除压缩与解压缩的实例
    delete (PointCloudEncoder);
    delete (PointCloudDecoder);
return 0;
}

2、体素内邻近搜索、K邻近搜索、半径内邻近搜索

void main ()
{
    pcl::PintCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);
    cloud->width=1000;
    cloud->height=1;
    cloud->pionts.resize(cloud->width*cloud->height);
    for(int 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);
    }
    
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>octree(128.0f)//初始化octree，分辨率为128.0
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();// 根据点云搭建octree
    vector<int>piontIndexVec;// 存储体素邻近的点的索引
    //体素内邻近搜索
    if(octree.voxelSearch(searchPoint,piontIndexVec))//searchPoint为搜索的关键点
    {
         for(int i=0;i<piontIndexVec.size();i++)
         {
         // 每个点的操作
         }
    }
    //K邻近搜索
    int k=10;
    vector<int>knnIndex;
    vector<float>knnDistance;
    if(octree.nearestKSearch(searchPoint,k,knnIndex,knnDistance)>0)
    {
    //  相关操作
    }
    //半径内邻近搜索
    float radius=256.0f;
    vector<int>radiusIndex;
    vector<float>radiusDistance;
    if(octree.radiusSearch(searchPoint,radius,radiusIndex,radiusDistance)>0)
    {
    //  相关操作
    }
}

3、无序点云数据集的空间变化检测

void main ()
{
    pcl::PintCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);
    cloud->width=1000;
    cloud->height=1;
    cloud->pionts.resize(cloud->width*cloud->height);
    for(int 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);
    }
    //OctreePointCloudChangeDetector 这个对象允许同时在内存中保管两个八叉树，内部使用内存池管理
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree(128.0f)//初始化octree，分辨率为128.0
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();// 根据点云搭建octree
    octree.switchBuffers();// 交换缓冲区，使用新的八叉树结构
    pcl::PintCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<PointXYZ>);
    cloudB->width=1000;
    cloudB->height=1;
    cloudB->pionts.resize(cloudB->width*cloudB->height);
    for(int i=0;i<cloudB->points.size();i++)
    {
        cloudB->points[i].x=1024.0f*rand()/(RAND_MAX+1.0f);
        cloudB->points[i].y=1024.0f*rand()/(RAND_MAX+1.0f);
        cloudB->points[i].z=1024.0f*rand()/(RAND_MAX+1.0f);
    }
    octree.setInputCloud(cloudB);
    octree.addPointsFromInputCloud();// 根据点云cloudB搭建octree
    //getPointIndicesFromNewVoxels()探测两个点云中体素间的不同，只能检测增加的点，不能检测减少的点
    vector<int>newPointIndex;
    octree.getPointIndicesFromNewVoxels(newPointIndex);//cloudB对比于原始cloud，添加了哪些点，这些点是在cloudB中
    for(int i=0;i<newPointIndex.size();i++)
    {
    // ~~~相关操作
    }
}