PCL 八叉树的应用（体素搜索+半径搜索+K邻域搜索+获取与射线相交的所有体素块的索引+获取用户指定范围内的体素块索引...）（OCtree_OctreePointCloudSearch）

注：体素搜索为八叉树特有，其余半径搜索、K邻域搜索和KD树的原理差异不大，可关联理解。

半径搜索、K邻域搜索时，KD树速度快于八叉树，一般使用KD树搜索。

除以上三个搜索外的其余搜索操作（【1】获取八叉树中，与射线相交的所有体素块的索引、【2】获取八叉树中，用户指定范围内的所有体素块的索引、【3】获取与输入点最邻近点的索引），不做可视化展示，仅提供方法批注。

其余可调用方法，如检查用户输入位置的体素块是否存在、获取树深、获取体素块中心点等在PCL八叉树的介绍：6.PCL八叉树关键函数中提及。

1.原理

PCL八叉树的介绍https://blog.csdn.net/xhm01291212/article/details/141917680?spm=1001.2014.3001.5502

KD树的原理及应用（K邻域搜索、半径搜索）https://blog.csdn.net/xhm01291212/article/details/141893876?spm=1001.2014.3001.5502

2.使用场景

• 体素搜索（搜索同查找点在同一体素块中的点云点）
• K邻域搜索（搜索距离输入点Q，最邻近的K个点）
• 半径搜索（搜索输入点Q半径R内的所有点）
• 获取八叉树中，与射线相交的所有体素块的索引
• 获取八叉树中，用户指定范围内的所有体素块的索引
• 获取与输入点最邻近点的索引

3.注意事项

半径搜索、K邻域搜索时，KD树速度快于八叉树，一般使用KD树搜索，八叉树使用较少。

体素搜索只能用八叉树。

4.关键函数

（1）体素搜索：返回值为与待查找点同一体素块中的点云点的索引。

 // 体素搜索
    std::vector<int> pointIdxsVox;                      // 得到与查找点在同一个体素块中的点云点索引
    bool bFlagVox = false;
    if (octree.voxelSearch(searchPoint, pointIdxsVox))
    {
        if (pointIdxsVox.size() > 0)
        {
            bFlagVox = true;
        }
    }

（2）半径搜索

// 半径搜索
    std::vector<int> pointIdxRadiusSearch;              // 半径搜索结果的索引
    std::vector<float> pointRadiusSquaredDistance;      // 半径搜索得到与查找点的距离的平方
    double radius = 0.03;                               // 查找半径
    bool bFlagRadius = false;
    if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
        if (pointIdxRadiusSearch.size() > 0)
        {
            bFlagRadius = true;
        }
    }

（3）K邻域搜索

 // K邻域搜索
    std::vector<int> pointIdxNKNSearch;                 // K邻域搜索结果的索引
    std::vector<float> pointNKNSquaredDistance;         // K邻域搜索得到与查找点的距离的平方
    int K = 500;                                        // K邻域值
    bool bFlagK = false;
    if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        if (pointIdxNKNSearch.size() > 0)
        {
            bFlagK = true;
        }
    }

（4）获取八叉树中，与射线相交的所有体素块的索引

  uindex_t
  getIntersectedVoxelIndices(Eigen::Vector3f origin,
                             Eigen::Vector3f direction,
                             Indices& k_indices,
                             uindex_t max_voxel_count = 0) const;

（5）获取八叉树中，用户指定范围内的所有体素块的索引

  uindex_t
  boxSearch(const Eigen::Vector3f& min_pt,
            const Eigen::Vector3f& max_pt,
            Indices& k_indices) const;

（6）获取与输入点最邻近点的索引

  inline void
  approxNearestSearch(const PointCloud& cloud,
                      uindex_t query_index,
                      index_t& result_index,
                      float& sqr_distance)
  {
    return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
  }

5.代码

#include <iostream>
#include <pcl/io/pcd_io.h>	
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

int main()
{
	/***************八叉树K邻域搜索及半径搜索********************/
	// 原始点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("D:/code/csdn/data/bunny.pcd", *cloud);    // 原始点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVox(new pcl::PointCloud<pcl::PointXYZ>);       // 体素搜索结果点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRadius(new pcl::PointCloud<pcl::PointXYZ>);    // 半径搜索结果点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudK(new pcl::PointCloud<pcl::PointXYZ>);         // K邻域搜索结果点云

    // 建立八叉搜索树
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.02);// 使用分辨率初始化八叉树
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();
    pcl::PointXYZ searchPoint = cloud->points[0];       // 待搜索点

    // 体素搜索
    std::vector<int> pointIdxsVox;                      // 得到与查找点在同一个体素块中的点云点索引
    bool bFlagVox = false;
    if (octree.voxelSearch(searchPoint, pointIdxsVox))
    {
        if (pointIdxsVox.size() > 0)
        {
            bFlagVox = true;
        }
    }
    pcl::copyPointCloud(*cloud, pointIdxsVox, *cloudVox);       // 将结果拷贝到新点云中

    // 半径搜索
    std::vector<int> pointIdxRadiusSearch;              // 半径搜索结果的索引
    std::vector<float> pointRadiusSquaredDistance;      // 半径搜索得到与查找点的距离的平方
    double radius = 0.03;                               // 查找半径
    bool bFlagRadius = false;
    if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
        if (pointIdxRadiusSearch.size() > 0)
        {
            bFlagRadius = true;
        }
    }
    pcl::copyPointCloud(*cloud, pointIdxRadiusSearch, *cloudRadius);       // 将结果拷贝到新点云中

    // K邻域搜索
    std::vector<int> pointIdxNKNSearch;                 // K邻域搜索结果的索引
    std::vector<float> pointNKNSquaredDistance;         // K邻域搜索得到与查找点的距离的平方
    int K = 500;                                        // K邻域值
    bool bFlagK = false;
    if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        if (pointIdxNKNSearch.size() > 0)
        {
            bFlagK = true;
        }
    }
    pcl::copyPointCloud(*cloud, pointIdxNKNSearch, *cloudK);       // 将结果拷贝到新点云中

    // 展示
    std::cout << "体素搜索得到点的个数：" << cloudVox->size() << std::endl;
    std::cout << "半径搜索得到点的个数：" << cloudRadius->size() << std::endl;
    std::cout << "K邻域搜索得到点的个数：" << cloudK->size() << std::endl;

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewVox(new pcl::visualization::PCLVisualizer("vox"));
    viewVox->addPointCloud(cloud, "V0");
    viewVox->addPointCloud<pcl::PointXYZ>(cloudVox, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudVox, 0, 0, 255), "V1");

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewRadius(new pcl::visualization::PCLVisualizer("Radius"));
    viewRadius->addPointCloud(cloud, "R0");
    viewRadius->addPointCloud<pcl::PointXYZ>(cloudRadius, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudRadius, 0, 255, 255), "R1");

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewK(new pcl::visualization::PCLVisualizer("K"));
    viewK->addPointCloud(cloud, "K0");
    viewK->addPointCloud<pcl::PointXYZ>(cloudK, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudK, 255, 0, 255), "K1");

    while (!viewVox->wasStopped() || !viewRadius->wasStopped() || !viewK->wasStopped())
    {
        viewVox->spinOnce(100);
        viewRadius->spinOnce(100);
        viewK->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

6.结果展示

（1）体素搜索

1）打印信息：

2）可视化结果：红色部分即为搜索结果。

（2）半径搜索：

1）打印信息：

2）可视化结果：蓝色部分即为搜索结果

（3）K邻域搜索

1）打印信息

2）可视化结果：粉色部分即为搜索结果。