使用 Octrees 的空间分区和搜索操作

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

#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>

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

int
main (int argc, char** argv)
{
  srand ((unsigned int) time (NULL));

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // Generate pointcloud data
  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);

  // Neighbors within voxel search

  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 nearest neighbor search

  int K = 10;

  std::vector<int> pointIdxNKNSearch;
  std::vector<float> pointNKNSquaredDistance;

  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;
  }

  // Neighbors within radius search

  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;
  }

}

体素内的邻居在 (337.594 749.75 534.938) 搜索
347.5 827.719 556.531
361.719 838.688 626.281
393.312 744.5 585.844
K 最近邻搜索 (337.594 749.75 534.938)，K=10
393.312 744.5 585.844（平方距离：5723.59）
347.5 827.719 556.531（平方距离：6643.55）
266.875 744.125 479.031（平方距离：8158.29）
244.969 782.625 523.781（平方距离：9784.62）
215.75 744.438 537.938（平方距离：14883.1）
438.312 688.688 581.469（平方距离：16038.1）
361.719 838.688 626.281（平方距离：16835.6）
396.031 876.656 569.844（平方距离：20738.6）
259.719 871.531 571.156（平方距离：22207）
323.375 630.5 440.344（平方距离：23370.7）
半径内的邻居在 (337.594 749.75 534.938) 搜索半径 = 237.461
255.219 559.656 531（平方距离：42936.8）
329.531 585.188 556.188（平方距离：27597.4）
457.938 583.656 614.875（平方距离：48459.8）
281.781 568.031 641.812（平方距离：47559）
195.438 648.75 383.344（平方距离：53390.1）
148.094 700.188 614.719（平方距离：44731.7）
122.281 747.875 482.438（平方距离：49119.2）
265.562 658.844 432.719（平方距离：23901.1）
269.969 595.719 606.781（平方距离：33460.3）
175.906 795.156 491.719（平方距离：30072.4）
266.875 744.125 479.031（平方距离：8158.29）
191.938 720.094 443.719（平方距离：30416.1）
215.75 744.438 537.938（平方距离：14883.1）
244.969 782.625 523.781（平方距离：9784.62）
150.281 847.625 545.938（平方距离：44786.5）
221.875 854.156 412.375（平方距离：39313.1）
259.719 871.531 571.156（平方距离：22207）
365.969 661.688 390.969（平方距离：29287.1）
323.375 630.5 440.344（平方距离：23370.7）
347.5 827.719 556.531（平方距离：6643.55）
361.719 838.688 626.281（平方距离：16835.6）
393.312 744.5 585.844（平方距离：5723.59）
438.312 688.688 581.469（平方距离：16038.1）
484.312 746.969 388.594（平方距离：42950.6）
481.781 782.406 466.812（平方距离：26497.5）
479.844 734.781 468.875（平方距离：24823.4）
343.75 872.094 640.031（平方距离：26050.6）
396.031 876.656 569.844（平方距离：20738.6）
360 971.156 585.344（平方距离：52063.6）
440.781 935.156 437.625（平方距离：54492.9）
471.875 917.719 489.344（平方距离：48323.7）
489.562 903.469 578.281（平方距离：48602.6）
283.062 628.594 692.562（平方距离：42498.1）
227.625 823.469 661.188（平方距离：33466.6）
179.906 842.531 681.781（平方距离：55036.8）
395.5 670.531 667（平方距离：27069.2）
336.25 590.031 675.656（平方距离：45313.6）
519.406 720.844 671.531（平方距离：52549.2）