小米汽车SU7，内饰全曝光！自带手机支架-CSDN博客

点击下方卡片，关注“3D视觉之心”公众号

第一时间获取3D视觉干货

写在前面

遥想当年Google大名鼎鼎的 Cartographer一经问世，掀起了激光SLAM的血雨腥风，其回环方法—— 2D 分支定界算法（Real-time loop closure in 2d LIDAR SLAM）把整个解空间用一棵树来表示，其根节点代表整个搜索窗口。树中的每一个节点的孩子节点都是对该节点所代表的搜索空间的一个划分，每个叶子节点都对应着一个解。整个搜索过程的基本思想可以总结为“缩小搜索空间”：先分支：不断地分割搜索空间；后定界：为每次分支之后的子节点确定一个上界。

然而，这些性能在 3D 地图中很难实现，因为内存消耗和处理成本急剧增加。

本文提出了一种准确快速的 3D 全局定位方法 3D-BBS，它扩展了现有的基于分支定界的 2D 扫描匹配算法。该方法平均只需要 878 ms 即可执行全局定位，并且在准确性和处理速度方面优于最先进的全局配准方法。为了优化整个算法在 3D 情况下的表现，主要贡献为：

本文提出的批处理 BnB 算法，可以在 GPU 上一次性快速进行大量分数计算
采用最佳优先搜索和旋转平移分支的组合：仅仅需要略微的近似，可以显著减少处理时间
利用通过稀疏哈希表创建的 3D 体素图，可以节省 3D 多分辨率体素图所需的内存

通过在模拟和真实环境中进行的实验，本文证明了3D-BBS只需在重力方向上粗略对齐的3D LiDAR点云和3D预建地图即可实现精确的全局定位。

原文链接：https://arxiv.org/pdf/2310.10023.pdf

代码链接：https://github.com/KOKIAOKI/3d_bbs

主要方法

问题定义

3D全局定位的已知条件有：

3D点云地图
待全局定位的当前帧点云
假设已知重力方向（e.g 由 IMU 测量获取）

待求解：6 自由度 pose: [x, y ,z, roll, pitch, yaw]

分支定界算法

使用层次树搜索来快速找到与全搜索结果等效的解决方案。树结构和叶节点分别表示搜索空间和问题的可行解。上层的节点表示一组子节点（即分支）。在每个非叶节点，计算其子节点分数的上限估计。如果计算出的上限低于临时最佳分数，我们可以用子节点修剪该节点，而无需近似（即修剪）。如果较早找到较高的临时最佳分数，BnB算法可以修剪许多节点，从而大大降低计算成本。因此，搜索顺序对于最大化 BnB 算法的性能（即搜索策略）非常重要。

用于 2D 全局定位的分支定界算法求的是 3 自由度 pose [x, y, yaw]

其中是当前帧点云，是 3 自由度变换矩阵，是各个自由度的搜索范围，2D BBS算法如下图所示：

多分辨率体素地图

为了减少 3D 多分辨率预计算地图的内存消耗，本文引入了基于空间哈希的稀疏体素地图。每个级别的体素分辨率表示为，其中是最小体素尺寸。为了搜索哈希桶，我们利用空间哈希函数 hash(v) 来计算相应哈希桶的索引，如下所示：在哈希桶构造中，我们寻找直到哈希冲突率变小（例如0.1%）。为了避免哈希冲突，我们实现了开放寻址方法：如果一个桶已经被另一个体素占据，我们会在增加桶索引的同时寻找一个空桶。

令 αβγ 为具有代表 6DoF 传感器位姿的离散参数的节点。我们使用预先计算的映射来计算节点的匹配分数，同时保持其子节点分数的上限。

分支算法

分支规则

不同于其他方法只搜索平移空间，本文同时对平移和旋转进行分支。如下图所示，平移搜索范围等于整个边界框。在平移分支中，一个父节点分支为 8 个步长为的子节点（如图(a)）。roll 和 pitch 分量在小范围内搜索（图（b））。yaw 的搜索范围为 π(图(c))。

在旋转分支中，我们为每个级别选择一个步长，使其小于 δ，并将搜索范围划分为相同大小的段：

值得注意的是，通过引入以上角度分支，方程 12 中的上限估计变得不是严格精确。然而，如实验所示，我们凭经验证实它仅在所有情况下低估了上限 0.001%。

搜索策略

尽管从初始集合中删除估计上限小于分数阈值的节点，但队列中仍保留大量节点。这会导致寻找 DFS 最佳解决方案的延迟并增加处理时间。因此，本文采用 BFS 并按分数降序对 C 进行排序。由于 BFS 选择具有最佳上限分数的节点，因此当全局最佳分数接近最大分数时（即，当 LiDAR 扫描与地图有较大重叠时），它往往比 DFS 更快地找到解决方案。然而，在中间级别，BFS 需要分支大量高分节点，因为大多数输入点与膨胀的体素重叠。为了加速分支过程，我们为分数计算实现了 GPU 并行批处理。

GPU 加速的批量分支定界

为了减少 CPU 到 GPU 数据复制和同步的开销，本文在 GPU 上并行处理多个节点的分数计算。批量 3D-BBS 的过程如下图所示：分支节点存储在 CPU 内存上的节点集中。当的大小超过批量大小 b（例如10,000个节点）时，作为单个内存块转移到GPU内存上的节点集，并在GPU上计算所有节点的分数。随后，所有计算出的分数被发送回。体素图 H 已提前复制到 GPU 内存以避免运行时开销。

实验

虚拟环境

实验设计了 9 种算法组合：处理类型（CPU 单线程、CPU 多线程（4 线程）和 GPU）、分支组件（仅平移、平移+旋转）、搜索策略的所有组合（ DFS 和 BFS）

配置 (a) - (i) 成功估计了传感器位姿，其平移和旋转误差小于所有评估点的标准阈值。旋转分支仅在 0.001% 的情况下显示出错误的上限估计，相对于真实上限的平均误差约为 1.5%。这些结果表明旋转分支仅引入了一个小的近似值。

使用 BFS 的配置 (d) - (f) 比使用 DFS 的配置 (a) - (c) 稍快。由于BFS首先对临时得分最高的节点进行分支，因此当扫描点与地图有较大重叠（即解的得分接近最大得分）时，它可以更快地找到全局解。然而，需要计算大量的各个级别的节点。在配置 (g) - (i) 中，旋转分支和 BFS 的组合有助于显着减少处理时间。采用批处理的配置 (i) 与采用 CPU 单线程的配置 (g) 相比，处理时间减少了 50% 以上。采用 CPU 处理的配置 (g) 和 (h) 也展示了不到 1 秒的快速处理，因为模拟环境中的最高分数允许更快的修剪。

真实环境

在真实数据上，与其他 SOTA 算法进行了对比：

结论

本文提出了一种基于BnB算法的3D全局定位方法，主要idea为：

使用稀疏哈希表来克服分层 3D 体素图的内存增加问题
提出了旋转平移分支和批量处理来减少处理时间

实验结果表明，3D-BBS准确估计了全局位姿，并在1秒内完成了定位过程。后续可扩展所提出的方法来处理极端情况（例如，退化区域或激光雷达扫描遮挡）。

参考

[1] 3D-BBS: Global Localization for 3D Point Cloud Scan Matching Using Branch-and-Bound Algorithm

[2] Real-time loop closure in 2d LIDAR SLAM

【3D视觉之心】技术交流群

3D视觉之心是面向3D视觉感知方向相关的交流社区，由业内顶尖的3D视觉团队创办！聚焦三维重建、Nerf、点云处理、视觉SLAM、激光SLAM、多传感器标定、多传感器融合、深度估计、摄影几何、求职交流等方向。扫码添加小助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

扫码添加小助理进群

【3D视觉之心】知识星球

3D视觉之心知识星球主打3D感知全技术栈学习，星球内部形成了视觉/激光/多传感器融合SLAM、传感器标定、点云处理与重建、视觉三维重建、NeRF与Gaussian Splatting、结构光、工业视觉、高精地图等近15个全栈学习路线，每天分享干货、代码与论文，星球内嘉宾日常答疑解惑，交流工作与职场问题。