KDTree

最新推荐文章于 2022-05-15 08:51:41 发布

梨鱼

最新推荐文章于 2022-05-15 08:51:41 发布

阅读量270

点赞数 1

分类专栏： PCL

本文链接：https://blog.csdn.net/xuxiaoxx/article/details/119763877

版权

PCL 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

PCL之KDTree理解心得

参考相关博客：https://www.yuque.com/huangzhongqing/pcl/ch23sn
https://zhuanlan.zhihu.com/p/23966698
https://www.cnblogs.com/eyeszjwang/articles/2429382.html

从特征点匹配讲起

图像的配准在于比较或者融合。对于一个对象在不同角度和环境下所获得的图像，因为扫描等影响，不可能一次性对对象进行完整3D数据的获取，因此需要从不同角度获取的数据进行拼接，最终得到完整的3D模型，即此过程则叫做特征点匹配。
因此可以通过一些距离函数在高维矢量之间进行相似性检索，从而实现特征点的匹配。常见的有高维空间索引结构和近似查询算法。而k-d树就是其中一种。

KDTree介绍

kDTree是一种类似于平衡二叉树的数据结构，因此先一一个小例子进行模型引入介绍（见dalao中的例子说明）。
注意： KDTree每一个节点的划分过程不是按照奇偶轮回来决定是哪一条轴的，而是按照各个维度上的方差（variance）来决定下一轮按哪一条轴进行划分。链接中的例子按照次数来决定不是很符合规矩。

KDTree构造的解析

首先计算所有维度对应的方差的大小；
然后取方差最大的维度作为分隔点的依据（当选择方差越大的维度，证明数据在这个维度上越分散，可以很好避免数据扎堆）；
接着按照改维度进行数值大小排序，取中位数作为分隔的根节点。大于中位数的点处于右边，小于中位数的点分到左边；
循环步骤1、2和3，直到某个节点的左右子树只剩下一个或者没有节点为止。
因此根据上面的算法描述，可以知道可利用递归进行树的建立。

KDTree的KNN算法的解析

首先根据给定的所要搜索的点（下面简称为tar）以及建立好的KDTree，然后进行搜索，创建一个数组（下面简称为path）在搜索的过程中记录下搜索经过的节点，搜索直到叶子节点为止。
接着计算叶子节点与tar的距离（下面简称最大距离为Maxdis），并将其存放在用来存放K邻近节点的数组中（下面简称为Ktar）。
然后由该叶子节点进行回溯，找到改叶子节点的父节点，计算tar与父节点的距离。
假如此时Ktar还没满，则直接放入Ktar中，并对Maxdis距离进行更新；
假如此时Ktar已经满了，比较tar到父节点和Maxdis的大小，假如Maxdis大于tar到父节点的距离，则将Ktar对应的节点与该父节点进行置换，并且对Maxdis重新进行更新。
比较tar到父节点分割线的垂直距离，假如该距离小于Maxdis，则进行父节点的另外一片子空间进行搜索，否则继续进行回溯。
注意： 这里可以这么理解，一个节点相当于一个大的分界线，当点到线的距离小于现有阶段空间点到点的距离，便会产生一个问题：是否另外一片区域存在更优的点，使得tar到该点的距离更小？因此需要对另外一片空间中的所有节点进行排查，即进入另外一片空间进行搜索。
重复步骤2、3和4，直到path中无节点为止。此时Ktar中存储的节点即为K邻近的节点。

KDTree代码

PCL文档中的代码

PCL中类pcl::KdTree是kd-tree数据结构的实现。并且提供基于FLANN进行快速搜索的一些相关子类与包装类。具体可以参考相应的API。下面给出2个类的具体用法：

pcl::search::KdTree < PointT >
pcl::search::KdTree是pcl::search::Search< PointT >的子类，是pcl::KdTree的包装类。包含(1) k 近邻搜索；(2) 邻域半径搜索。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h> // 包含kdtree头文件
typedef pcl::PointXYZ PointT;
int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("read.pcd", *cloud);
	// 定义KDTree对象
	pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
	kdtree->setInputCloud(cloud); // 设置要搜索的点云，建立KDTree
	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方
	PointT point = cloud->points[0]; // 初始化一个查询点
	
	// 查询距point最近的k个点
	int k = 10;
	int size = kdtree->nearestKSearch(point, k, indices, distances);
	std::cout << "search point : " << size << std::endl;
	// 查询point半径为radius邻域球内的点
	double radius = 2.0;
	size = kdtree->radiusSearch(point, radius, indices, distances);
	std::cout << "search point : " << size << std::endl;
	system("pause");
	return 0;
}

注意： 搜索结果默认是按照距离point点的距离从近到远排序；如果InputCloud中含有point点，搜索结果的的第一个点是point本身。

pcl::KdTreeFLANN < PointT >
pcl::KdTreeFLANN是pcl::KdTree的子类，可以实现同样的功能。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 包含相关头文件
#include <pcl/kdtree/kdtree_flann.h>
typedef pcl::PointXYZ PointT;
int main()
{
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::io::loadPCDFile("read.pcd", *cloud);
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KDtree
	kdtree.setInputCloud(cloud); // 设置要搜索的点云，建立KDTree
	std::vector<int> indices; // 存储查询近邻点索引
	std::vector<float> distances; // 存储近邻点对应距离的平方
	PointT point = cloud->points[0]; // 初始化一个查询点
	
	// 查询距point最近的k个点
	int k = 10;
	int size = kdtree.nearestKSearch(point, k, indices, distances);
	std::cout << "search point : " << size << std::endl;
	// 查询point半径为radius邻域球内的点
	double radius = 2.0;
	size = kdtree.radiusSearch(point, radius, indices, distances);
	std::cout << "search point : " << size << std::endl;
	system("pause");
	return 0;
}