pcl里面使用KdTree来搜索

下面这个教程我们将学会怎么用KdTree找一个特殊点附近的K个最近邻，然后我们也将复习怎么通过一个特殊的半径来找里面所有的近邻。

一个k-d树，或者k维的树是一个计算机科学里面的数据结构。它是一个有其它约束影响的二叉搜索树。K-d树是在深度和最近邻搜索里面很有用的。我们这次的目的是生成一个3维的k-d trees。一个k-d tree的每个层次在某个维度上分割成所有的子树，使用一个垂直于相应坐标轴的高维平面。在树的根部，所有的子树将会被分割以第一维(如果第一维坐标系比根部少，它将会成为左子树，如果比根部多，它将会成为右子树)。每一层的树将会在下一层进行分叉，它会跳转到第一层如果全部都分完了。最有效的去建立k-d tree的方法是使用一个分割的方法，就像快速排序。你可以在你的左子树和右子树上重复这一过程，直到最后一个你要去分割的树只有一个元素。

2维的k-d树

下面是一个最近邻搜索的工作

 

 

代码

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

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

int
main (int argc, char** argv)
{
  
  srand (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);
  }

  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

  kdtree.setInputCloud (cloud);

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

  // K nearest neighbor search

  int K = 10;

  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

  std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            <<