kd-tree

kd-tree 简单理解为一种数据结构（所以点云数据处理中最为核心的问题就是建立离散点间的拓扑关系，实现基于邻域关系的快速查找。）

k-d树 （k-dimensional树的简称），是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。K-D树是二进制空间分割树的特殊的情况。用来组织表示K维空间中点的几何，是一种带有其他约束的二分查找树，为了达到目的，通常只在三个维度中进行处理因此所有的kd_tree都将是三维的kd_tree,kd_tree的每一维在指定维度上分开所有的字节点，在树 的根部所有子节点是以第一个指定的维度上被分开。
k-d树算法可以分为两大部分，一部分是有关k-d树本身这种数据结构建立的算法，另一部分是在建立的k-d树上如何进行最邻近查找的算法。

#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 (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);
  }
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近邻搜索
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 (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;
  }
// 在半径r内搜索近邻
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 (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;
  }
return 0;
}

1.#include <pcl/kdtree/kdtree_flann.h>；k-d树的头文件。

2.pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;建立一个kd-tree对象（KdTreeFLANN是继承了kd-tree数据结构的具有3D检索功能的kd-tree子类）

3.kdtree.setInputCloud (cloud);设置输入点云为搜索空间（其实有2个参数，还有一个参数是点的索引）

4.pcl::PointXYZ searchPoint;定义查询点

5.std::vector<int>pointIdxNKNSearch(K);   储存查询点紧邻索引（变量的定义）
std::vector<float>pointNKNSquaredDistance(K);储存紧邻点对应距离的平方

6. kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) 主要是用来进行K邻域收缩，参数主要是  查询点  个数  索引  欧式距离。

 

同理是方法以某一半径搜索邻近点，主要函数为

7.kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)