一、原理：KD树

    kd-tree 数据结构是计算机科学中⽤来组织具有k维空间中若⼲点的数据结构。它是⼀个具有其他约束 的⼆进位搜索树。K-d树对于范围搜索和最近邻搜索是⾮常有⽤的。为了我们的⽬的，我们通常只处理三  维点云，所以我们所有的k-d树都是三维的。K-d树的每⼀层都使⽤垂直于相应轴的超平⾯，沿着特定的维 度分割所有的⼦级。在树的根部，所有的⼦节点都将根据第⼀维(即，如果第⼀维坐标⼩于根，它将在左⼦ 树中，如果它⼤于根，那么它将明显地在右⼦树中)分割。树中的每⼀层都在下⼀个维度上划分，⼀旦所有 其他维度都被耗尽，就返回到第⼀个维度。构建k-d树最有效的⽅法是使⽤⼀个分区⽅法：将中间点放置在 根部，所有的东⻄都有⼀个更⼩的⼀维值，左边更⼩，右边更⼤。然后，在左树和右⼦树上重复此过程， 直到要分区的最后⼀棵树仅由⼀个元素组成。 如图 1是 k=2 的⼆维数据搜索示意图，kd-tree 的每⼀层为数据的⼀个维度，并以⼀个⼦节点来拆分 该维数据区间，取这⼀维度内⼩于该节点值的数据放在左⼦数，反之⼤于节点值的放在右⼦树。kd-tree 的每⼀层都是在数据的下⼀个维度分开，当数据的维度⽤完时，则重新返回到第⼀个维度继续迭代直⾄确 定所查找的数据，该⽅法使得时间复杂度降低到 。创建 kd-tree常将数据值每个维度的中值作为 分割超平⾯。

二、具体功能：

1、K近邻搜索 nearestKSearch函数

1 int pcl::KdTreeFLANN< PointT, Dist >::nearestKSearch ( const Poin tT & point,
2 unsigned int k,
3 Indices & k_indices,
4 std::vector< float > & k_sqr_distances
5 ) const

    point ：⼀个给定的有效(即有限)查询点 
    k ：要搜索的邻居数量 
    k_indices ：相邻点的合成指数(必须被预先调整为k !) 
    k_sqr_distances ：到相邻点的平⽅距离(必须预先调整为k !)

2、K半径搜索 radiusSearch函数

1 int pcl::KdTreeFLANN< PointT, Dist >::radiusSearch ( const PointT & point, 
2 double radius, 
3 Indices & k_indices, 
4 std::vector< float > & k_sqr_distances,
5 unsigned int max_nn = 0 
6 ) const 

     point ：⼀个给定的有效(即有限)查询点 
     radius ：包围p_q所有邻居的球⾯半径 
     k_indices ：相邻点的合成指数 
     k_sqr_distances ：到相邻点的平⽅距离
     max_nn ：如果给定，则返回半径范围内搜索到的邻居的个数限定为这个值。如果max_nn设置为0或 ⼤于输⼊云中的点数，则返回半径内的所有邻居。

简单示例

三、官网示例：

1、K近邻搜索 nearestKSearch函数

1 #include <iostream> 
2 #include <vector> 
3 #include <pcl/kdtree/kdtree_flann.h> //kdtree近邻搜索 
4 #include <pcl/io/pcd_io.h> //⽂件输⼊输出 
5 #include <pcl/point_types.h> //点类型相关定义 
6 #include <pcl/visualization/pcl_visualizer.h>//可视化相关定义 
7 #include <boost/thread/thread.hpp> 
8 using namespace std; 
9
10 int main() 
11 { 
12 //读取点云数据 
13 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCl oud<pcl::PointXYZRGB>); 
14 if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("Road - Cloud.pcd" , *cloud) == -1) 
15 { 
16 PCL_ERROR("Cloudn't read file!"); 
17 return -1; 
18 } 
19
20 //建⽴kd-tree 
21 pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree; //建⽴kdtree对象 
22 kdtree.setInputCloud(cloud); //设置需要建⽴kdtree的点云指针 
23
24 //K近邻搜索 
25 pcl::PointXYZRGB searchPoint = cloud->points[1000]; //设置查找点 
26 cloud->points[1000].r = 0;//查询点着⾊（绿⾊） 
27 cloud->points[1000].g = 255; 
28 cloud->points[1000].b = 0; 
29 int K = 100; //设置需要查找的近邻点个数 
30 vector<int> pointIdxNKNSearch(K); //保存每个近邻点的索引 
31 vector<float> pointNKNSquaredDistance(K); //保存每个近邻点与查找 点之间的欧式距离平⽅ 
32
33 cout << "K nearest neighbor search at (" << searchPoint.x 
34 << " " << searchPoint.y 
35 << " " << searchPoint.z 
36 << ") with K=" << K << endl; 
37
38 if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) 
39 { 
40 for (size_t i = 1; i < pointIdxNKNSearch.size(); ++i){ // i=1时不包含被查询点本身，i=0时包含被查询点 
41 cloud->points[pointIdxNKNSearch[i]].r = 255;//查询点邻 域内的点着⾊ 
42 cloud->points[pointIdxNKNSearch[i]].g = 0; 
43 cloud->points[pointIdxNKNSearch[i]].b = 0; 
44 } 
45 } 
46 cout << "K = 100近邻点个数：" << pointIdxNKNSearch.size() << en dl; 
47 //可视化 
48 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(n ew pcl::visualization::PCLVisualizer("3D Viewer")); 
49 pcl::visualization::PointCloudColorHandlerRGBField<pcl::Point XYZRGB> rgb(cloud); 
50 viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "sample cloud" ); 
51 viewer->setPointCloudRenderingProperties(pcl::visualization:: PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); // 设置点云⼤⼩ 
52 while (!viewer->wasStopped()) 
53 { 
54 viewer->spinOnce(100); 
55 boost::this_thread::sleep(boost::posix_time::microseconds(10000)); 
56 } 
57
58 return 0; 
59 }

2、K半径搜索 radiusSearch函数

1 #include <iostream> 
2 #include <vector> 
3 #include <pcl/kdtree/kdtree_flann.h> //kdtree近邻搜索 
4 #include <pcl/io/pcd_io.h> 
5 #include <pcl/point_types.h> 
6 #include <pcl/visualization/pcl_visualizer.h>//可视化相关定义 
7
8 using namespace std; 
9
10 int main() 
11 { 
12 //读取点云 
13 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCl oud<pcl::PointXYZRGB>); 
14 if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("Road - Cloud.pcd" , *cloud) == -1) 
15 { 
16 PCL_ERROR("Cloudn't read file!"); 
17 return -1; 
18 } 
19
20 //建⽴kd-tree 
21 pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree; //建⽴kdtree对象 
22 kdtree.setInputCloud(cloud); //设置需要建⽴kdtree的点云指针 
23 //radius半径搜索 
24 pcl::PointXYZRGB searchPoint = cloud->points[1000]; //设置查 找点 
25 cloud->points[1000].r = 0; 
26 cloud->points[1000].g = 255;
27 cloud->points[1000].b = 0; 
28 vector<int> pointIdxRadiusSearch; //保存每个近邻点的索引 
29 vector<float> pointRadiusSquaredDistance; //保存每个近邻点与查找 点之间的欧式距离平⽅ 
30 float radius = 3; //设置查找半径范围 
31 cout << "Neighbors within radius search at (" << searchPoint. x 
32 << " " << searchPoint.y 
33 << " " << searchPoint.z 
34 << ") with radius=" << radius << endl; 
35
36 if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSe arch, pointRadiusSquaredDistance) > 0) 
37 { 
38 for (size_t i = 1; i < pointIdxRadiusSearch.size(); ++i){ 
39 cloud->points[pointIdxRadiusSearch[i]].r = 255; 
40 cloud->points[pointIdxRadiusSearch[i]].g = 0; 
41 cloud->points[pointIdxRadiusSearch[i]].b = 0; 
42 } 
43 } 
44 cout << "半径3近邻点个数： " << pointIdxRadiusSearch.size() << e ndl; 
45 //可视化 
46 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(n ew pcl::visualization::PCLVisualizer("3D Viewer")); 
47 pcl::visualization::PointCloudColorHandlerRGBField<pcl::Point XYZRGB> rgb(cloud); 
48 viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "sample cloud" ); 
49 viewer->setPointCloudRenderingProperties(pcl::visualization:: PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); // 设置点云⼤⼩ 
50 while (!viewer->wasStopped()) 
51 { 
52 viewer->spinOnce(100);
53 boost::this_thread::sleep(boost::posix_time::microseconds (10000)); 
54 } 
55
56 return 0; 
57 }

三、结果