pcl应用八叉树实例

原创已于 2024-02-01 11:30:00 修改 · 1.1k 阅读

18 ·

CC 4.0 BY-SA版权

文章标签：

#PCL #八叉树 #算法

于 2024-01-29 09:22:52 首次发布

PCL 同时被 2 个专栏收录

12 篇文章

订阅专栏

算法

5 篇文章

订阅专栏

本文详细介绍了PCL中八叉树的数据结构及其在空间划分、搜索（包括体素搜索、最近邻搜索和范围搜索）以及无序点云数据集空间变化检测的应用。着重讲解了关键函数OctreePointCloudSearch和voxelSearch的使用方法。

pcl应用八叉树实例

文章目录

1、基本概念

八叉树结构通过循环递归的划分方法对大小为2 n ∗ 2 n ∗ 2 n
的三维空间的几何对象进行划分，从而构成一个具有根节点的方向图。
在八叉树结构中，如果被划分的体元具有相同的属性，则该体元结构构成一个叶节点；否则继续将该体元剖分成8个子立方体，依次递归剖分。
对于2 n ∗ 2 n ∗ 2 n 大小的空间对象，最多剖分n 次。
八叉树多用于在3D空间中搜索。
八叉树算法的算法实现简单，但在大数据量的点云数据下，其使用比较困难的是最小粒度（叶节点）的确定，粒度较大时，有的节点数据量可能仍比较大，后续查询效率仍比较低，反之，粒度较小，八叉树的深度增加，需要的内存空间也比较大（每个非叶子节点需要八个指针），效率也降低。而等分的划分依据，使得在数据重心有偏斜的情况下，受划分深度限制，其效率不是太高。
k-d在邻域查找上比较有优势，但在大数据量的情况下，若划分粒度较小时，建树的开销也较大，但比八叉树灵活些。在小数据量的情况下，其搜索效率比较高，但在数据量增大的情况下，其效率会有一定的下降，一般是线性上升的规律。

在这里插入图片描述

2、基于八叉树的空间划分及搜索操作

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.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>);

  cloud->width =1000;
  cloud->height =1;
  cloud->points.resize (cloud->width * cloud->height);
  
  for (size_t i=0; i< cloud->points.size (); ++i) //随机产生点云数据集
  {
    cloud->points[i].x =1024.0f* rand () / (RAND_MAX +1.0f);
    cloud->points[i].y =1024.0f* rand () / (RAND_MAX +1.0f);
    cloud->points[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 (size_t i=0; i<pointIdxVec.size (); ++i)
    {
      std::cout<<"    "<< cloud->points[pointIdxVec[i]].x <<" "<< cloud->points[pointIdxVec[i]].y <<" "<< cloud->points[pointIdxVec[i]].z <<std::endl;
    }
  }

  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 (size_t i=0; i<pointIdxNKNSearch.size (); ++i)
    {
      std::cout<<"    "<<   cloud->points[ pointIdxNKNSearch[i] ].x <<" "<< cloud->points[pointIdxNKNSearch[i] ].y <<" "<< cloud->points[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 (size_t i=0; i<pointIdxRadiusSearch.size (); ++i)
    {
      std::cout<<"    "<<   cloud->points[ pointIdxRadiusSearch[i] ].x <<" "<< cloud->points[pointIdxRadiusSearch[i] ].y <<" "<< cloud->points[pointIdxRadiusSearch[i] ].z <<" (squared distance: "<<pointRadiusSquaredDistance[i] <<")"<<std::endl;
    }
  }
}

2.1、关键函数说明

2.1.2 OctreePointCloudSearch 类

类OctreePointCloudSearch定义在文件pcl/octree/octree_search.h中

/** \brief Constructor.
  * \param[in] resolution octree resolution at lowest octree level
  */
OctreePointCloudSearch (const double resolution) :
  OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution)
{
}

浮点型参数resolution：八叉树最深层的最小体素的尺寸。
体素：体素(voxel)，是体积元素(volumepixel)的简称。体素是数字数据于三维空间分割上的最小单位，体素用于三维成像、科学数据与医学影像等领域。概念上类似二维空间的最小单位——像素，像素用在二维计算机图像的影像数据上。有些真正的三维显示器运用体素来描述它们的分辨率。

2.1.2 voxelSearch 函数

函数voxelSearch定义在文件pcl/octree/octree_search.h中
体素近邻搜索，它把查询点所在的体素中其他点的索引作为查询结果返回，结果以点索引的向量的形式保存，因此搜索点和搜索结果之间的距离取决于八叉树的最深层的最小体素尺寸参数。

/** \brief Search for neighbors within a voxel at given point
  * \param[in] point point addressing a leaf node voxel
  * \param[out] point_idx_data the resultant indices of the neighboring voxel points
  * \return "true" if leaf node exist; "false" otherwise
  */
bool
voxelSearch (const PointT& point, std::vector<int>& point_idx_data);

第一次参数：要查询的点
第二个参数：一个vector，执行后这里面存放的是查询点所在的体素中其他点的索引
如果这个查询点在八叉树的一个叶子节点中，返回True，否则返回False

3、无序点云数据集的空间变化检测

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
	
int main (int argc, char**argv)
{
  srand ((unsigned int) time (NULL));

  float resolution =32.0f; 
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree (resolution); // 初始化空间变化检测对象
  
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> ); // 128个点的无序点云cloudA
  cloudA->width =128;
  cloudA->height =1;
  cloudA->points.resize (cloudA->width * cloudA->height);
  for (size_t i=0; i<cloudA->points.size (); ++i)
  {
    cloudA->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
    cloudA->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
    cloudA->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
  }

  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud (); // 用cloudA构建八叉树

  octree.switchBuffers ();
  // 类OctreePointCloudChangeDetector定义在pcl/octree/octree_pointcloud_changedetector.h中，继承自Octree2BufBase类
  // Octree2BufBase类允许同时在内存中保存和管理两个八叉树。
  // 通过成员函数switchBuffers()，重置了八叉树对象的缓冲区，但把之前的八叉树数据仍保留在内存中。
  
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> ); // 128个点的无序点云cloudB
  cloudB->width =128;
  cloudB->height =1;
  cloudB->points.resize (cloudB->width *cloudB->height);
  for (size_t i=0; i<cloudB->points.size (); ++i)
  {
    cloudB->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
    cloudB->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
    cloudB->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
  }

  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud (); // 用cloudB构建八叉树，新的八叉树和旧的八叉树共享八叉树对象，但同时在内存中驻留
  
  std::vector<int>newPointIdxVector; // 放索引值

  octree.getPointIndicesFromNewVoxels (newPointIdxVector); // 用于探测cloudB相对于cloudA增加的点集，但是不能探测在cloudA上减少的点集

  std::cout<<"Output from getPointIndicesFromNewVoxels:"<<std::endl;
  for (size_t i=0; i<newPointIdxVector.size (); ++i)
  {
    std::cout<<i<<"# Index:"<<newPointIdxVector[i]<<"  Point:"<<cloudB->points[newPointIdxVector[i]].x <<" "<<cloudB->points[newPointIdxVector[i]].y <<" "<<cloudB->points[newPointIdxVector[i]].z <<std::endl;
  }
}