PCL学习总结Octree

Posted 2021-05-29 非晚非晚

1. 八叉树简介

八叉树是一种用于描述三维空间的树状数据结构，八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，将八个子节点所表示的体积元素加在一起就等于父节点的体积。

2. 点云的压缩

点云信息一般比较包含了丰富的信息而且点云数量也是比较多，这就需要我们对点云数据进行压缩。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")
  {
  }
/************************************************************************************************
  在OpenNIGrabber采集循环执行的回调函数cloud_cb_中，首先把获取的点云压缩到stringstream缓冲区，下一步就是解压缩，
  它对压缩了的二进制数据进行解码，存储在新的点云中解码了点云被发送到点云可视化对象中进行实时可视化
*************************************************************************************************/
  
 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);
    }
  }
/**************************************************************************************************************
 在函数中创建PointCloudCompression类的对象来编码和解码，这些对象把压缩配置文件作为配置压缩算法的参数
 所提供的压缩配置文件为OpenNI兼容设备采集到的点云预先确定的通用参数集，本例中使用MED_RES_ONLINE_COMPRESSION_WITH_COLOR
 配置参数集，用于快速在线的压缩，压缩配置方法可以在文件/io/include/pcl/compression/compression_profiles.h中找到，
  在PointCloudCompression构造函数中使用MANUAL——CONFIGURATION属性就可以手动的配置压缩算法的全部参数
******************************************************************************************************************/
  void run ()
  {

    bool showStatistics = true;  //设置在标准设备上输出打印出压缩结果信息

    // 压缩选项详情在: /io/include/pcl/compression/compression_profiles.h
    // 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> ();
    /***********************************************************************************************************
    下面的代码为OpenNI兼容设备实例化一个新的采样器，并且启动循环回调接口，每从设备获取一帧数据就回调函数一次，，这里的
    回调函数就是实现数据压缩和可视化解压缩结果。
   ************************************************************************************************************/
    //创建从OpenNI获取点云的抓取对象
    // create a new grabber for OpenNI devices
    pcl::Grabber* interface = new pcl::OpenNIGrabber ();

    // 建立回调函数
     // make callback function from member function
    boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);

    //建立回调函数和回调信息的绑定
     // 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 (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;  //创建一个新的SimpleOpenNIViewer  实例并调用他的run方法
  v.run ();
  return (0);
}

3. 八叉树的搜索

octree是一种用于管理稀疏3D数据的树形数据结构，每个内部节点都正好有八个子节点，pcl中的octree搜索有三种方式：

• 实现“体素内*邻搜索（Neighbors within VOxel Search）”
• “K*邻搜索（K Nearest Neighbor Search）”
• “半径内*邻搜索”（Neighbors within Radius Search）

代码示例如下：

#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
/***********************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;
  }

  // Neighbors within radius search
  /***********************半径R搜索********************************/

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

4. 无序点云数据集的空间变化检测

除了搜索功能，pcl中的八叉树还有一种功能，就是提取或检测两个点云数据集中，点云的非共同点云数据，也就是检测两个点云数据的变化部分。octree使用“双缓冲”技术实现这一功能。

代码示例：

#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
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (std::size_t i = 0; i < cloudA->size (); ++i)
  {
    (*cloudA)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudA)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudA)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }
  //输出cloudA
  // std::cout<<"cloudA:"<<std::endl;
  // for (const auto& point: *cloudA)
  //   std::cerr << "    " << point.x << " "
  //                       << point.y << " "
  //                       << point.z << std::endl;

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

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  octree.switchBuffers ();//双缓冲

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

  for (std::size_t i = 0; i < cloudB->size (); ++i)
  {
    (*cloudB)[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudB)[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    (*cloudB)[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }
  //输出cloudB
  // std::cout<<"cloudB:"<<std::endl;
  // for (const auto& point: *cloudA)
  //   std::cerr << "    " << point.x << " "
  //                       << point.y << " "
  //                       << point.z << std::endl;
  // 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
  // 获取前一cloudA对应八叉树在cloudB对应在八叉树中没有的点集
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

  // Output points
  //输出cloudB中，cloudA没有的点云
  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)[newPointIdxVector[i]].x << " "
              << (*cloudB)[newPointIdxVector[i]].y << " "
              << (*cloudB)[newPointIdxVector[i]].z << std::endl;

}