参考引用
- Point Cloud Library
- 黑马机器人 | PCL-3D点云
- 【量化课堂】KD-Tree系列
- KD-Tree原理详解
1. 引言
- 通过激光雷达或双目相机获取到的点云，一般数据量较大且分布不均匀，数据主要表征了目标物表面的大量点的集合，这些离散的点如果希望实现基于邻域关系的快速查找比对功能，就必须对这些离散的点之间建立拓扑关系
- 常见的空间索引一般是自上而下逐级划分空间的各种索引结构，包括 BSP 树，KD tree、KDB tree、R tree、CELL tree、Octrees（八叉树）等，有了这些关系，我们就可以实现点云的降采样、计算特征向量、点云匹配和点云拆分等功能
2. BST（Binary Search Tree，二叉搜索树）
-
二叉搜索树定义
- 二叉搜索树，也称：二叉查找树，二叉排序树。它要么是一棵空树，要么是具有下列性质的二叉树：
- 若它的左子树不为空，则左子树上所有结点的值均小于它的根结点的值
- 若它的右子树不空，则右子树上所有结点的值均大于它的根结点的值
- 它的左、右子树也分别为二叉搜索树
- 二叉搜索树，也称：二叉查找树，二叉排序树。它要么是一棵空树，要么是具有下列性质的二叉树：
-
复杂度
- 不论哪一种操作，所花的时间都和树的高度成正比：若有 n 个元素，则平均每次操作需要 O ( l o g n ) O(logn) O(logn) 的时间
-
最近邻（1NN）搜索：设查询点为 11
- 从 8 开始：worst distance = 11 - 8 = 3（其中 worst distance 是在查询点周围搜索到的最大距离），因此 8 的左支离查询点至少距离为 3 ，故从右支开始往下查询，并且查询范围是（8，14）
- 查询到 10：worst distance = 11 - 10 = 1，因此更新最近邻为 10，并且更新查询范围是（10，12）
- 由于 10 只有右支，故往下查询到 14，worst distance = 14 - 10 = 4，不更新
- 再次往下查询到 13，不符合查询范围（10，12），发现到底了故往上返回到 14
- 继续往上返回到 10
- 最后到达顶点 8，由于 8 的左支离查询点至少距离为 3，并且最新的最近邻为 10，查询范围是（10，12），因此不考虑左支的查询，查询结束，最近邻为 10
3. kNN（k-Nearest-Neighbours，k最近邻）
- 在判定一个未知事物时，可以观察离它最近的几个样本，这就是 kNN（k最近邻）的方法
- kNN（k-Nearest Neighbours）是机器学习中最简单易懂的算法，它的适用面很广，并且在样本量足够大的情况下准确度很高，kNN 可以用来进行分类或者回归
3.1 一只兔子帮你理解 kNN
- 有一种兔子叫悲伤（Grief），它们的平均身高是 50 厘米，平均体重 5 公斤。我们拿来 100 只悲伤，分别测量它们的身高和体重，画在坐标图上，用绿色方块表示
- 还有一种兔子叫痛苦（Agony）。它们体型比较小，平均身高是 30 厘米，平均体重是 4 公斤。我们将 100 只痛苦的身高和体重画在同一个坐标图上，用蓝色三角表示
- 最后一种兔子叫绝望（Despair）。它们的平均身高 45 厘米，平均体重 2.5 公斤。100 只绝望的数据用黄色圆圈表示
在上面这些数据中，(身高，体重) 的二元组叫做特征（features），兔子的品种则是分类标签（class label）。我们想解决的问题是，给定一个未知分类的新样本的所有特征，通过已知数据来判断它的类别。某机器人只有测量兔子的身高和体重的能力，怎么让它判断一未知兔子的类别？我们给机器人预设一个整数 k，让它去寻找距离最近的 k 个数据样本进行分析：
- 机器人测量出这只兔子身长 40 厘米，体重 2.7 公斤，就是下面图中那颗闪闪发亮的红星
-
kNN 算法如何对这次观测进行分类要取决于 k 的大小。直觉告诉我们这只兔子像是一只绝望，因为除了最近的蓝色三角外，附近其他都是黄色圆圈；如果设 k = 15，算法会判断这只兔子是一只绝望。但是如果设 k = 1，那么由于距离最近的是蓝色三角，会判断迷之兔子是一只痛苦
-
如果按照 15NN 和 1NN 的方法对这个二维空间上的每一个点进行分类，会形成以下的分割
- 在两组分类中，1NN 的分类边界明显更 “崎岖”，但是对历史样本没有误判
- 而 15NN 的分类边界更平滑，但是对历史样本有误判现象
- 选择 k 的大小取决于对偏差和方差之间的权衡
3.2 距离函数
- 在选择一个数量 k 还只是小问题，更重要的是距离的计算方法，毕竟，当我们说 “最近的k个点” 时，这个 “近” 是怎么衡量的？
- 使用 kNN 时需要根据特征数据的取值区间来调整坐标轴的比例，这个做法叫作标准化或者归一化。为什么要这么做呢？拿上面的例子来说，一只兔子的身长（cm）数值平均是它的体重（kg）的 10 倍左右，如果我们在这组数值上直接使用 L 2 L_2 L2 距离函数的话就会导致横轴的距离比重明显放大，分类结果也不合理，如下图所示：
如果把坐标轴成其他的单位，比如毫米和吨，并用相应的新数值来计算距离，又会得到完全不同的分类标准。甚至，在极端情况下，如果身高用纳米并且体重用吨计量，那么相比之下身高的数值会奇高无比，以至于两点之间的距离是完全由身高决定的，体重则没有任何权重
- 为了解决这个问题，我们应该在计算距离时把所有坐标轴进行归一化
- 在之前的例子中，由于横轴数值大约是竖轴的 10 倍左右，所以我们将横轴（身高）的数值压缩 10 倍，即计算距离时使用下式就可以得出合理的 kNN 分类
3.3 概率 kNN
- 上面的 kNN 算法返回的是对一组特征的绝对分类，告诉我们这只兔子被判断为哪一个类别。可有时我们并不想知道一个确切地分类，而想知道它属于某个分类的概率是多大。比如我们发现一只身长 37 体重 4.8 的兔子，在下图五角星的位置
- 这只兔子的特征数据在悲伤和痛苦的分界处，机器不论判断它属于哪个类别都很有可能是错的。这时，类似“它有一半可能性是痛苦，一半可能性是悲伤”的反馈会更有意义
- 为了这个目的，我们同样找出距离问题特征最近的 k 个样本，但与其寻找数量最多的分类，我们统计其中每个类别的分别有多少个，再除以 k 得到一个属于每一个类别概率值
- 比如在上图里，距离五角星最近的 15 个样本中，有 8 只悲伤和 7 只痛苦，由此判断：它有 53% 的可能性是悲伤，47% 的可能性是痛苦，0% 的可能性是绝望
- 在整个二维空间中的每一个点上进行概率 kNN 算法，可以得到每个特征点是属于某个类别的概率热力图，图中颜色越深代表概率越大
- 相比于绝对的分类，这些概率的计算会给我们更有效的表述以及更多的应用空间
kNN 虽然思路简单，但实现起来有一个问题那就是计算量很大，当数据量很多时，拿一组特征来和所有样本依次计算距离并选取最近的 k 个，是非常耗费时间的，其中 kNN 的一个高效算法为：KD-Tree
4. KD-Tree 算法
4.1 KD-Tree 理论基础
- kd-tree 简称 k 维树，是计算机中用于在 k 维空间中一些点建立关系的数据结构，它是一个包含特定约束的二叉搜索树，常被用于高维空间中的搜索，比如范围搜索和最近邻搜索，kd-tree 是二进制空间划分树的一种特殊情况
- 如果特征的维度是 D，样本的数量是 N，那么一般来讲 kd 树算法的复杂度是 O(DlogN)，相比于穷算的 O(DN) 省去了非常多的计算量
- 通常只处理三维空间的点云（激光雷达），因此所有的 kd 树都是三维空间的，由于三维点云的数目一般都比较大，所以使用 kd-tree 来进行检索，可以减少很多的时间消耗，可以确保点云的关联点寻找和配准处于实时的状态
- 下面的数据在进行算法解析中，并不是全部都会用到，一般情况会用到的数据是：数据矢量，切割轴号，左支节点，右支节点，这些数据就已经满足 kd-tree 的构建和检索
struct kdtree{ Node-data - 数据矢量 数据集中某个数据点，是n维矢量（这里也就是k维） Range - 空间矢量 该节点所代表的空间范围 split - 整数 垂直于分割超平面的方向轴序号 Left - kd树 由位于该节点分割超平面左子空间内所有数据点所构成的k-d树 Right - kd树 由位于该节点分割超平面右子空间内所有数据点所构成的k-d树 parent - kd树 父节点 }
4.2 构建 KD-Tree
- kd-tree 的构建就是按照某种顺序将无序化的点云进行有序化排列，方便进行快捷高效的检索，构建算法如下：
Input: 无序化的点云，维度 k Output： 点云对应的 kd-tree Algorithm： 1、初始化切分轴：对每个维度的数据进行方差的计算，取最大方差的维度作为切分轴，标记为 r 2、确定节点：对当前数据按切分轴维度进行检索，找到中位数数据（如果一共有偶数个元素，则选择 中位左边或右边的元素，左或右并无影响），并将其放入到当前节点上 3、划分双支： 划分左支：在当前切分轴维度，所有小于中位数的值划分到左支中 划分右支：在当前切分轴维度，所有大于等于中位数的值划分到右支中 4、更新切分轴：r = (r + 1) % k; 5、确定子节点： 确定左节点：在左支的数据中进行步骤 2 确定右节点：在右支的数据中进行步骤 2
构造 kd-tree 实例
- 首先随机在 R 2 \mathbb{R}^2 R2 中随机生成 13 个点作为数据集。起始的切分轴 r = 0：这里 r = 0 对应 x 轴，而 r = 1 对应 y 轴
- 首先先沿 x 坐标进行切分选出 x 坐标的中位点，获取最根部节点的坐标
-
并且按照该点的 x 坐标将空间进行切分，所有 x 坐标小于 6.27 的数据用于构建左枝，x 坐标大于 6.27 的点用于构建右枝
-
在下一步中 r = 0 + 1 = 1 mod 2 对应 y 轴，左右两边再按照 y 轴的排序进行切分， 。得到下面的树，左边的 x 是指这该层的节点都是沿 x 轴进行分割的（空间的切分如下图二）
- 下一步中 r ≡ 1 + 1 ≡ 0 mod 2，对应 x 轴，所以下面再按照 x 坐标进行排序和切分
- 最后每一部分都只剩一个点，将他们记在最底部的节点中，因为不再有未被记录的点，所以不再进行切分（切分同上），就此完成了 kd-tree 的构造
4.3 k-近邻搜索
- 在构建了完整的 kd-tree 之后，想要使用它来进行高维空间的搜索。所以，这里讲解一下比较常用的最近邻搜索（最近邻搜索是 k-近邻的特例，也就是 1 近邻），其中范围搜索也是同样的道理
- 在激光点云中，常规的 kNN 算法时间复杂度会空前高涨。为减少时间消耗，工程上一般使用 kd-tree 进行 k-近邻搜索
使用 kd-tree 进行 k-近邻搜索实例
- 设想查询的点为 p = (−1,−5)，设距离函数是 L2 距离，想找距离 p 最近的 k = 3 个点
- （一）根据 p 的坐标值和每个节点的切分向下搜索（也就是说，如果树的节点是按照
x
r
x_r
xr = a 进行切分，并且 p 的 r 坐标小于 a，则向左枝进行搜索；反之则走右枝）
- 首先，从顶部开始
- 和这个节点的 x 轴比较一下，p 的 x 值更小，因此向左枝进行搜索
- 这次对比 y 轴，p 的 y 值更小，因此向左枝进行搜索
- 这个节点只有一个子枝，就不需要对比了，由此找到了最底部的节点 (−4.6,−10.55)
-
（二）当达到一个底部节点时，将其标记为访问过。如果 L 里不足 k 个点，则将当前节点的特征坐标加入 L ；如果 L 不为空并且当前节点的特征与 p 的距离小于 L 里最长的距离，则用当前特征替换掉 L 中离 p 最远的点
- 将当前结点标记为访问过，并记录下 L = [(−4.6,−10.55)]
-
（三）如果当前节点不是整棵树最顶端节点，则向上爬一个节点；反之输出 L，算法完成。如果当前（向上爬之后的）节点未曾被访问过，将其标记为被访问过，然后执行（1）和（2）；如果当前节点被访问过，则再次向上爬一个节点
（1）如果此时 L 里不足 k 个点，则将节点特征加入 L；如果 L 中已满 k 个点，且当前节点与 p 的距离小于 L 里最长的距离，则用节点特征替换掉 L 中离最远的点
（2）计算 p 和当前节点切分线的距离。如果该距离大于等于 L 中距离 p 最远的距离并且 L 中已有 k 个点，则在切分线另一边不会有更近的点，执行（三）；如果该距离小于 L 中最远的距离或者 L 中不足 k 个点，则切分线另一边可能有更近的点，因此在当前节点的另一个枝从（一）开始执行- 不是最顶端节点，向上爬一个节点到达 (−6.88,−5.4)
- 执行（1），因为记录下的点只有一个，小于 k = 3，所以也将当前节点记录下，有 L = [(−4.6,−10.55),(−6.88,−5.4)]。再执行（2），因为当前节点的左枝是空的，所以直接跳过，回到步骤（三），由于不是顶部，继续往上爬一个节点
- 由于还是不够三个点，于是将当前点也记录下，有 L = [(−4.6,−10.55),(−6.88,−5.4),(1.24,−2.86)]，并且当前结点修改为被访问过的。由于当前节点有其他的分枝，并且经计算得出 p 点和 L 中的三个点的距离分别是 6.62，5.89，3.10，但 p 和当前节点的分割线的距离只有 2.14，小于与 L 的最大距离
- 因此，在分割线的另一端可能有更近的点。于是在当前结点的另一个分枝从头执行（一），在红线这里
- 要用 p 和这个节点比较 x 坐标：p 的 x 坐标更大，因此探索右枝 (1.75,12.26)，并发现右枝已经是最底部节点，因此启动（二）
- 经计算，(1.75,12.26) 与 p 的距离是 17.48，要大于 p 与 L 的最大距离，因此不将其放入记录中
- 然后（三）判断出不是顶端节点，再往上爬一个节点
- 通过（1）计算这个节点与 p 的距离是 4.91，要小于 p 与 L 的最大距离 6.62
- 因此，用这个新的节点替代 L 中离 p 最远的 (−4.6,−10.55)
- 然后再通过（2）比对 p 和当前节点的分割线的距离
- 这个距离小于 L 与 p 的最小距离，因此要到当前节点的另一个枝执行（一），但那个枝只有一个点，故直接到（二）
- 计算距离发现这个点离 p 比 L 最大距离更远，因此不进行替代
- 通过（三）发现不是顶点，再往上爬一个节点
- 这个是已经访问过的了，所以再往上爬一个节点
- 同理这个是已经访问过的了，所以再往上爬一个节点
- 到顶点结束了吗？当然不，还没轮到（三）呢，现在是（1）的回合，计算比对发现顶端节点与 p 的距离比 L 最大距离还要更远，因此不进行更新
- 然后是（2），计算 p 和分割线的距离发现也是更远，因此也不需要检查另一个分枝
- 执行（三），判断当前是顶点，计算完成！输出距离 p 最近的三个点 L = [(−6.88,−5.4),(1.24,−2.86),(−2.96,−2.5)]
4.4 KD-Tree 近邻搜索代码示例
-
kd_tree.cpp
/* 方式一：指定搜索最近的 K 个邻居 方式二：通过指定半径搜索邻居 */ #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)); // 使用随机数据创建并填充 PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); cloud->width = 1000; cloud->height = 1; // 1 表示点云为无序点云 cloud->points.resize(cloud->width * cloud->height); // 1. 使用了 C++ 的标准库函数 rand() 来生成一个 0 到 RAND_MAX 之间的随机整数 // 然后将其除以 (RAND_MAX + 1.0f) 得到一个 0 到 1 之间的随机实数 // 2. 对于输入的点云 cloud，代码使用一个循环对其中每个点进行操作。在每次循环中 // 代码将当前点的 x、y、z 坐标分别设置为 [0, 1024] 范围内的随机实数 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); } // 创建 Kd-Tree 的实现类 KdTreeFLANN (Fast Library for Approximate Nearest Neighbor) pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // 将待搜索的点云数据集 cloud 设置为 KdTreeFLANN 的输入 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 个临近点 // pointIdxKNNSearch 保存搜索到的近邻点的索引 // pointKNNSquaredDistance 保存对应近邻点的距离的平方 int K = 10; std::vector<int> pointIdxKNNSearch(K); std::vector<float> pointKNNSquaredDistance(K); std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << K << std::endl; /* 1. nearestKSearch()的输入参数包括： 待搜索的点、要找到的最近邻数目 K、存储最近邻索引的向量、存储对应平方距离的向量 2. 如果成功找到至少一个最近邻，则进入 for 循环，依次输出每个最近邻点的 x、y、z 坐标和到搜索点的平方距离 */ if (kdtree.nearestKSearch(searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0) { for (size_t i = 0; i < pointIdxKNNSearch.size(); ++i) std::cout << " " << cloud->points[pointIdxKNNSearch[i]].x << " " << cloud->points[pointIdxKNNSearch[i]].y << " " << cloud->points[pointIdxKNNSearch[i]].z << " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl; } // 方式二：通过指定半径 k-近邻搜索 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 << " (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(); } return 0; } -
配置文件 CMakeLists.txt
cmake_minimum_required(VERSION 3.5 FATAL_ERROR) project(kdtree_search) find_package(PCL 1.2 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) add_executable(kdtree_search kdtree_search.cpp) target_link_libraries(kdtree_search ${PCL_LIBRARIES}) -
编译并执行
$ mkdir build $ cd build $ cmake .. $ make $ ./kdtree_search// 输出结果 K nearest neighbor search at (888.876 1009.54 308.168) with K=10 827.477 959.101 295.293 (squared distance: 6479.63) 987.269 1023.7 358.487 (squared distance: 12413.7) 928.859 995.463 183.952 (squared distance: 17226.4) 903.099 899.302 232.047 (squared distance: 18149) 866.187 902.418 415.973 (squared distance: 23611.4) 1022.08 944.851 384.353 (squared distance: 27732.5) 917.167 845.161 327.023 (squared distance: 28176.2) 1007.55 988.147 188.48 (squared distance: 28866.8) 765.84 1007.7 190.899 (squared distance: 28893.3) 870.826 871.352 208.202 (squared distance: 29414.6) Neighbors within radius search at (888.876 1009.54 308.168) with radius=169.287 827.477 959.101 295.293 (squared distance: 6479.63) 987.269 1023.7 358.487 (squared distance: 12413.7) 928.859 995.463 183.952 (squared distance: 17226.4) 903.099 899.302 232.047 (squared distance: 18149) 866.187 902.418 415.973 (squared distance: 23611.4) 1022.08 944.851 384.353 (squared distance: 27732.5) 917.167 845.161 327.023 (squared distance: 28176.2)
5. Octree（八叉树）算法
- 八叉树可以提前终止搜索，而 kd-tree 最终都会回到一开始的根节点，因为无法提前终止搜索，必须回到根节点才能确定是不是遍历了空间的所有可能性
- kd-tree 只有一个维度的信息不足以确定什么时候可以终止搜索；而八叉树有 3 个维度信息，一旦以查询点为中心、以最坏距离为半径构建的一个球完全落在了某个立方体里，就知道搜索范围限定在这个立方体中，立方体外面的东西不再考虑（不需要考虑根节点位置）
5.1 Octree 理论基础
- 八叉树定义
- 八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积，一般中心点作为节点的分叉中心
Voxel 译为体积元素，简称体素，描述了一个预设的最小单位的正方体
- 八叉树（Octree）若不为空树的，树中任一节点的子节点恰好只会有八个或零个，也就是子节点不会有 0 与 8 以外的数目
- 想象一个立方体，最少可以切成多少个相同等分的小立方体？答案就是 8 个。再想象有一个房间，房间里某个角落藏着一枚金币，可以把房间当成一个立方体，先切成八个小立方体，然后排除掉没有放任何东西的小立方体，再把有可能藏金币的小立方体继续切八等份，如此下去，平均在 l o g 8 ( n ) log_8(n) log8(n)（n 表示房间内的所有物体数）的时间内就可找到金币
- 八叉树就是用在 3D 空间中的场景管理，可以很快地知道物体在 3D 场景中的位置，或侦测与其它物体是否有碰撞以及是否在可视范围内
- 八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积，一般中心点作为节点的分叉中心
- 八叉树原理
- 设定最大递归深度
- 找出场景的最大尺寸，并以此尺寸建立第一个立方体
- 依次将单位元元素丢入能被包含且没有子节点的立方体
- 若没达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分给八个子立方体
- 若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为根据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形
- 重复 3，直到达到最大递归深度
5.2 构建 Octree
5.3 k-近邻搜索
5.4 Octree 近邻搜索代码示例
-
octree_search.cpp
#include <pcl/point_cloud.h> #include <pcl/octree/octree_search.h> #include <pcl/visualization/cloud_viewer.h> #include <iostream> #include <vector> #include <ctime> int main(int argc, char **argv) { srand((unsigned int) time(NULL)); // 首先定义并实例化一个共享的 Point Cloud 结构，并用随机点填充 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); cloud->width = 1000; 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); } // 创建一个 octree 实例，用设置分辨率进行初始化 float resolution = 128.0f; // 设置分辨率为 128 pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); // resolution 描述了 octree 叶子节点的最小体素尺寸 // 这两句是最关键的，建立 PointCloud 和 octree 之间的联系 octree.setInputCloud(cloud); // 设置输入点云 octree.addPointsFromInputCloud(); // 通过点云构建 octree // 初始化一个随机的点，作为目标查询点 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(); } } -
配置文件 CMakeLists.txt
cmake_minimum_required(VERSION 3.5 FATAL_ERROR) project(octree_search) find_package(PCL 1.2 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) add_executable(octree_search octree_search.cpp) target_link_libraries(octree_search ${PCL_LIBRARIES}) -
编译并执行
$ mkdir build $ cd build $ cmake .. $ make $ ./octree_search// 输出结果 Neighbors within voxel search at (252.328 696.188 682.94) 263.416 626.134 739.909 227.4 715.465 716.466 K nearest neighbor search at (252.328 696.188 682.94) with K=10 227.4 715.465 716.466 (squared distance: 2117.02) 182.548 736.153 660.19 (squared distance: 6984.08) 232.876 779.102 673.087 (squared distance: 7350.15) 264.602 782.368 696.721 (squared distance: 7767.66) 263.416 626.134 739.909 (squared distance: 8275.91) 294.628 715.86 604.602 (squared distance: 8313.08) 250.773 591.312 744.471 (squared distance: 14787.5) 279.201 816.457 655.874 (squared distance: 15919.4) 222.472 728.584 555.59 (squared distance: 18158.9) 265.322 671.457 820.796 (squared distance: 19784.7) Neighbors within radius search at (252.328 696.188 682.94) with radius=137.033 250.773 591.312 744.471 (squared distance: 14787.5) 182.548 736.153 660.19 (squared distance: 6984.08) 294.628 715.86 604.602 (squared distance: 8313.08) 263.416 626.134 739.909 (squared distance: 8275.91) 227.4 715.465 716.466 (squared distance: 2117.02) 222.472 728.584 555.59 (squared distance: 18158.9) 279.201 816.457 655.874 (squared distance: 15919.4) 264.602 782.368 696.721 (squared distance: 7767.66) 232.876 779.102 673.087 (squared distance: 7350.15)
