PCL 中基于 Octree 的查找

一定要注意 Voxel 查找的使用，切勿滥用！！！

另外要注意 resolution 的含义

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <vector>
#include <ctime>
#include <boost/concept_check.hpp>

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

  std::cout << "\n\n---------------------------------------------------------\n\n";

  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::cout << "\n\n---------------------------------------------------------\n\n";

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