PCL octree_search八叉树的使用

一、概述

  PCL中八叉树的简单使用案例

二、代码

octree_search.cpp

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

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

int
main ()
{
  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

  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

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

}

三、结果

Neighbors within voxel search at (98.0312 327.375 836.094)
    131.031 345.656 768.656
    105.875 293.062 745.906
K nearest neighbor search at (98.0312 327.375 836.094) with K=10
    131.031 345.656 768.656 (squared distance: 5971.02)
    114.156 376.5 907.375 (squared distance: 7754.3)
    45.7188 375.781 900.594 (squared distance: 9240.01)
    105.875 293.062 745.906 (squared distance: 9372.66)
    69.0312 374.375 751.688 (squared distance: 10174.4)
    67.7188 243.438 883.594 (squared distance: 10220.6)
    49.6875 211.969 804.25 (squared distance: 16669.7)
    158.219 217.25 880.719 (squared distance: 17741.4)
    232.719 320.594 820.094 (squared distance: 18442.7)
    37.7188 378.344 948.188 (squared distance: 18800.4)
Neighbors within radius search at (98.0312 327.375 836.094) with radius=187.789
    49.6875 211.969 804.25 (squared distance: 16669.7)
    0.03125 325 721.406 (squared distance: 22762.9)
    164.656 203.031 722.031 (squared distance: 32910.5)
    173.5 330.344 669.969 (squared distance: 33301.9)
    158.719 349.75 665.75 (squared distance: 33200.6)
    131.031 345.656 768.656 (squared distance: 5971.02)
    105.875 293.062 745.906 (squared distance: 9372.66)
    232.719 320.594 820.094 (squared distance: 18442.7)
    12.125 403.531 721.562 (squared distance: 26297.1)
    35.375 477.375 835.062 (squared distance: 26426.9)
    61.8438 496.469 785.656 (squared distance: 32446.2)
    69.0312 374.375 751.688 (squared distance: 10174.4)
    158.219 217.25 880.719 (squared distance: 17741.4)
    82.5 232.625 951.531 (squared distance: 22544.6)
    67.7188 243.438 883.594 (squared distance: 10220.6)
    99.6875 271.594 991.594 (squared distance: 27294.5)
    251.656 272.875 916.969 (squared distance: 33111.7)
    62.125 443.969 936.844 (squared distance: 25033.9)
    45.7188 375.781 900.594 (squared distance: 9240.01)
    37.7188 378.344 948.188 (squared distance: 18800.4)
    114.156 376.5 907.375 (squared distance: 7754.3)
    211.875 406.344 873.344 (squared distance: 20584)