PCL中使用KdTree在点云中进行K近邻及半径查询
KdTree背景知识
KdTree(也称k-d树)是一种用来分割k维数据空间的高维空间索引结构，其本质就是一个带约束的二叉搜索树，基于KdTree的近似查询的算法可以快速而准确地找到查询点的近邻，经常应用于特征点匹配中的相似性算法。
而索引结构中相似性查询算法有两种基本的方式：一种是范围查询（radius searches），另一种是K近邻查询（K-neighbor searches）。范围查询就是给定查询点和查询距离的阈值（以查询点为圆心，查询距离为半径），从数据集中找出所有与查询点距离小于阈值的数据（半径内的数据）；K近邻查询是给定查询点及正整数K，从数据集中找到距离查询点最近的K个数据，当K=1时，就是最近邻查询（nearest neighbor searches）
对于三维的点云来说，所有的K-D树都是三维的，构建k-d树是一个逐级展开的递归过程，在每一级展开时都使用垂直于相应轴的超平面沿特定维度分割所有剩下数据集。在KdTree的根节点上，所有数据都将根据第一个维度进行拆分（如果第一个维度坐标小于根节点数据，则子数据将位于左子树中，如果子数据大于根节点数据，则子元素显然位于右子树中）。KdTree中的下一层在下一个维度上进行划分，当所有其他维度都用尽后，将返回到第一个维度。构建K-D树最有效的方法是使用一种类似于快速排序的分区方法，将中间点放在根节点上，然后比中间点小的数值放在左子树上，比中间点大的数值放在右子树上，最后在左右子树上重复此过程，直到分割至最后一个元素。
PCL中KdTree的使用
PCL 中使用KdTree需要加入头文件：
#include 创建KdTree对象并设置输入点云：
pcl::KdTreeFLANN kdtree;
kdtree.setInputCloud (cloud); K近邻搜索：
初始化K值并新建两个数组存储搜索后得到的K紧邻结果，kdtree调用nearestKSearch即可得到K个最紧邻点的索引和平方距离。
// K nearest neighbor search
int K = 10;
std::vector pointIdxNKNSearch(K);
std::vector pointNKNSquaredDistance(K);
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
<< " " << cloud->points[ pointIdxNKNSearch[i] ].y
<< " " << cloud->points[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
} 特定半径范围内查询（Radius Search）：
新建两个数组存储搜索得到半径R范围内结果，然后调用radiusSearch得到半径R内所有点的索引和平方距离。
// Neighbors within radius search
std::vector pointIdxRadiusSearch;
std::vector pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
} 编译并运行程序
开发环境： VS2015 & PCL1.8.1
工程配置PCL头文件及库
Include:
$(PCL_ROOT);
$(PCL_ROOT)\include\pcl-1.8;
$(PCL_ROOT)\3rdParty\Eigen\eigen3;
$(PCL_ROOT)\3rdParty\FLANN\include;
$(PCL_ROOT)\3rdParty\VTK\include\vtk-8.0;
$(PCL_ROOT)\3rdParty\Boost\include\boost-1_64;
lib:
$(PCL_ROOT)\lib\*.lib
$(PCL_ROOT)\3rdParty\VTK\lib\*.lib
$(PCL_ROOT)\3rdParty\FLANN\lib\*.lib
$(PCL_ROOT)\3rdParty\Boost\lib\*.lib相关工程与程序可在我的Github: https://github.com/yazhouzheng/KdTreeInPCL 找到。
编译运行程序可的如下结果：
D:\asher\project\KdTreeInPCL\bin>KdTreeInPCL.exe
K nearest neighbor search at (35.3125 465.594 237.813) with K=10
45.0625 433.031 234.906 (squared distance: 1163.83)
72.5313 437.719 211.656 (squared distance: 2846.4)
48.5 514.344 210 (squared distance: 3324.01)
20.25 501.813 281.719 (squared distance: 3466.44)
4.90625 468.781 303.813 (squared distance: 5290.7)
27 543.656 202.656 (squared distance: 7398.81)
110.031 515.625 272.188 (squared distance: 9267.66)
133.438 411.313 204.438 (squared distance: 13688.9)
99.4063 375.813 189.219 (squared distance: 14530)
87.9688 573.438 220.438 (squared distance: 14704.8)
Neighbors within radius search at (35.3125 465.594 237.813) with radius=128.359
45.0625 433.031 234.906 (squared distance: 1163.83)
72.5313 437.719 211.656 (squared distance: 2846.4)
48.5 514.344 210 (squared distance: 3324.01)
20.25 501.813 281.719 (squared distance: 3466.44)
4.90625 468.781 303.813 (squared distance: 5290.7)
27 543.656 202.656 (squared distance: 7398.81)
110.031 515.625 272.188 (squared distance: 9267.66)
133.438 411.313 204.438 (squared distance: 13688.9)
99.4063 375.813 189.219 (squared distance: 14530)
87.9688 573.438 220.438 (squared distance: 14704.8)
150.969 479.719 200.313 (squared distance: 14982.1)References
1 二维KdTree 的构造及查找算法介绍： https://baike.baidu.com/item/kd-tree/2302515
2 PCL中使用KdTree 搜索：http://www.pointclouds.org/documentation/tutorials/kdtree_search.php#kdtree-search