[PCL教程] PCL漫游之KdTree,OcTree，RANSAC

7 KD树 KdTree

7.1 基本原理

KD树，在k维空间中组织点云的数据结构，是一种二叉搜索树（对于3维点云，k=3）。可用于最近邻的搜索。KD树包括构建阶段和搜索阶段。
参考：KD树简介

• 构建阶段：即对空间的切分，每轮切分按照一定原则先后切分k个维度。切分域(维度)的先后顺序可以根据方差（空间点的分布）确定，切分点选择中间点即可。

切分域的选择不一定用方差方式，可以简单的d mod n维度方式，d是树节点深度，n代表维度数量。该方法简单，效果也不错。

• 搜索阶段：1）寻找目标数据的近似最近点，即根据目标数据从根节点开始搜索kd树，找到对应的叶子节点作为近似最近点。2）回溯，沿着搜索路径回溯，以目标数据和近似最近点的距离作为判断依据，看看有无更近的点。

7.2 代码

//头文件
#include <pcl/kdtree/kdtree_flann.h>
...
// 定义KdTree对象
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// 设置输入
kdtree.setInputCloud (cloud);
// 搜索点
pcl::PointXYZ searchPoint;
// 近邻点数目
int K = 10;
// 近邻点搜索和距离
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
// 搜索K近邻 （nearestKSearch），若多于0个，打印出来
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