PCL点云库学习笔记（搜索方法）

一、k-d tree

我们所有的k-d树都是三维空间的。 k-d树的每个级都使用特定维度拆分所有子级。 在树的根部，所有子项都将根据第一维进行拆分（即，如果第一维坐标小于根，则它将在左子树中；如果大于根，则显然会在 右边的子树）。 树中向下的每个级别都在下一个维度上划分，一旦所有其他级别都用尽后，将返回到第一个维度。 构造k-d树的最有效方法是使用一种分区方法，将中值点放置在根上，具有较小一维值在左子树，较大的值在右子树。 然后，在左右两个子树上都重复此过程，直到要分区的最后一棵树仅由一个元素组成。

理论部分见：link.
代码

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

  // 产生任意的点云数据
  cloud->width = 1000;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (std::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);
  }

 //创建kdtree对象，并将随机创建的云设置为输入。创建一个被分配随机坐标的searchPoint
  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;

//用于存储k个最近邻
  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

  std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;

//把KdTree计算的值和距离返回到储存点中，打印出随机搜索点的所有10个最近邻居的位置
  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDista