KD树学习

KD树

本质上就是一个二叉树，一种储存方式，能够快速查找邻近点。
理论讲解可以参考这个：

https://www.yuque.com/huangzhongqing/pcl/ch23sn

1. 从官网摘的代码，自己打一遍，去了解下怎么用

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

  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 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;

  if ( kdtree.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 ( kdtree.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;
  }


  return 0;
}

https://pcl.readthedocs.io/projects/tutorials/en/latest/kdtree_search.html#kdtree-search
KD树

俩种查找方式：

● 一种是范围查询：范围查询就是给定查询点和查询距离的阈值，从数据集中找出所有与查询点距离小于阈值的数据；
● 另一种是K近邻查询：K近邻查询是给定查询点及正整数K，从数据集中找到距离查询点最近的K个数据，当K=1时，就是最近邻查询。

KD树这个存储方式很常用，查找邻近点很快。

点云文件来查找点

pcl::search::KdTree是pcl::search::Search< PointT >的子类，是pcl::KdTree的包装类。包含(1) k 近邻搜索；(2) 邻域半径搜索。
需要读取文件，用其API就可以

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h> // 包含kdtree头文件
typedef pcl::PointXYZ PointT;
int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("read.pcd", *cloud);
	// 定义KDTree对象
	pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
	kdtree->setInputCloud(cloud); // 设置要搜索的点云，建立KDTree
	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方
	PointT point = cloud->points[0]; // 初始化一个查询点
	
	// 查询距point最近的k个点
	int k = 10;
	int size = kdtree->nearestKSearch(point, k, indices, distances);
	std::cout << "search point : " << size << std::endl;
	// 查询point半径为radius邻域球内的点
	double radius = 2.0;
	size = kdtree->radiusSearch(point, radius, indices, distances);
	std::cout << "search point : " << size << std::endl;
	system("pause");
	return 0;
}

pcl::KdTreeFLANN是pcl::KdTree的子类，可以实现同样的功能。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 包含相关头文件
#include <pcl/kdtree/kdtree_flann.h>
typedef pcl::PointXYZ PointT;
int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("read.pcd", *cloud);
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KDtree
	kdtree.setInputCloud(cloud); // 设置要搜索的点云，建立KDTree
	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方
	PointT point = cloud->points[0]; // 初始化一个查询点
	
	// 查询距point最近的k个点
	int k = 10;
	int size = kdtree.nearestKSearch(point, k, indices, distances);
	std::cout << "search point : " << size << std::endl;
	// 查询point半径为radius邻域球内的点
	double radius = 2.0;
	size = kdtree.radiusSearch(point, radius, indices, distances);
	std::cout << "search point : " << size << std::endl;
	system("pause");
	return 0;
}

代码来源： https://www.yuque.com/huangzhongqing/pcl/xr8761