PCL——（6）八叉樹Octree

@

目錄

• 一、八叉樹簡介：
• 二、構建步驟
• 三、點雲八叉樹應用算法:
  • 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

支付寶

微信