【PCL】教程 octree_search 八叉树的构建、体素内搜索、K近邻搜索和半径内搜索

使用八叉树进行空间分区和搜索操作 

八叉树是一种基于树的数据结构，用于管理稀疏的三维数据。每个内部节点都有八个子节点。在本教程中，我们将学习如何使用八叉树对点云数据进行空间分区和邻域搜索。特别是，我们将讲解如何执行 "体素内邻居搜索"、"K 最近邻搜索 "和 "半径内邻居搜索"。

终端输出：

K nearest neighbor search at (54.6875 786.5 19.9375) with K=10
    88.75 906.094 56.875 (squared distance: 16827.3)
    151.125 886.562 83.8438 (squared distance: 23396.7)
    61.9688 932.469 97.75 (squared distance: 27414.7)
    188.812 711.719 109.156 (squared distance: 31541.7)
    238.906 757.594 55.4688 (squared distance: 36034.6)
    217.594 874.125 82.25 (squared distance: 38099.4)
    170.062 628.906 0.28125 (squared distance: 38533.5)
    173.75 616.719 16.2812 (squared distance: 43014.9)
    85.6875 675.156 193.375 (squared distance: 43439)
    96.0938 816.375 222.5 (squared distance: 43638.6)
Neighbors within radius search at (54.6875 786.5 19.9375) with radius=113.641

这段代码是使用C++编写的，并且利用了PCL（Point Cloud Library，点云库）来进行点云数据的搜索和处理。主要内容包括初始化点云数据，构建八叉树索引，并通过不同的搜索方法来查询点云数据。

具体来说，这段代码首先包含本PCL库中处理点云和八叉树搜索的相关头文件，还有标准输入输出头文件和向量处理头文件。在 main() 函数中完成了以下几个步骤：

1. 初始化一个点云对象 cloud。

2. 随机生成1000个点的点云数据，这些点的X、Y、Z坐标值是通过随机数生成的。

3. 创建一个八叉树搜索对象 octree，并设置其分辨率为128。

4. 将生成的点云数据添加到八叉树索引中。

5. 生成一个随机的搜索点 searchPoint。

6. 执行体素内的邻域搜索，将搜索到的点索引存储在 pointIdxVec 向量中，并打印出这些点的坐标。

7. 执行K近邻搜索，找到距离 searchPoint 最近的K个点（这里K设置为10），并打印出这些点的坐标和它们到搜索点的平方距离。

8. 最后执行半径内的邻域搜索，设置一个随机的搜索半径 radius，查找在这个半径内的所有点，并打印出它们的坐标和到搜索点的平方距离。

这段代码演示了PCL中八叉树的基本使用方法，包括八叉树的构建、体素内搜索、K近邻搜索和半径内搜索。八叉树是一种用于空间划分和快速点查找的数据结构，在点云数据处理中广泛应用。

//  包含PCL(点云库)和标准输入输出库头文件
#include <pcl/point_cloud.h> // 引入PCL库的点云定义头文件
#include <pcl/octree/octree_search.h> // 引入PCL库的八叉树搜索相关头文件


#include <iostream> // 引入标准输入输出流库，用于控制台输入输出
#include <vector> // 引入STL的vector容器类
#include <ctime> // 引入C标准库时间函数


int main ()
{
  // 初始化随机数种子
  srand ((unsigned int) time (NULL));


  // 创建一个新的点云指针
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);


  // 生成点云数据
  // 设定点云宽度为1000，高度为1，表示所有点在同一行
  cloud->width = 1000;
  cloud->height = 1;
  // 调整点云总点数
  cloud->points.resize (cloud->width * cloud->height);


  // 遍历并随机生成点云中的点
  for (std::size_t i = 0; i < cloud->size (); ++i)
  {
    (*cloud)[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    (*cloud)[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
  }


  // 设置八叉树的分辨率
  float resolution = 128.0f;


  // 创建八叉树并设置分辨率
  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);


  // 设置输入点云到八叉树
  octree.setInputCloud (cloud);
  // 从输入点云中添加点到八叉树
  octree.addPointsFromInputCloud ();


  // 创建并随机生成一个搜索点
  pcl::PointXYZ searchPoint;
  searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);


  // 体素内近邻搜索
  std::vector<int> pointIdxVec;


  // 如果搜索点的体素内存在点，则输出这些点的坐标
  if (octree.voxelSearch (searchPoint, pointIdxVec))
  {
    std::cout << "Neighbors within voxel search at (" << searchPoint.x 
     << " " << searchPoint.y 
     << " " << searchPoint.z << ")" 
     << std::endl;


    // 遍历所有在同一体素中的点并输出它们的坐标
    for (std::size_t i = 0; i < pointIdxVec.size (); ++i)
   std::cout << "    " << (*cloud)[pointIdxVec[i]].x 
       << " " << (*cloud)[pointIdxVec[i]].y 
       << " " << (*cloud)[pointIdxVec[i]].z << std::endl;
  }


  // 最近的K个邻居搜索
  int K = 10;
  std::vector<int> pointIdxNKNSearch;
  std::vector<float> pointNKNSquaredDistance;


  // 进行K邻近搜索，并输出搜索点的坐标以及K的值
  std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;


  // 如果找到了则输出每一个近邻点的坐标和它们的平方距离
  if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
  {
    for (std::size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
      std::cout << "    "  <<   (*cloud)[ pointIdxNKNSearch[i] ].x 
                << " " << (*cloud)[ pointIdxNKNSearch[i] ].y 
                << " " << (*cloud)[ pointIdxNKNSearch[i] ].z 
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
  }


  // 半径内邻居搜索
  std::vector<int> pointIdxRadiusSearch; // 用于存储搜索到的点的索引
  std::vector<float> pointRadiusSquaredDistance; // 用于存储搜索到的点到搜索点的平方距离


  float radius = 256.0f * rand () / (RAND_MAX + 1.0f); // 生成一个随机的搜索半径


  std::cout << "Neighbors within radius search at (" << searchPoint.x 
      << " " << searchPoint.y 
      << " " << searchPoint.z
      << ") with radius=" << radius << std::endl; // 输出搜索信息，包括搜索点坐标和搜索半径


  if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
  {
    // 如果搜索到了邻居点，则输出这些点的信息
    for (std::size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
      std::cout << "    "  <<   (*cloud)[ pointIdxRadiusSearch[i] ].x 
                << " " << (*cloud)[ pointIdxRadiusSearch[i] ].y 
                << " " << (*cloud)[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }


}

octree.voxelSearch(searchPoint, pointIdxVec)

octree.nearestKSearch(
              searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)

octree.radiusSearch(
              searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)