PCL:octree

可能最近的项目和数据结构之类的杠上了,开始恶补一些数据结构的知识,加上学习PCL经常会使用kd-tree,octree之类的,作为知识储备也好,或者觉得其实这些也挺有趣的,记录下来方便以后查阅也罢,总之还是会努力加油的啦O(∩_∩)O哈哈~

八叉树(Octree)是一种用于描述三维空间的树状数据结构,每个根节点有8个子节点(理想情况下),也许就是八叉树名字的由来。我们先从一维开始,如何快速的在x轴上找到一个理想的数,一般以0为基准,会问大于0还是小于零,也就是说一维的情况下就是以二叉树为代表做查找或者插入等操作。

那么在二维的情况下呢,这个最直观了,在白纸上就能这么分,把一整块区域分成四块,如下图所示,然后对每块在进行细分,这就是四叉树。
PCL:octree_第1张图片

三维呢?西瓜都吃过吧,怎么切三刀把西瓜平均分成八分?这用到的就是八叉树的原理啦!然后把每一块再进行细分,直到达到最大的level数,或者说已经分到了体素级别了。什么叫体素(voxel),在二维图像中通常都会说像素,就是表示二维图像的最小单位(当然还有亚像素,一般情况下还是用像素作为基本的单位)对应到三维就是体素(如果长度为1),可能理解起来就是一个非常小的立方体,如果存在,则这个小立方体是填充状态,如果是空的,就代表没有数据。查找对应的数据,在通常情况下规模是八分之一的缩小的。极端情况下,比如说你查找的数是蹲在墙角的,那么找它的时间会长一些。

PCL:octree_第2张图片

这段代码主要做的工作就是在随机生成的点云中,利用KNN或者RadiusSearch,在构造好的octree中查找指定的点云。

#include 
#include
#include
#include 
#include 
#include 

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

  pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

  //generate pointcloud data
  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);
  }

  //分辨率描述了最小体元素的长度为128,如果点云的边界框是已知的,应该使用defineBoundingBox的方法分配给octree
  float resolution = 128.0f;

  pcl::octree::OctreePointCloudSearch 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 << "Neighbour 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 neighbour 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
             << " ( square distance: " << pointNKNSquaredDistance[i] << " ) " << std::endl;
  }

  std::vector<int>pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquareDistance;

  float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

  std::cout << "Neighbours within radius search at ( " << searchPoint.x
         << " " << searchPoint.y
         << " " << searchPoint.z
         << " ) with radius = " << radius << std::endl;

  if(octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquareDistance) > 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: " << pointRadiusSquareDistance[i] << " ) " << std::endl;     
  }
}

PCL:octree_第3张图片

数据是随机取得,每次实验结果可能会有所不同O(∩_∩)O哈哈~

什么叫“organized” point cloud?什么又叫“unorganized” point cloud?看paper的时候有些疑惑,后来查了资料得知:“unorganized” (LZ感觉翻译过来总是怪怪的,就不翻译了)就是指点云的特征会有所不同,例如会有不同的尺寸,分辨率,密度或者排序,没有一个特别统一的形式。对于“organized”的点云通常是指有固定宽度或者高度的单个深度图,对于一些数据的分析相对来说可能快一些,例如直接用rgbd相机获取的点云应该就是属于organized的

下面这段代码的主要工作是随机生成两个点云,都构造成ocree,检索点云B中体素上的点,如果在点云A中不存在,就返回点的indices

#include 
#include 
#include 
#include 
#include 
#include 

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 octree(resolution);
    pcl::PointCloud::Ptr cloudA(new pcl::PointCloud);

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

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

    //switch octree buffers: this resets octree but keeps previous tree structure in memery
    octree.switchBuffers();

    pcl::PointCloud::Ptr cloudB(new pcl::PointCloud);

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

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

    std::vector<int> newPointIdxVector;

    //get vector of point indices from octree voxel which did not exist in previous buffers
    octree.getPointIndicesFromNewVoxels(newPointIdxVector);

    //Output points
    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;


    return 0;
}

PCL:octree_第4张图片

参考地址:
http://www.pointclouds.org/documentation/tutorials/octree.php#octree-search
http://www.pointclouds.org/documentation/tutorials/octree_change.php#octree-change-detection

你可能感兴趣的:(SLAM)