PCL：k-d tree 1 讲解

1.简介

kd-tree简称k维树，是一种空间划分的数据结构。常被用于高维空间中的搜索，比如范围搜索和最近邻搜索。kd-tree是二进制空间划分树的一种特殊情况。（在激光雷达SLAM中，一般使用的是三维点云。所以，kd-tree的维度是3）

由于三维点云的数目一般都比较大，所以，使用kd-tree来进行检索，可以减少很多的时间消耗，可以确保点云的关联点寻找和配准处于实时的状态。

2.原理

2.1数据结构

kd-tree，是k维的二叉树。其中的每一个节点都是k维的数据，数据结构如下所示：

struct kdtree{Node-data - 数据矢量   数据集中某个数据点，是n维矢量（这里也就是k维）Range     - 空间矢量   该节点所代表的空间范围split     - 整数       垂直于分割超平面的方向轴序号Left      - kd树       由位于该节点分割超平面左子空间内所有数据点所构成的k-d树Right     - kd树       由位于该节点分割超平面右子空间内所有数据点所构成的k-d树parent    - kd树       父节点
}

上面的数据在进行算法解析中，并不是全部都会用到。一般情况下，会用到的数据是{数据矢量，切割轴号，左支节点，右支节点}。这些数据就已经满足kd-tree的构建和检索了。

2.2 构建K-d tree

kd-tree的构建就是按照某种顺序将无序化的点云进行有序化排列，方便进行快捷高效的检索。构建算法如下：

Input:  无序化的点云，维度k
Output：点云对应的kd-tree
Algorithm：
1、初始化分割轴：对每个维度的数据进行方差的计算，取最大方差的维度作为分割轴，标记为r；
2、确定节点：对当前数据按分割轴维度进行检索，找到中位数数据，并将其放入到当前节点上；
3、划分双支：划分左支：在当前分割轴维度，所有小于中位数的值划分到左支中；划分右支：在当前分割轴维度，所有大于等于中位数的值划分到右支中。
4、更新分割轴：r = (r + 1) % k;
5、确定子节点：确定左节点：在左支的数据中进行步骤2；确定右节点：在右支的数据中进行步骤2；

这样的化，就可以构建出一颗完整的kd-tree了。（???更新分割轴r=(r+1)%k原理是啥？？？）

拿个例子说明为：二维样例：{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}

构建步骤：

1、初始化分割轴：

发现x轴的方差较大，所以，最开始的分割轴为x轴。

2、确定当前节点：

对{2，5，9，4，8，7}找中位数，发现{5,7}都可以，这里我们选择7，也就是(7,2);

3、划分双支数据：

在x轴维度上，比较和7的大小，进行划分：

左支：{(2,3)，(5,4)，(4,7)}

右支：{(9,6)，(8,1)}

4、更新分割轴：

一共就两个维度，所以，下一个维度是y轴。

5、确定子节点：

左节点：在左支中找到y轴的中位数(5,4)，左支数据更新为{(2,3)}，右支数据更新为{(4,7)}

右节点：在右支中找到y轴的中位数(9,6)，左支数据更新为{(8,1)}，右支数据为null。

6、更新分割轴：

下一个维度为x轴。

7、确定(5,4)的子节点：

左节点：由于只有一个数据，所以，左节点为(2,3)

右节点：由于只有一个数据，所以，右节点为(4,7)

8、确定(9,6)的子节点：

左节点：由于只有一个数据，所以，左节点为(8,1)

右节点：右节点为空。

最终，就可以构建整个的kd-tree了。

示意图如下所示：

二维空间表示：(二维坐标系下的分割示意图，如左图) kd-tree表示：（如右图）

2.3 最近邻搜索

在构建了完整的kd-tree之后，我们想要使用他来进行高维空间的检索。所以，这里讲解一下比较常用的最近邻检索，其中范围检索也是同样的道理。

最近邻搜索，其实和之前我们曾经学习过的KNN很像。不过，在激光点云章，如果使用常规的KNN算法的话，时间复杂度会空前高涨。因此，为了减少时间消耗，在工程上，一般使用kd-tree进行最近邻检索。

由于kd-tree已经按照维度进行划分了，所以，我们在进行比较的时候，也可以通过维度进行比较，来快速定位到与其最接近的点。由于可能会进行多个最近邻点的检索，所以，算法也可能会发生变化。因此，我将从最简单的一个最近邻开始说起。

一个最近邻

一个最近邻其实很简单，我们只需要定位到对应的分支上，找到最接近的点就可以了。

举个例子：查找(2.1,3.1)的最近邻。

计算当前节点(7,2)的距离，为6.23，并且暂定为(7,2)，根据当前分割轴的维度（2.1 < 7），选取左支。
计算当前节点(5,4)的距离，为3.03，由于3.03 > 6.23，暂定为(5,4)，根据当前分割轴维度（3.1 < 4），选取左支。
计算当前节点(2,3)的距离，为0.14，由于0.14 < 3.03，暂定为(2,3)，根据当前分割轴维度（2.1 > 2），选取右支，而右支为空，回溯上一个节点。
计算(2.1,3.1)与(5,4)的分割轴{y = 4}的距离，（为什么要算与分割轴的距离啊？？？）如果0.14小于距离值，说明就是最近值。如果大于距离值，说明，还有可能存在值与(2.1,3.1)最近，需要往右支检索。

由于0.14 < 0.9，（此处为什么用与分割轴的距离作比较？？？？，因为如果与分割轴的距离大于现有距离，那么分割轴另一侧分支的所有与它的距离更大，就不用比较了）我们找到了最近邻的值为(2,3)，最近距离为0.14。

多个最近邻

多个近邻其实和一个最近邻类似，不过是存储区间变为了多个，判定方法还是完全一样。

参考文章：

3、常用函数

kd-tree在日常使用中，一般会在两个方面使用：

最近邻搜索
距离范围搜索

距离范围搜索的原理和最近邻搜索的差不多，把满足距离的全部放进去就可以了。

最近邻搜索的函数在激光点云匹配中找最近点的时候用的比较多：

//头文件
#include <pcl/kdtree/kdtree_flann.h>
//设定kd-tree的智能指针
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
//输入三维点云，构建kd-tree
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
//在点云中寻找点searchPoint的k近邻的值，返回下标pointSearchInd和距离pointSearchSqDis
kdtreeCornerLast->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);

其中，当k为1的时候，就是最近邻搜索。当k大于1的时候，就是多个最近邻搜索。

距离范围搜索：

//在点云中寻找和点searchPoint满足radius距离的点和距离，返回下标pointIdxRadiusSearch和距离pointRadiusSquaredDistance
kdtreeCornerLast->radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

完整代码：

#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>);// Generate pointcloud datacloud->width = 1000;cloud->height = 1;cloud->points.resize (cloud->width * cloud->height);for (std::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 nearest neighbor searchint 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 (std::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;}// Neighbors within radius searchstd::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 (std::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;
}

PCL：k-d tree 1 讲解相关推荐

PCL ：K-d tree 2 结构理解
K-d tree 基础思路:(先看之前的KNN思想,更容易理解) 导语:kd 树是一种二叉树数据结构,可以用来进行高效的 kNN 计算.kd 树算法偏于复杂,本篇将先介绍以二叉树的形式来记录和索引空间 ...
ICPC 南昌现场赛 K：Tree（dsu on tree + 动态开点线段树）
Tree 让我们找满足一下五个条件的(x,y(x, y(x,y)点对有多少: x≠yx \neq yx=y xxx不是yyy的祖先 yyy不是xxx的祖先 dis(x,y)≤kdis(x, y)\ ...
KD Tree原理讲解
1.引子在一张地图上,有600多个单位,每个单位之间都需要独立寻路,检测碰撞和寻找最近的敌方目标.当这一切需要在手机上流畅运行并尽可能快的在服务器进行模拟时,最简单的平方算法O(N^2)已经不能满足 ...
KD tree （K dimensional tree）多维空间搜索近邻
kd树(k-dimensional树的简称),是一种分割k维数据空间的数据结构.主要应用于多维空间关键数据的搜索(如:范围搜索和最近邻搜索).
《机器学习实战》萌新入门笔记 ① — — K近邻算法趣味讲解和书本实例详细注释后代码
开坑前言:大一在读新生开启了自己的机器学习之旅,从接触到现在已经有快两个月了,现在回过头为夯实基础,从开始看起<机器学习实战>这本书,即使是最简单的KNN算法,详细看过作者用python实 ...
PCL：超详细的基于法向量和曲率的区域生长算法原理以及源码解读
---------原理介绍: (1)首先计算出来各点的曲率值,将曲率值按照从小到大的顺序进行排序. (2)设置一空的种子点序列和一个空的聚类数组. (3)选取曲率最小的点放入上述种子点序列中. (4) ...
PCL：从法线计算到曲率计算并可视化
----------------法线求解原理-------------- 表面法线是几何体表面的重要属性,在很多领域都有大量应用,例如:在进行光照渲染时产生符合可视习惯的效果时需要表面法线信息才能正常 ...
PCL：点云数据基于法线的边界提取(从最初的法线估计理论推导到最终的边界提取)
该边界提取采用PCL库里边的方法,基于法线估计来实现的边界检测与提取: 首先从原始点云上计算出法线,再由法线结合数据估计出边界.(这样解释还是特别抽像吧) ------------法线求解:(平面的法 ...
Pcl：Normal的定义结构及输出
1. pcl::Normal在pcl官网中的定义 /*brief A point structure representing normal coordinates and the surface ...

PCL：k-d tree 1 讲解

3、常用函数

PCL：k-d tree 1 讲解相关推荐

最新文章

热门文章