构建KD-Tree
百度百科定义： kd-tree（k-dimensional树的简称），是一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。 [1] 主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。
其中K代表数据的维度，在激光雷达三维点云中，KD-Tree的维度是3。由于点云数据的数量一般较大，使用KD-Tree构建数据索引能够帮助我们快速的进行检索。
一个 kd-tree的节点的数据结构一般包括下述类型：
其中，常用的数据是： 数据适量，切割轴split，父节点，左支节点，右支节点。这些就能满足 kd-tree的构建与检索了。
- 伪代码
输入: 大量无序化的点云，维度3 输出：点云对应的kd-tree数据结构 构建过程： 1、初始化分割轴：对每个维度的数据进行方差的计算，取最大方差的维度作为分割轴（另外一种说法是随机选择一个维度）； 2、确定节点：对当前数据按分割轴维度进行检索，找到中位数数据，并将其放入到当前节点上； 3、划分双支： 划分左支：在当前分割轴维度，所有小于中位数的值划分到左支中； 划分右支：在当前分割轴维度，所有大于等于中位数的值划分到右支中。 4、更新分割轴：继续选择方差最大的轴作为分割轴（或者随机选择、或顺序选择）; 5、确定子节点： 确定左节点：在左支的数据中进行步骤2； 确定右节点：在右支的数据中进行步骤2；
为了更好的理解上述伪代码，我们以下述例子进行更加详细的说明：
- 二维样例：
{（1,1），（4,5），（2,9），（5,7），（6,1），（7,2），（9,3）}
- 构建步骤：
- 1）初始化分割轴：
这里为了简单，不计算方差，直接随机选择X轴作为分割轴。
2）确定当前节点：
对X轴中的{1,4,2,5,6,7,9}选取中位数为5，则选择**（5,7）** 点作为当前节点。
3）划定左右分支数据：
在分割轴X轴维度上，比较与中位数5的大小，进行划分：
左支：（1,1），（4,5），（2,9） 右支：（6,1），（7,2），（9,3）
4）更新左右分支的分割轴为y轴：
5）在左右分支中确定节点：
左支：y轴坐标的中位数为5，选取（4,5）作为子节点 右支：y轴坐标的中位数为2，选取（7,2）作为子节点
6）分别对子节点划分新的左支右支：
（4,5）节点：左支：（1,1），右支：（2,9） （7,2）节点：左支：（6,1），右支：（9,3）
最终，就构建完成了整个KD-Tree。
示意图如下：
在对应的二维空间表示：
最近邻搜索
假设，在构建kd-tree之后，我们想在上述的点集合中要找一个与（2,2）点最近的点。
计算与当前节点(5,7)的距离，为5.8，并且暂定为(5,7)，根据当前分割轴的维度（2 < 5），选取左支。
计算与左支节点(4,5)的距离，为3.6，小于5.8，暂定为(4,5)，根据当前分割轴的维度（2 < 5），选取左支。
计算与左支节点(1,1)的距离，为1.4，小于3.6，暂定为(1,1)，根据当前分割轴的维度（2 > 1），选取右支，而右支为空，回溯到上一节点。
计算（2,2）与(4,5)分割轴5之间的距离为3，3>1.4，说明当前节点(1,1)即为（2,2）点的最近邻。
距离范围搜索的原理和最近邻搜索的差不多，把满足距离的全部放进去就可以了。
PCL函数用法
kd-tree在PCL库的日常使用中，一般会在两个方面使用：
- 最近邻搜索
//头文件 #include <pcl/kdtree/kdtree_flann.h> //设定kd-tree的智能指针 pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>()); //输入三维点云，构建kd-tree kdtreeCornerLast->setInputCloud(laserCloudCornerLast); //在点云中寻找点searchPoint的k近邻的值，返回下标pointSearchInd和距离pointSearchSqDis // k的值为搜索最近邻点的个数 kdtreeCornerLast->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
- 距离范围搜索
//在点云中寻找和点searchPoint满足radius距离的点和距离，返回下标pointIdxRadiusSearch和距离pointRadiusSquaredDistance kdtreeCornerLast->radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
