pcl之kdtree的使用

A k-d tree, or k-dimensional tree, is a data structure used in computer science for organizing some number of points in a space with k dimensions. It is a binary search tree with other constraints imposed on it. K-d trees are very useful for range and nearest neighbor searches.

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>

int main(int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
  ....
  pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
  kdtree.setIputCloud(cloud);
  pcl::PointXYZI search_point;
  ....
  // k nearest neighbor search
  int k = 10;
  std::vector<int> k_indices(k);
  std::vector<float> k_sqr_distances(k;)
  if (kdtree.nearestKSearch(search_point, k, k_indices, k_sqr_distances) > 0)
  {
    //do something
  }
  
  // neighbors within radius search
  float radius = 1.0;
  std::vector<int> 
  std::vector<float>
  if (kdtree.radiusSearch(search_point, radius, radius_indices, radius_sqr_distance) > 0)
  {
    //do something
  }

}

值得注意的是： 返回的是square_distance

参考

http://pointclouds.org/documentation/tutorials/kdtree_search.php