（十）OcTree教程二--OcTree在PCL中的应用-空间划分和近邻搜索

OcTree在PCL中的应用

八叉树作为三维空间中点的重要数据结构在PCL中有很多应用，下面就分别从空间划分、近邻搜索、点云压缩和空间变化检测几个方面来介绍。

空间划分和近邻搜索

八叉树是将空间平均得分为8个部分，每个节点就是一个部分，我们假设三维空间是一个正立方体，每个节点就是一个小立方体。划分到最后，一个叶子结点代表一个最小的空间范围，我们称之为体素。八叉树的建立过程也就是空间划分的过程。
基于八叉树的近邻搜索可以分为三种：
- 范围搜索
- k最近邻搜索
- 体素内搜索
前两种搜索方式已经在k-d树种介绍过了，这里就不再赘述了，而体素内搜索是octree特有的。它是在给定查询点后，将其所在体素内的所有其他点作为搜索目标的搜索方式。

体素内搜索

头文件

#include <pcl/octree/octree.h>

步骤

1. 初始化ocTree树

// 确定分辨率，即最小体素的边长
float resolution = 1.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);
// 载入点云
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();

载入点云后，八叉树会根据分辨率和点云的空间维度得到深度，并得到一个Bounding Box，即一个长方体盒子，它能将整个点云装在它的内部，也就相当于划分出来了点云的空间范围。如果点云的空间范围我们是知道的，那么可以通过defineBoundingBox函数可以直接设定

octree.defineBoundingBox(0, 0, 0, 1, 1, 1);     // 参数分别为min_x,min_y,min_z,max_x,max_y,max_z

1. 输入查询点进行搜索

octree.voxelSearch(searchPoint, pointIdxVec)

关键函数：

bool
        voxelSearch (const PointT& point, std::vector<int>& point_idx_data);

• 输入输出参数：
  
  • point[in]：查询点，必须保证在一个最小体素内
  • point_idx_data[out]：搜索到的相同体素内的点集，point_ind_data为它们在点云中的下标
  • 返回值(bool)：若返回true，证明这个最小体素存在，否则不存在。

示例

我们载入一个1mx1mx1m的立方体点云，立方体内点均匀分布，一共有100x100x100个点，最小距离1cm。
选择查询点为立方体中心点，八叉树分辨率为0.3m，得到的结果如下：

红色的点为搜索到的点

我们打印出了一些信息

程序

/*
 * 功能：octree体素内搜索介绍
 */
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/octree/octree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>

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

int
main(int argc, char** argv)
{
    if (argc<5)
    {
        std::cout << "Usage: " << "<Resolution> " << "<SearchPoint.x>" << "<SearchPoint.y>" << "<SearchPoint.z>" << std::endl;
        return -2;
    }
    else
        cout << "Resolution : " << argv[1] << endl;
    //// 构造一个1m X 1m X 1m立方体点云，每个点之间间隔1cm
    //pcl::PointCloud<pcl::PointXYZ>::Ptr pCubeCloud(new pcl::PointCloud<pcl::PointXYZ>);
    //for (int x = 0; x < 100; x++)
    //{
    //  for (int y = 0; y < 100; y++)
    //  {
    //      for (int z = 0; z < 100; z++)
    //      {
    //          // 注意这里是pCubeCloud->push_back，而不是pCubeCloud->points->push_back
    //          pCubeCloud->push_back(pcl::PointXYZ((float)x/100.0f, (float)y/100.0f, (float)z/100.0f));
    //      }
    //  }
    //}
    //// save
    //pcl::io::savePCDFile("cube.pcd", *pCubeCloud, true);

    // load
    pcl::PointCloud<pcl::PointXYZ>::Ptr pCube(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile("cube.pcd", *pCube) == -1)
        return -1;

    // 初始化八叉树
    float resolution = atof(argv[1]);                                               // 设置体素大小????
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
    //octree.defineBoundingBox(0, 0, 0, 1, 1, 1);
    // 将点云载入八叉树
    octree.setInputCloud(pCube);
    octree.addPointsFromInputCloud();

    // 初始化查询点
    pcl::PointXYZ searchPoint(atof(argv[2]), atof(argv[3]), atof(argv[4]));                         // 查询点

    // 体素内搜索
    std::vector<int> pointIdxVec;
    pcl::PointCloud<pcl::RGB>::Ptr pPointsRGB(new pcl::PointCloud<pcl::RGB>);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pCloudShow(new pcl::PointCloud<pcl::PointXYZRGBA>);


    if (octree.voxelSearch(searchPoint, pointIdxVec))
    {
        std::cout << "Neighbors within voxel search at (" << searchPoint.x
            << " " << searchPoint.y
            << " " << searchPoint.z << ")"
            << std::endl;

        std::cout << "Searched Points Number : " << pointIdxVec.size() << endl;

        std::cout << "Leaf Count : " << octree.getLeafCount() << std::endl;             // 叶子数
        std::cout << "Tree Depth : " << octree.getTreeDepth() << std::endl;             // 八叉树深度
        std::cout << "Branch Count : " << octree.getBranchCount() << std::endl;         // 非叶子结点数
        std::cout << "Voxel Diameter : " << octree.getVoxelSquaredDiameter() << std::endl;  // ???Voxel Side Length*3
        std::cout << "Voxel Side Length : " << octree.getVoxelSquaredSideLen() << std::endl;// 分辨率的平方
        double minx, miny, minz, maxx, maxy, maxz;
        octree.getBoundingBox(minx, miny, minz, maxx, maxy, maxz);
        std::cout << "BoundingBox: " <<  "(" << minx << " - " << maxx << ")" << " , " << "(" << miny << " - " << maxy << ")" << " , " << "(" << minz << " - " << maxz << ")" << std::endl;                                // 整个八叉树的范围


    }

    // 给搜索到的点上色，原始点云中的点全为蓝色，搜索到的上为红色
    pPointsRGB->width = pCube->size();
    pPointsRGB->height = 1;
    pPointsRGB->resize(pPointsRGB->width*pPointsRGB->height);

    pCloudShow->width = pCube->size();
    pCloudShow->height = 1;
    pCloudShow->resize(pPointsRGB->width*pPointsRGB->height);

    for (size_t i = 0; i < pPointsRGB->size(); i++)
    {
        pPointsRGB->points[i].b = 255;
    }

    for (size_t i = 0; i < pointIdxVec.size(); ++i)
    {
        pPointsRGB->points[pointIdxVec[i]].b = 0;
        pPointsRGB->points[pointIdxVec[i]].r = 255;
    }

    // 合并不同字段
    pcl::concatenateFields(*pCube, *pPointsRGB, *pCloudShow);

    // 可视窗口初始化
    pcl::visualization::PCLVisualizer viewer;
    viewer.setCameraFieldOfView(0.785398);      // fov 大概45度
    viewer.setBackgroundColor(0.5, 0.5, 0.5);   // 背景设为灰色
    viewer.setCameraPosition(
        0, 0, 5,                                // camera位置
        0, 0, -1,                               // view向量--相机朝向
        0, 1, 0                                 // up向量
        );
    viewer.addPointCloud(pCloudShow,"Out");
    viewer.addCoordinateSystem(1.0, "cloud", 0);
    while (!viewer.wasStopped()) { // 显示，直到‘q’键被按下
        viewer.spinOnce();
    }

    //pcl::io::savePLYFileBinary("voxelSearchResult.ply", *pCloudShow);     // 用cloudcompare读入时rgba变为0
    system("pause");
    return 0;
}