点云拓扑之k-d Tree
通过雷达，激光扫描，立体摄像机等三维测量设备获取的点云数据，具有数据量大，分布不均匀等特点，作为三维领域中一个重要的数据来源，点云主要是表征目标表面的海量点的集合，并不具备传统网格数据的几何拓扑信息，所以点云数据处理中最为核心的问题就是建立离散点间的拓扑关系，实现基于邻域关系的快速查找。
原理概述
k-d树 （k-dimensional树的简称），是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。K-D树是二进制空间分割树的特殊的情况。用来组织表示K维空间中点的几何，是一种带有其他约束的二分查找树，为了达到目的，通常只在三个维度中进行处理，因此所有的kd_tree都将是三维的kd_tree，kd_tree的每一维在指定维度上分开所有的子节点，在树的根部所有子节点是在第一个指定的维度上被分开。
k-d树算法可以分为两大部分，一部分是有关k-d树本身这种数据结构建立的算法，另一部分是在建立的k-d树上如何进行最邻近查找的算法。
构建算法
k-d树是一个二叉树，每个节点表示一个空间范围。下表给出的是k-d树每个节点中主要包含的数据结构。
| 域名 | 数据类型 | 描述 |
| Note-data | 数据矢量 | 数据集中某个数据点，是n维矢量（这里也就是k维） |
| Range | 空间矢量 | 该节点所代表的空间范围 |
| split | 整数 | 垂直于分割超平面的方向轴序号 |
| Left | k-d树 | 由位于该节点分割超平面左子空间内所有数据点所构成的k-d树 |
| Right | k-d树 | 由位于该节点分割超平面右子空间内所有数据点所构成的k-d树 |
| parent | k-d树 | 父节点 |
以一个简单直观的实例来介绍k-d树算法。假设有6个二维数据点{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}，数据点位于二维空间内。k-d树算法就是要确定图中这些分割空间的分割线（多维空间即为分割平面，一般为超平面）。下面就要一步步展示k-d树是如何确定这些分割线的。
由于此例简单，数据维度只有2维，所以可以简单地给x，y两个方向轴编号为0,1，也即split={0,1}。
（1）确定split域的首先该取的值。分别计算x，y方向上数据的方差得知x方向上的方差最大，所以split域值首先取0，也就是x轴方向；
（2）确定Node-data的域值。根据x轴方向的值2,5,9,4,8,7排序选出中值为7，所以Node-data = （7,2）。这样，该节点的分割超平面就是通过（7,2）并垂直于split = 0（x轴）的直线x = 7；
（3）确定左子空间和右子空间。分割超平面x = 7将整个空间分为两部分，如图2所示。x < = 7的部分为左子空间，包含3个节点{（2,3），（5,4），（4,7）}；另一部分为右子空间，包含2个节点{（9,6），（8,1）}。
（4）k-d树的构建是一个递归的过程。然后对左子空间和右子空间内的数据重复根节点的过程就可以得到下一级子节点（5,4）和（9,6）（也就是左右子空间的'根'节点），同时将空间和数据集进一步细分。如此反复直到空间中只包含一个数据点。最后生成的k-d树如图所示:
详解KDTree-CSDN博客KDTree详解：详解KDTree-CSDN博客
查找算法（最近邻搜索）
在k-d树中进行数据的查找也是特征匹配的重要环节，其目的是检索在k-d树中与查询点距离最近的数据点。星号表示要查询的点（2.1,3.1）。通过二叉搜索，顺着搜索路径很快就能找到最邻近的近似点，也就是叶子节点（2,3）。而找到的叶子节点并不一定就是最邻近的，最邻近肯定距离查询点更近，应该位于以查询点为圆心且通过叶子节点的圆域内。为了找到真正的最近邻，还需要进行'回溯'操作：算法沿搜索路径反向查找是否有距离查询点更近的数据点。此例中先从（7,2）点开始进行 二叉查找，然后到达（5,4），最后到达（2,3），此时搜索路径中的节点为<（7,2），（5,4），（2,3）>，首先以（2,3）作为 当前最近邻点，计算其到查询点（2.1,3.1）的距离为0.1414，然后回溯到其父节点（5,4），并判断在该父节点的其他子节点空间中是否有距离查询点更近的数据点。以（2.1,3.1）为圆心，以0.1414为半径画圆，如图4所示。发现该圆并不和超平面y = 4交割，因此不用进入（5,4）节点右子空间中去搜索。
KDTree函数详解：02KDTree · 语雀
应用实例
以下案例通过两种方式进行邻域搜索：
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv)
{
// 用系统时间初始化随机种子
srand(time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 生成点云数据1000个
cloud->width = 1000;
cloud->height = 1; // 1 表示点云为无序点云
cloud->points.resize(cloud->width * cloud->height);
// 给点云填充数据 0 - 1023
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);
}
// 创建KdTree的实现类KdTreeFLANN (近似近邻快速库)
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// pcl::search::KdTree<pcl::PointXYZ> kdtree;
// 设置搜索空间，把cloud作为输入
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个最近邻
// 创建K和两个向量来保存搜索到的数据
// K = 10 表示搜索10个临近点
// pointIdxNKNSearch 保存搜索到的临近点的索引
// pointNKNSquaredDistance 保存对应临近点的距离的平方
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
<< " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// 方式二：通过指定半径搜索
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// 创建一个随机[0,256)的半径值
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
<< " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
//可视化
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.0, 0.0, 0.5);
viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
// 添加从原点到搜索点的线段
viewer.addLine(originPoint, searchPoint);
// 添加一个以搜索点为圆心，搜索半径为半径的球体
viewer.addSphere(searchPoint, radius, "sphere", 0);
// 添加一个放到200倍后的坐标系
viewer.addCoordinateSystem(200);
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
}效果：
点云拓扑之octree（八叉树）
建立空间索引在点云数据处理中有着广泛的应用，常见的空间索引一般是自顶而下逐级划分空间的各种空间索引结构，比较有代表性的包括BSP树，KD树，KDB树，R树，四叉树，八叉树等索引结构，而这些结构中，KD树和八叉树使用比较广泛。 PCL 对八叉树的数据结构建立和索引方法进行了实现，以方便在此基础上对点云进行处理操作 。
八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。一般中心点作为节点的分叉中心。
原理概述
八叉树（Octree）的定义是：若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。那么，这要用来做什么？想象一个立方体， 我们最少可以切成多少个相同等分的小立方体？答案就是8个。再想象我们有一个房间，房间里某个角落藏着一枚金币，我们想很快的把金币找出来，聪明的你会怎 么做？我们可以把房间当成一个立方体，先切成八个小立方体，然后排除掉没有放任何东西的小立方体，再把有可能藏金币的小立方体继续切八等份….如此下去， 平均在Log8(房间内的所有物品数)的时间内就可找到金币。因此，八叉树就是用在3D空间中的场景管理，可以很快地知道物体在3D场景中的位置，或侦测与其它物体是否有碰撞以及是否在可视范围内。
pcl的octree库提供了从点云数据创建具有层次的数据结构的方法。这样就可以对点数据集进行空间分区，下采样和搜索操作。每个八叉树节点有八个子节点或没有子节点。根节点描述了一个包围所有点的3维包容盒子。
pcl_octree提供了有效的最近邻搜索（邻域搜索）API，例如“ 体素（Voxel）邻域搜索”，“ K最近邻搜索”和“半径邻域搜索”。叶子节点类也提供其他功能，例如空间“占用率”和“每个体素（Voxel）的点密度”检查；序列化和反序列化功能可将八叉树结构有效地编码为二进制格式；此外，内存池实现减少了昂贵的内存分配和释放操作，以便快速创建八叉树。
下图说明了最低树级别的八叉树节点的体素边界框。八叉树体素围绕着兔子表面的每个3D点。红点代表点数据。
八叉树实现原理
(1)设定最大递归深度。
(2)找出场景的最大尺寸，并以此尺寸建立第一个立方体。
(3)依序将单位元元素丢入能被包含且没有子节点的立方体。
(4)若没达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体。
(5)若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。
(6)重复3，直到达到最大递归深度。
八叉树的逻辑结构如下：
假设要表示的形体V可以放在一个充分大的正方体C内，C的边长为2n，形体V=C，它的八叉树可以用以下的递归方法来定义：八叉树的每个节点与C的一个子立方体对应，树根与C本身相对应，如果V＝C，那么V的八叉树仅有树根，如果V≠C，则将C等分为八个子立方体，每个子立方体 与树根的一个子节点相对应。只要某个子立方体不是完全空白或完全为V所占据，就要被八等分，从而对应的节点也就有了八个子节点。这样的递归判断、分割一直要进行到节点所对应的立方体或是完全空白，或是完全为V占据，或是其大小已是预先定义的体素大小，并且对它与V之交作一定的“舍入”，使体素或认为是空白的，或认为是V占据的。
应用实例
ocTree最近邻搜索
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv) {
srand((unsigned int)time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 生成点云数据
cloud->width = 10000;
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);
}
// float resolution = 0.01f;
// 设置分辨率为128
float resolution = 128.0f;
// resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
// 设置输入点云
octree.setInputCloud(cloud);
// 通过点云构建octree
octree.addPointsFromInputCloud();
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);
// 方式一：“体素近邻搜索”,它把查询点所在的体素中其他点的索引作为查询结果返回
// 结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数
std::vector<int> pointIdxVec;
if (octree.voxelSearch(searchPoint, pointIdxVec)) {
std::cout << "Neighbors within voxel search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z << ")"
<< std::endl;
for (size_t i = 0; i < pointIdxVec.size(); ++i)
std::cout << " " << cloud->points[pointIdxVec[i]].x
<< " " << cloud->points[pointIdxVec[i]].y
<< " " << cloud->points[pointIdxVec[i]].z << std::endl;
}
// 方式二：K 近邻搜索,本例中K被设置成10, "K 近邻搜索”方法把搜索结果写到两个分开的向量中
// 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉
// 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。
int K = 10;
std::vector<int> pointIdxNKNSearch;
std::vector<float> pointNKNSquaredDistance;
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if (octree.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;
}
// 方式三：半径内近邻搜索
// “半径内近邻搜索”原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中,
// 这两个向量分别存储结果点的索引和对应的距离平方
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 (octree.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;
}
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.0, 0.0, 0.5);
viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
// 添加从原点到搜索点的线段
viewer.addLine(originPoint, searchPoint);
// 添加一个以搜索点为圆心，搜索半径为半径的球体
viewer.addSphere(searchPoint, radius, "sphere", 0);
// 添加一个放到200倍后的坐标系
viewer.addCoordinateSystem(200);
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
}
无组织点云数据的空间变化检测
//@Description:：无序点云数据集的空间变化检测： https://www.cnblogs.com/li-yao7758258/p/6441595.html
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
int main(int argc, char** argv)
{
srand((unsigned int)time(NULL)); //用系统时间初始化随机种子
// 八叉树的分辨率，即体素的大小
float resolution = 32.0f;
// 初始化空间变化检测对象
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>); //创建点云实例cloudA生成的点云数据用于建立八叉树octree对象
// 为cloudA点云填充点数据
cloudA->width = 128; //设置点云cloudA的点数
cloudA->height = 1; //无序点
cloudA->points.resize(cloudA->width * cloudA->height); //总数
for (size_t i = 0; i < cloudA->points.size(); ++i) //循环填充
{
cloudA->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
cloudA->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
cloudA->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
}
// 添加点云到八叉树中，构建八叉树
octree.setInputCloud(cloudA); //设置输入点云
octree.addPointsFromInputCloud(); //从输入点云构建八叉树
/***********************************************************************************
点云cloudA是参考点云用其建立的八叉树对象描述它的空间分布，octreePointCloudChangeDetector
类继承自Octree2BufBae类，Octree2BufBae类允许同时在内存中保存和管理两个octree，另外它应用了内存池
该机制能够重新利用已经分配了的节点对象，因此减少了在生成点云八叉树对象时昂贵的内存分配和释放操作
通过访问 octree.switchBuffers ()重置八叉树 octree对象的缓冲区，但把之前的octree数据仍然保留在内存中
************************************************************************************/
// 交换八叉树的缓冲，但是CloudA对应的八叉树结构仍然在内存中
octree.switchBuffers();
// cloudB点云用于建立新的八叉树结构，与前一个cloudA对应的八叉树共享octree对象，同时在内存中驻留
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>); //实例化点云对象cloudB
// 为cloudB创建点云
cloudB->width = 128;
cloudB->height = 1;
cloudB->points.resize(cloudB->width * cloudB->height);
for (size_t i = 0; i < cloudB->points.size(); ++i)
{
cloudB->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
cloudB->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
cloudB->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
}
// 添加cloudB到八叉树中
octree.setInputCloud(cloudB);
octree.addPointsFromInputCloud();
/**************************************************************************************************************
为了检索获取存在于couodB的点集R，此R并没有cloudA中的元素，可以调用getPointIndicesFromNewVoxels方法，通过探测两个八叉树之间
体素的不同，它返回cloudB 中新加点的索引的向量，通过索引向量可以获取R点集 很明显这样就探测了cloudB相对于cloudA变化的点集，但是只能探测
到在cloudA上增加的点集，二不能探测减少的
****************************************************************************************************************/
std::vector<int> newPointIdxVector; //存储新添加的索引的向量
// 获取前一cloudA对应八叉树在cloudB对应八叉树中没有的点集
octree.getPointIndicesFromNewVoxels(newPointIdxVector);
// 打印点集
std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
for (size_t i = 0; i < newPointIdxVector.size(); ++i)
std::cout << i << "# Index:" << newPointIdxVector[i]
<< " Point:" << cloudB->points[newPointIdxVector[i]].x << " "
<< cloudB->points[newPointIdxVector[i]].y << " "
<< cloudB->points[newPointIdxVector[i]].z << std::endl;
}KDTree函数详解：03Octree · 语雀
1090
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
