PCL教程指南-Spatial Partitioning and Search Operations with Octrees(八叉树数据组织及搜索)

最新推荐文章于 2023-08-15 22:32:15 发布

点云哥

最新推荐文章于 2023-08-15 22:32:15 发布

阅读量733

点赞数

分类专栏：点云处理文章标签： pcl

本文链接：https://blog.csdn.net/qq_41795143/article/details/112539617

版权

点云处理专栏收录该内容

16 篇文章 12 订阅

订阅专栏

PCL教程指南-八叉树数据组织及搜索

官方原文档
对点云数据组织为八叉树结构，进行体素内搜索,邻近搜索,半径搜索
对文档代码进行解释，并扩展相关内容

#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>);

  // 生成1000个随机点
  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);

  // 搜索searchPoint所在体素内其他点

  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;

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

}

扩展与解释

不同类型八叉树问题

文档中提及的不同类型，都是不同功能的八叉树子类

类型	作用
OctreePointCloudPointVector (equal to OctreePointCloud)	这个八叉树可以在每个叶节点保存一个点索引列表
OctreePointCloudSinglePoint	这个八叉树类在每个叶节点上只有一个点索引。只存储分配给叶节点的最近的点索引
OctreePointCloudOccupancy	这个八叉树在其叶节点上不存储任何点信息。它可以用于空间占用检查。
OctreePointCloudDensity	这个八叉树计算每个叶节点体素内的点数。它允许空间密度查询

这些类型区分主要依靠子节点和叶节点特征划分，源码中使用八叉树容器pcl::octree::OctreeContainerBase进行封装

观察八叉树相关类文档会发现，例如pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >很多都带有LeafContainerT, BranchContainerT泛型模板，他们就是继承自pcl::octree::OctreeContainerBase实现的容器类。而以上5种类型则是根据不同的容器类实现的。举例说明：

类	容器类
OctreePointCloudSinglePoint	typename LeafContainerT = OctreeContainerPointIndex, typename BranchContainerT = OctreeContainerEmpty
OctreePointCloudDensity	typename LeafContainerT = OctreePointCloudDensityContainer,typename BranchContainerT = OctreeContainerEmpty>

…

对于使用者来说，除非自定义否则容器类用户不需要设置，简单使用时泛型模板只需要关心PointT即可

扩展方法

除此之外，搜索类中还有几个常用的方法：

getIntersectedVoxelCenters()获取与射线相交的所有体素的中心点的向量(origin，direction)

int pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::getIntersectedVoxelCenters	(	
Eigen::Vector3f 	origin,
Eigen::Vector3f 	direction,
AlignedPointTVector & 	voxel_center_list, //AlignedPointTVector为 std::vector<PointT, Eigen::aligned_allocator<PointT> >
int 	max_voxel_count = 0 
)		const

boxSearch()在矩形搜索区域内搜索点，搜索矩形边缘上的点也包括在内。


int pcl::octree::OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT >::boxSearch	(	
const Eigen::Vector3f & 	min_pt,
const Eigen::Vector3f & 	max_pt,
std::vector< int > & 	k_indices 
)		const

有多种扩展型方法都带有Recursive，例如:

方法	作用
getIntersectedVoxelCentersRecursive()	除了获取射线相交的中心向量，还有递归树中所有叶节点
boxSearchRecursive()	除了获取矩形搜索框的点，还有递归树中所有相关的叶节点

…
此类方法即递归搜索树，除了一般结果还有与节点相关的获取类型： const OctreeNode * node | const OctreeKey & key
节点类型和八叉树键类型，节点类型即八叉树结构中的节点，而八叉树键即包含了各个维度的数值（点云三维所以包含XYZ的数值）