（十）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>intmain(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;}