沉默杀手HomeArchivesTagsCategories
KD树和最近邻搜索
|PCL点云|
Word count: 933|Reading time: 3 min

kD树的构建步骤：

  1. 在K维数据集合中选择具有最大方差的维度k，然后在该维度上选择中值为pivot对该数据集合进行划分，得到两个子集合；同时创建一个树结点node，用于存储

  2. 对两个子集合重复（1）步骤的过程，直至所有子集合都不能再划分为止；

  • 选择数据方差大的维度，是因为沿该坐标轴方向上数据点分散的比较开；另外可以减少最近邻搜索时的回溯代价，减少子树的访问

  • 对于n个实例的k维数据来说，建立kd-tree的时间复杂度为 ， 这有时会比较复杂

最近邻搜索

算法的时间复杂度是
举例如下：
KD树构建
坐标图
搜索过程

  • 不会去搜索根节点的另一颗子树
  • 不需要计算每一个节点的欧式距离，而多数情况只需要计算点到超平面的距离就可以了。
  • KD树在维度较小时（比如20、30），算法的查找效率很高，然而当数据维度增大（例如：K≥100），查找效率会随着维度的增加而迅速下降。假设数据集的维数为 D，一般来说要求数据的规模 N 满足 ，才能达到高效的搜索，否则大部分的点都会被查询。

PCL中的KD树

nearestKSearch参数

1
2
3
4
5
6
void pcl::search::Search< PointT >::nearestKSearch(
								const PointCloud & 	cloud,
								int 	k,
								std::vector< Indices > & 	k_indices,
								std::vector< std::vector< float > > & 	k_sqr_distances 
) const

对给定的点，搜索 k-nearest neighbors

参数：

  • [in] cloud the point cloud data
  • [in] k the number of neighbors to search for
  • [out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
  • [out] 查询到的点的距离平方

这个函数的返回值是 查询到的个数。

PCL中的KD-Tree一般适用于三维特征点。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
#include <pcl/kdtree/kdtree_flann.h>  //kdtree近邻搜索
#include <pcl/io/pcd_io.h>  //文件输入输出
#include <pcl/point_types.h>  //点类型相关定义
#include <pcl/visualization/cloud_viewer.h>  //点类型相关定义

#include <iostream>
#include <vector>

int main()
{
	// 读取点云
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("rabbit.pcd", *cloud) == -1)
	{
		PCL_ERROR("Cloudn't read file!");
		return -1;
	}
	// 原始点云着色
	for (size_t i = 0; i < cloud->points.size(); ++i){
		cloud->points[i].r = 255;
		cloud->points[i].g = 255;
		cloud->points[i].b = 255;
	}

	//  建立kd-tree
	pcl::KdTreeFLANN<pcl::PointXYZRGB>  kdtree;  //建立kdtree对象
	kdtree.setInputCloud(cloud); //设置需要建立kdtree的点云指针

	//   K近邻搜索
	pcl::PointXYZRGB searchPoint = cloud->points[1000];   // 设置查找点
	int K = 900;  //设置需要查找的近邻点个数
	std::vector<int> pointIdxNKNSearch(K);  //  保存每个近邻点的索引
	std::vector<float> pointNKNSquaredDistance(K); // 保存每个近邻点与查找点之间的欧式距离平方

	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	{
		for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i){
			cloud->points[pointIdxNKNSearch[i]].r = 0;
			cloud->points[pointIdxNKNSearch[i]].g = 255;
			cloud->points[pointIdxNKNSearch[i]].b = 0;
		}		
	}
	std::cout << "K = 900近邻点个数：" << pointIdxNKNSearch.size() << endl;

	//   显示点云
	pcl::visualization::CloudViewer viewer("cloud viewer");
	viewer.showCloud(cloud);
	// 或者 pause()
	system("pause");
	return 0;
}

如果运行时出现报警 Failed to find match for field ‘rgb’ ，则把pcl::PointXYZRGBA>改为pcl::PointXYZ>

KD树和八叉树的比较

从搜索点数来看，当原点云与目标点云重叠区域较大时，优先选择nearestKSearch接口来寻找最近邻，否则优先使用radiusSearch接口来寻找最近邻。 PCL的ICP用的就是 nearestKSearch

参考：
点云数据通过pcl的kdtree搜索关键点某半径邻域内的区域