PCL——八叉树Octree

Posted 2020-12-16 long5683

@

目录

• 一、八叉树简介：
• 二、构建步骤
• 三、点云八叉树应用算法:
  • 3.1 Octree用于点云压缩
  • 3.2 基于Octree的空间划分及搜索
  • 3.3 无序点云的空间变化检测
  • 3.4 占据检测
  • 3.5 获取所有占用体素的中心点(Voxel grid filter/downsampling)
  • 3.6 删除点所在的体素
  • 3.7 迭代器
  • 3.8 光线跟踪碰撞检测
    • 支付宝
    • 微信

一、八叉树简介：

• 体素化使用空间的均匀分割
• 八叉树对空间非均匀分割(按需分割)
  • 1D数据的2叉树表示
  • 2D数据的4叉树表示
  • 3D数据的8叉树表示
    

二、构建步骤

(1).设定最大递归深度。
(2).找出场景的最大尺寸，并以此尺寸建立第一个立方体。
(3).依序将单位元元素丢入能被包含且没有子节点的立方体。
(4).若没达到最大递归深度，就进行细分八等份，再将该立
方体所装的单位元元素全部分担给八个子立方体。
(5).若发现子立方体所分配到的单位元元素数量不为零且跟
父立方体是一样的，则该子立方体停止细分，因为根据空
间分割理论，细分的空间所得到的分配必定较少，若是一
样数目，则再怎么切数目还是一样，会造成无穷切割的情
形。
(6).重复3，直到达到最大递归深度。

三、点云八叉树应用算法:

• 搜索操作(邻域、半径、体素搜索)
• 降采样(体素网格/体素质心滤波器)
• 点云压缩
• 空间变化检测
• 空间点密度分析
• 占用检查/占位地图
• 碰撞检测
• 点云合并

3.1 Octree用于点云压缩

在下面，我们将解释如何有效地压缩单点云和点云流。在这个例子中，我们使用OpenNIGrabber捕捉点云，并使用PCL点云压缩技术进行压缩。

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>

#include <pcl/compression/octree_pointcloud_compression.h>

#include <stdio.h>
#include <sstream>
#include <stdlib.h>

#ifdef WIN32
# define sleep(x) Sleep((x)*1000)
#endif

class SimpleOpenNIViewer
{
public:
  SimpleOpenNIViewer () :
    viewer (" Point Cloud Compression Example")
  {
  }

  void
  cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
  {
    if (!viewer.wasStopped ())
    {
      // stringstream to store compressed point cloud
      std::stringstream compressedData;
      // output pointcloud
      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut (new pcl::PointCloud<pcl::PointXYZRGBA> ());

      // compress point cloud
      PointCloudEncoder->encodePointCloud (cloud, compressedData);

      // decompress point cloud
      PointCloudDecoder->decodePointCloud (compressedData, cloudOut);


      // show decompressed point cloud
      viewer.showCloud (cloudOut);
    }
  }

  void
  run ()
  {

    bool showStatistics = true;

    // for a full list of profiles see: /io/include/pcl/compression/compression_profiles.h
    pcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

    // instantiate point cloud compression for encoding and decoding
    PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> (compressionProfile, showStatistics);
    PointCloudDecoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA> ();

    // create a new grabber for OpenNI devices
    pcl::Grabber* interface = new pcl::OpenNIGrabber ();

    // make callback function from member function
    std::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
      [this] (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud) { cloud_cb_ (cloud); };

    // connect callback function for desired signal. In this case its a point cloud with color values
    boost::signals2::connection c = interface->registerCallback (f);

    // start receiving point clouds
    interface->start ();

    while (!viewer.wasStopped ())
    {
      sleep (1);
    }

    interface->stop ();

    // delete point cloud compression instances
    delete (PointCloudEncoder);
    delete (PointCloudDecoder);

  }

  pcl::visualization::CloudViewer viewer;

  pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudEncoder;
  pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* PointCloudDecoder;

};

int
main (int argc, char **argv)
{
  SimpleOpenNIViewer v;
  v.run ();

  return (0);
}

3.2 基于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->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); // instantiate an object ptr
  // Set input cloud data
  octree.setInputCloud (cloud); 
 // 定义Octree边界框(可选操作)
 //计算输入点云的边界框
  octree.defineBoundingBox();
 //手动定义点云的边界框
  octree.defineBoundingBox(minX,minY,minZ,maxX,maxY,maxZ);
	// 输入点云添加到Octree
  octree.addPointsFromInputCloud ();

// create a virtual searchPoint
  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->points[pointIdxVec[i]].x 
       << " " << cloud->points[pointIdxVec[i]].y 
       << " " << cloud->points[pointIdxVec[i]].z << std::endl;
  }

  // K nearest neighbor search
  // 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->points[ pointIdxNKNSearch[i] ].x 
                << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
                << " " << cloud->points[ 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->points[ pointIdxRadiusSearch[i] ].x 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }
	// 删除八叉树，释放内存
	octree.deleteTree()
}

PCL八叉树组件提供了几种八叉树类型。它们的叶节特征不同。

CctreePointCloudPointVector (等于OctreePointCloud):	该八叉树能够保存每一个叶节点上的点索引列。
OctreePointCloudSinglePoint:	该八叉树类仅仅保存每一个叶节点上的单个点索引。仅仅保存最后分配给叶节点的点索引。
OctreePointCloudOccupancy	该八叉树不存储它的叶节点上的任何点信息。它能用于空间填充情况检查。
OctreePointCloudDensity:	该八叉树存储每一个叶节点体素中的点的数目。它可以进行空间点集密程度查询。

3.3 无序点云的空间变化检测

检测出点云B中不在点云A空间的点的索引

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

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

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

  // Octree resolution - side length of octree voxels
  float resolution = 32.0f;

  // Instantiate octree-based point cloud change detection class
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

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

  // Generate pointcloud data for cloudA
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (std::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);
  }

  // Add points from cloudA to octree
  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud ();

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  //交换八叉树缓存,但是cloudA对应的八叉树结构仍在内存中
  octree.switchBuffers ();

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
   
  // Generate pointcloud data for cloudB 
  cloudB->width = 128;
  cloudB->height = 1;
  cloudB->points.resize (cloudB->width * cloudB->height);

  for (std::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);
  }

  // Add points from cloudB to octree
  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud ();

  std::vector<int> newPointIdxVector;

  // Get vector of point indices from octree voxels which did not exist in previous buffer
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

  // Output points
  std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
  for (std::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;

}

3.4 占据检测

3.5 获取所有占用体素的中心点(Voxel grid filter/downsampling)

std::vector<PointXYZ> pointGrid;
octree.getOccupiedVoxelCenters(pointGrid) ;

3.6 删除点所在的体素

3.7 迭代器

3.8 光线跟踪碰撞检测

# 打赏
码字不易，如果对您有帮助，就打赏一下吧O(∩_∩)O

支付宝

微信