D6 无人机环境表示——从体素世界到高斯椭球¶

地面机器人用一张 2D 代价地图就能安全穿越走廊，四旋翼却不行——它在三维空间中飞行，需要知道头顶的树枝有多低、前方的窗户能否穿过、左侧的墙壁距自己多远。更关键的是，连续轨迹优化器不要"这格是障碍/这格是自由"的二值回答，它要的是**距离和梯度**——"我离最近的障碍还有 0.37 米，往右偏 15 度能最快远离它"。这个需求催生了一整个数据结构家族：从八叉树概率建图（OctoMap）到体素哈希距离场（voxblox/FIESTA）、环形缓冲滑窗栅格（Ewok/ROG-Map）、增量 kd 树（ikd-Tree）、GPU 加速体素（nvblox），直到最近的 3D 高斯椭球碰撞原语（Splat-Nav）。

本章不是数据结构的展览橱窗——它要回答一个贯穿始终的工程问题：轨迹优化器需要什么信息，这些数据结构怎么高效地给它。每种表示方法的讲解都锚定在"查询语义→数据结构→更新算法→与规划器的接口"这条链路上，最终收敛到选型决策：什么传感器、什么飞行速度、什么规划器，决定了你该用哪种表示。

性质：算法工程教学——理论与代码并重，源码精读与算法实现

前置自测¶

答不出 3 题及以上，先回顾前置章节再读本章。

三维空间查询：给定一个三维坐标 $(x,y,z)$ 和一棵平衡 kd 树，最近邻查询的时间复杂度是多少？为什么 kd 树在高维时退化？（提示：回顾数据结构基础）
贝叶斯概率更新：如果一个体素的先验占据概率是 $P=0.5$，传感器模型给出 $P(\text{occupied}|z)=0.7$，一次观测后后验概率是多少？两次相同观测后呢？（提示：乘法 vs log-odds 加法）
哈希表 O(1)：C++ 的 std::unordered_map 在最坏情况下查询复杂度是多少？为什么 voxblox 仍然选择它而非 std::map？（提示：均摊复杂度 vs 最坏复杂度对实时系统的影响）
Eigen 内存对齐：为什么 Eigen::Vector3d 不需要特殊对齐但 Eigen::Vector4d 需要 EIGEN_MAKE_ALIGNED_OPERATOR_NEW？这和 SSE/AVX 指令集有什么关系？（提示：16 字节 vs 32 字节对齐）
距离场梯度：如果你有一个连续标量场 $d(\mathbf{p})$，它的梯度 $\nabla d(\mathbf{p})$ 的物理意义是什么？为什么轨迹优化器需要这个梯度？（提示：梯度下降的方向信息）

本章目标¶

学完本章后，你应该能够：

**解释**六种主流无人机环境表示（OctoMap、voxblox、FIESTA、Ewok、ROG-Map、ikd-Tree）的查询语义差异——分别提供 O(?) 的什么信息（概率/TSDF/ESDF/kNN），并说清为什么查询语义的不同决定了它们适配不同的规划器；
手写 OctoMap 的 log-odds 贝叶斯更新公式的完整推导，说清 clamping 机制为什么能处理动态障碍物，并解释 log-odds 相比原始概率乘法的数值优势；
描述 voxblox 从深度图输入到 ESDF 梯度输出的完整数据流（投影集成 → TSDF 融合 → 准欧氏双波前 → 三线性插值 → $d(\mathbf{p})$ 和 $\nabla d(\mathbf{p})$），说清每个阶段的设计权衡；
**实现**环形缓冲的三维索引映射（位与运算替代取模），解释为什么缓冲区大小必须是 2 的幂，以及滑窗移动时只需清零边缘切片而非拷贝整个数组；
论证 ROG-Map 计数器膨胀的正确性——为什么计数器方案是唯一能正确处理"两个障碍共同膨胀了一个体素、其中一个消失"场景的 O($k^3$) 方案；
对比 ikd-Tree 在 SLAM（ICP 最近邻匹配）和规划（碰撞距离查询）中的双重角色，解释"一棵树服务两个子系统"的架构优势和并发控制挑战；
**做出**面对具体工程场景的选型决策：什么传感器 + 什么飞行速度 + 什么规划器 → 该用哪种环境表示。

本章知识导航¶

本章按"从简到繁、从基线到前沿"的顺序展开，每一节解决一个递进的问题：

节	知识点	难度	它解决什么	依赖
§D6.1	为什么无人机需要特殊环境表示	⭐	建立"2D costmap 不够用"的认知	无
§D6.2	OctoMap——概率八叉树与 log-odds	⭐⭐	理解概率建图和八叉树压缩的基线方法	贝叶斯基础
§D6.3	TSDF——从计算机视觉到机器人规划	⭐⭐	理解 voxblox 的前置数据结构	§D6.2
§D6.4	voxblox TSDF→ESDF 完整流水线	⭐⭐⭐	掌握 ESDF 的规划接口和生成算法	§D6.3
§D6.5	FIESTA——增量 ESDF 的极致优化	⭐⭐⭐	理解双队列如何加速 ESDF 更新	§D6.4
§D6.6	Ewok 环形缓冲——嵌入式局部地图	⭐⭐	掌握零拷贝滑窗的精妙数据结构	§D6.2
§D6.7	nvblox——GPU 加速体素建图	⭐⭐⭐	理解 GPU 体素哈希和 PBA ESDF	§D6.4
§D6.8	ROG-Map——2024 年 LiDAR 无人机事实标准	⭐⭐⭐	深入计数器膨胀和滑窗栅格	§D6.6
§D6.9	ikd-Tree——SLAM 与规划共享的数据结构	⭐⭐⭐	理解增量 kd 树的双重角色	§D6.2
§D6.10	3DGS 用于规划——Splat-Nav 与 SAFER-Splat	⭐⭐⭐⭐	理解隐式表示的规划化范式转变	§D6.4
§D6.11	选型决策矩阵与接口对接	⭐⭐	工程选型和与规划器的接口	全章

关于本章的代码：本章涉及 C++（核心数据结构实现）和伪代码（算法逻辑）。C++ 片段以 voxblox、Ewok、ikd-Tree 的实际源码为基础，经过简化和注释以突出教学要点。所有代码的目的是"验证和演示理论结论"，而非作为可直接运行的完整程序。

前置知识桥接¶

回顾 Eigen 矩阵基础。 所有体素和树操作的底层运算都依赖 Eigen。核心要点是：固定大小矩阵（Matrix3d、Vector3d）在栈上分配，编译器可以进行 SIMD 优化；动态大小矩阵（MatrixXd）在堆上分配，适用于维度在运行期才确定的场景。在高频循环（如 1kHz 控制、100Hz 地图更新）中，固定大小矩阵比动态大小快 10-100 倍。本章的体素坐标运算、kd 树距离计算、椭球碰撞检测等，全部基于 Eigen 的固定大小类型。

回顾 C++ 并发基础。 ikd-Tree 的后台重建线程、voxblox 的多线程集成都依赖 C++ 并发原语（std::mutex、std::thread、原子操作）。核心要点是：多线程共享可变状态时必须加锁或使用无锁数据结构；锁的粒度决定了并行度——太粗则线程互相等待，太细则锁开销吞噬并行收益。本章的 ikd-Tree 并行再平衡是展示"锁粒度权衡"的绝佳案例。

回顾 SLAM 项目精读（FAST-LIO2）。 ikd-Tree 已在 FAST-LIO2 精读中以"SLAM 内部数据结构"的身份出现过——它是 ICP 最近邻匹配的加速器。本章将从规划器的视角重新审视同一棵树：同样的 Nearest_Search 接口，在 SLAM 中用于点到平面残差计算，在规划中用于碰撞距离查询。这种"一棵树服务两个子系统"的架构设计，是本章的重要教学主题之一。

如果跳过本章会怎样¶

场景一：你用 Fast-Planner 做室内避障，发现轨迹总是贴着障碍物飞、偶尔擦到墙壁。你怀疑是规划器的问题，调了一天参数——其实是 voxblox 的 ESDF 分辨率太粗（0.2m），导致距离场不够精确。你不知道 ESDF 精度和体素大小的关系，也不知道准欧氏距离本身就有约 8% 的误差。
场景二：你的四旋翼在林地高速飞行（15 m/s），用 OctoMap 建图。飞了 200 米后，地图内存涨到 500 MB，查询延迟从 0.1ms 涨到 5ms，规划器频繁超时。你不知道 OctoMap 是全局地图，没有滑窗裁剪机制——而 ROG-Map 的环形缓冲可以把内存固定在 30 MB、查询恒定 O(1)。

预计阅读时间¶

模式	时间	适合
精读（含源码精读、手推 log-odds/ESDF、跑 voxblox 实验）	20-28 小时	要做环境表示选型和集成的工程师
速读（理解每种方法的核心思想和选型依据）	6-8 小时	先建立全局图景
速查（查选型矩阵、API 接口、公式）	0.5-1 小时	已熟悉、回来查细节

科研发展脉络¶

无人机环境表示的发展主线由四个研究组推动，呈现清晰的递进关系：

年份	工作	出处	核心贡献
2013	Hornung 等, "OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees"	Autonomous Robots	鼻祖：八叉树 + log-odds 概率更新 + 剪枝压缩；建立了三态语义（occupied/free/unknown）的范式
2017	Oleynikova 等（ETH ASL）, "Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning"	IROS 2017	TSDF→ESDF 流水线：分组射线投射 + 准欧氏双波前；体素哈希替代八叉树，O(1) 查询
2017	Usenko 等（TUM）, "Ewok: Real-Time Trajectory Replanning for MAVs Using Uniform B-splines and a 3D Circular Buffer"	IROS 2017	环形缓冲：$2^n$ 大小 + 位与模运算零拷贝滑窗 + 可分离 EDT
2019	Han, Gao 等（HKUST）, "FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots"	IROS 2019	双队列增量 ESDF：插入/删除队列分离 + 双向链表索引；比 voxblox ESDF 快 5 倍且精度更高
2021	Cai, Xu, Zhang（HKU MaRS）, "ikd-Tree: An Incremental K-D Tree for Robotic Applications"	RA-L 2022	增量 kd 树：惰性删除 + 箱式操作 + 降采样 + $\alpha$ 平衡并行重建；FAST-LIO2 的核心数据结构
2024	Millane 等（NVIDIA/ETH）, "nvblox: GPU-Accelerated Incremental SDF Mapping"	ICRA 2024	voxblox 的 CUDA 翻版；PBA 精确 ESDF；TSDF 集成 177 倍加速；Isaac ROS 集成
2024	Ren 等（HKU MaRS）, "ROG-Map: An Efficient Robocentric Occupancy Grid Map for Large-scene and High-resolution LiDAR-based Motion Planning"	IROS 2024	机器人中心滑窗栅格：计数器增量膨胀/去膨胀；0.05m 分辨率 50 Hz；SUPER 系统的地图层
2025	Chen 等（Stanford MSL）, "Splat-Nav: Safe Real-Time Robot Navigation in Gaussian Splatting Maps"	TRO 2025	3DGS 几何用于规划：椭球碰撞闭式判定；凸走廊 + Bezier 优化；无需 ESDF 和体素化

这条线的内在逻辑：OctoMap 建立了概率三维建图的范式 → voxblox 用体素哈希换取 O(1) 查询并引入 ESDF → FIESTA 在算法层面极致优化 ESDF 更新 → Ewok/ROG-Map 用环形缓冲实现固定内存滑窗 → nvblox 把整条流水线搬到 GPU → ikd-Tree 打通 SLAM 与规划的数据壁垒 → 3DGS 探索"不需要体素化"的全新范式。每一步都在回答同一个问题的不同侧面：如何更快、更准、更省地给规划器提供距离信息。

§D6.1 为什么无人机需要特殊的环境表示 ⭐¶

动机：从地面 2D costmap 说起¶

在进入具体算法之前，我们必须回答一个根本问题：地面移动机器人的 2D costmap 已经足够成熟且高效，为什么四旋翼不能直接复用？

ROS Navigation Stack / Nav2 中的 Costmap2D 是地面机器人的标准环境表示。它的核心设计非常优雅：将世界投影到水平面，形成一张 2D 网格（通常 0.05m 分辨率），每个栅格存储一个 uint8_t 代价值（0 = 自由，253 = 绝对障碍，254 = 膨胀障碍，255 = 未知）。多层叠加（静态层、障碍层、膨胀层、voxel 层）通过"最大值合并"组合。这个设计对地面机器人非常有效，因为地面机器人受到一个强约束：它们只在一个平面上运动。

但四旋翼在三维空间中飞行，面临五个根本挑战。理解这五个挑战是理解本章所有数据结构存在理由的前提。

如果不用三维环境表示会怎样¶

挑战	具体问题	用 2D costmap 的后果
三维自由度	四旋翼可以从上方飞越矮树、从下方穿越桥梁	投影到 2D 后无法区分不同高度的通道，可行路径被错判为不可行
连续距离与梯度	轨迹优化器需要 $d(\mathbf{p})$ 和 $\nabla d(\mathbf{p})$	2D costmap 只有离散代价值（0-253），没有物理单位，没有梯度方向
内存爆炸	3D 均匀网格的存储量是 2D 的 $N_z$ 倍	40m x 40m x 20m、0.05m 分辨率 = 256 MB（嵌入式不可接受）
滑窗需求	四旋翼飞行速度快（10-20 m/s），SLAM 地图持续增长	不支持高效裁剪，旧体素累积导致内存和查询延迟持续增长
动态障碍去膨胀	3D 中多个障碍物可能共同膨胀同一个体素	朴素清除一个障碍的膨胀区域会错误地清除另一个障碍的膨胀

让我们逐一深入每个挑战。

挑战一：三维自由度。 地面机器人的构型空间是 $(x, y, \theta)$ 三维（SE(2)），四旋翼的位置空间是 $(x, y, z)$ 三维（$\mathbb{R}^3$），加上偏航角 $\psi$ 后的规划空间是四维。将 3D 世界投影到 2D 意味着丢失了垂直方向的信息。考虑一个具体场景——森林中的倒木横在 1.5m 高度：

森林中的倒木场景:

            ===== 倒木 (z=1.5m) =====
           /                           \
地面 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 地面

2D costmap 视角: 倒木处为障碍, 无法飞行 ← 错误!
3D 地图视角:   倒木下方 z<1.5m 有自由空间, 可以从下面穿过
               倒木上方 z>2.0m 也是自由空间, 可以从上面飞过

这不是一个罕见的边缘情况。无人机在室内飞行时，桌子、架子、管道随处可见——它们在 2D 投影中是"障碍"，在 3D 中却有丰富的可飞行空间。

挑战二：连续距离场（ESDF）而非离散代价。 这是最关键的区别，值得花更多篇幅讲清楚。地面规划器（A*、D*、TEB）使用离散代价值进行图搜索——它们不需要知道"离障碍多远"，只需要知道"这格能不能走"。但四旋翼的连续轨迹优化器（Fast-Planner、GCOPTER）需要的是**连续距离场和梯度**：

\[ J_{\text{collision}} = \int_0^T \max\!\left(0,\, d_{\text{safe}} - d(\mathbf{p}(t))\right)^3 \, dt \]

其中 $d(\mathbf{p}(t))$ 是轨迹上每个点到最近障碍的欧氏距离（Euclidean Distance），$\nabla d(\mathbf{p})$ 是距离场的梯度（Gradient）。这个梯度告诉优化器"往哪个方向推轨迹才能远离障碍"。没有梯度，基于梯度下降的轨迹优化就无法工作。

本质洞察：2D costmap 回答的是一个布尔问题——"这里能不能走"。ESDF 回答的是一个连续问题——"这里离障碍多远、往哪个方向远离障碍最快"。这两个问题的信息量差异，决定了无人机需要完全不同的环境表示。

挑战三：内存与实时性的矛盾。 简单地把 2D costmap 扩展到 3D，内存会爆炸：

2D costmap 内存:
  40m x 40m, 0.05m 分辨率
  = 800 x 800 = 640,000 格子 x 1 byte = 640 KB

3D 均匀网格内存:
  40m x 40m x 20m, 0.05m 分辨率
  = 800 x 800 x 400 = 256,000,000 格子 x 1 byte = 256 MB

如果存 float ESDF 值:
  = 256,000,000 x 4 bytes = 1 GB  ← 完全不可行

因此，无人机的 3D 环境表示必须解决**稀疏存储**问题。这就是八叉树（OctoMap 的剪枝）、体素哈希（voxblox 只分配有数据的 Block）、环形缓冲（Ewok/ROG-Map 的固定大小窗口）等数据结构存在的根本原因。

挑战四：局部性与滑窗需求。 四旋翼飞行速度快，SLAM 地图持续增长。如果环境表示不支持高效的滑窗裁剪，旧体素会不断累积——占用内存、拖慢查询速度、可能引入 SLAM 漂移误差。专门为无人机设计的地图表示（ROG-Map、Ewok）都采用**机器人中心的滑动窗口**设计，内存占用固定。

挑战五：动态障碍物的正确去膨胀。 在 3D 中，当两个障碍物的膨胀区域重叠时，朴素的"清除膨胀"逻辑会出错：

朴素膨胀的 bug:

障碍A 膨胀了体素 X (A 和 X 距离 < 安全半径)
障碍B 也膨胀了体素 X (B 和 X 距离 < 安全半径)
障碍A 消失了 → 清除 A 的膨胀区域 → 体素 X 被清除!
→ 但体素 X 仍在 B 的膨胀范围内! → 安全性破坏!

ROG-Map 的计数器方案解决了这个问题——每个体素维护一个计数器，记录有多少个障碍物贡献了它的膨胀。计数器从 2 降到 1 时膨胀保留，降到 0 才撤销。这个看似简单的设计背后有严格的正确性保证（见 §D6.8）。

五个核心需求小结¶

需求	2D costmap 能否满足	无人机环境表示的解决方案
三维障碍区分	不能（投影丢失高度信息）	八叉树 / 体素网格 / 点云 kd 树
连续距离与梯度	不能（离散代价，无物理单位）	ESDF（voxblox / FIESTA / nvblox）
稀疏存储	不需要（2D 天然紧凑）	八叉树剪枝 / 体素哈希 / 环形缓冲
滑窗局部地图	部分（rolling_window 模式）	环形缓冲（Ewok / ROG-Map）
正确的膨胀/去膨胀	简单（2D 足够）	计数器膨胀（ROG-Map）

认知转换：从"我需要一张完整的地图"（SLAM 思维）到"我需要轨迹优化器能用的距离场和梯度"（规划思维）。这个认知转换是理解本章所有内容的钥匙。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为 OctoMap 的 dynamicEDT3D 等价于 voxblox 的 ESDF

错误做法：用 OctoMap + dynamicEDT3D 给 Fast-Planner 提供距离场
现象/后果：dynamicEDT3D 将整个 bounding box 存为稠密 3D 数组，30m x 30m x 10m 在 0.1m 分辨率下占 36 MB（仅距离值），失去了八叉树的压缩优势；且更新速度慢于 voxblox 的增量双波前
根本原因：dynamicEDT3D 是一个"补丁"——它在八叉树之上叠加了一个稠密距离数组，两者的设计哲学互相矛盾（八叉树追求压缩，稠密数组追求 O(1) 访问）
正确做法：如果需要 ESDF，直接用 voxblox（体素哈希 + 增量双波前）或 FIESTA（双队列增量更新）；如果不需要 ESDF，用 EGO-Planner 的 BFS 伪梯度

陷阱 2（思维陷阱）：以为"分辨率越高越好"

错误做法：把 voxblox 分辨率设为 0.01m 以求"最精确"
现象/后果：30m x 30m x 12m 空间产生 $10^{10}$ 级体素，内存爆炸，ESDF 更新无法实时
根本原因：分辨率、内存、速度是三角权衡。体素大小减半，体素总数增加 8 倍（$2^3$），ESDF 更新量也近似 8 倍
正确做法：体素大小 $\approx$ 机器人尺寸 / 10。四旋翼直径约 0.5m → 体素约 0.05m。需要更高精度时用 nvblox（GPU 加速）而非在 CPU 上硬扛

练习¶

内存估算：计算一个 50m x 50m x 15m 的室外场景，在 0.05m 分辨率下，均匀 3D 网格存储 float ESDF 值需要多少 GB？如果用八叉树（假设 90% 的空间是连续自由空间，可以被剪枝到深度 3），大约能压缩到多少？
设计决策：假设你的四旋翼携带一个 Intel RealSense D435i（RGB-D 深度相机），在室内以 3 m/s 飞行，规划器是 Fast-Planner。你应该选择哪种环境表示？给出你的推理过程。

§D6.2 OctoMap——概率八叉树与 log-odds 更新 ⭐⭐¶

动机：为什么从 OctoMap 讲起¶

OctoMap（Hornung 等, Autonomous Robots 2013）是机器人 3D 地图表示的**开山之作**，至今仍是最被广泛引用的 3D 建图方法（2200+ 引用）。虽然在实际无人机系统中它已被更高效的方法替代，但它建立的三个关键概念至今仍是所有后续方法的理论基础：

三态语义（Three-state Semantics）：空间中的每个体素有三种状态——occupied（障碍）、free（可通行）、unknown（未观测）。"unknown" 状态对规划至关重要——规划器不应把未知空间当作自由空间（否则四旋翼会飞进未探索区域撞墙）。
概率融合（Probabilistic Fusion）：多次观测通过贝叶斯滤波融合——一次观测到障碍不意味着一定是障碍（可能是传感器噪声），多次观测才逐渐确信。
层次压缩（Hierarchical Compression）：八叉树天然支持多分辨率——大片连续空间可以合并为一个大节点，极大节省内存。

这三个概念不仅是 OctoMap 的核心，也是理解后续所有方法的前提。voxblox 保留了概率融合（改为 TSDF 加权平均），ROG-Map 保留了三态语义，ikd-Tree 用惰性删除替代了概率衰减。每种后续方法都是在 OctoMap 的基础上，针对特定短板进行改进。

如果不用概率融合会怎样¶

想象一个没有概率融合的替代方案：每次传感器观测到某个位置有障碍，就立刻标记该体素为 occupied；观测到该位置为空，就立刻标记为 free。这种"最近观测覆盖"的策略有两个致命问题：

问题	后果
传感器噪声	一个噪声点就能制造一个虚假障碍，规划器为虚假障碍绕路
动态物体闪烁	人走过的路径上，体素在 occupied 和 free 之间反复切换

概率融合通过累积多次观测来"投票"——多次看到障碍才确信是障碍，多次看到空才确信是空。这是贝叶斯推断在空间表示中的直接应用。

核心机制：贝叶斯占据滤波与 log-odds¶

OctoMap 对每个体素维护一个占据概率 $P(n \mid z_{1:t})$——在观测到 $z_1, z_2, \ldots, z_t$ 后，节点 $n$ 被占据的概率。

贝叶斯递推公式——从贝叶斯定理出发：

\[ P(n \mid z_{1:t}) = \left[ 1 + \frac{1 - P(n \mid z_t)}{P(n \mid z_t)} \cdot \frac{1 - P(n \mid z_{1:t-1})}{P(n \mid z_{1:t-1})} \cdot \frac{P(n)}{1 - P(n)} \right]^{-1} \]

这个公式看起来复杂，但本质是**把新观测的信息和历史累积的信息通过几率比（Odds Ratio）相乘**。直接乘法有两个数值问题：（1）概率值在 $(0,1)$ 区间内反复相乘，容易趋近 0 或 1 导致精度丢失；（2）乘法比加法慢。

解决方案是引入 log-odds（对数几率）表示：

\[ L(n) = \log \frac{P(n)}{1 - P(n)} \]

$L$ 和 $P$ 的关系是单调映射：$P = 0.5$ 对应 $L = 0$（完全不确定），$P \to 1$ 对应 $L \to +\infty$（确定是障碍），$P \to 0$ 对应 $L \to -\infty$（确定是自由）。

在 log-odds 下，贝叶斯递推变为**简单的加法**：

\[ L(n \mid z_{1:t}) = L(n \mid z_{1:t-1}) + L(n \mid z_t) - L_0 \]

其中 $L_0 = L(n)$ 是先验（通常 $L_0 = 0$，对应 $P = 0.5$）。

本质洞察：log-odds 把概率的乘法转化为加法——这不是数学上的巧合，而是指数函数族（Exponential Family）的普遍性质。高斯分布的信息矩阵相加、HMM 的前向算法 log-sum-exp，本质上都是同一个技巧：在对数域中用加法代替乘法，获得数值稳定性和计算效率。这和 MPPI 中的 $\rho$ 减法（Ch2 §2.1）是同一个数值稳定化家族的成员。

实际更新的工作方式非常简单：

观测到"该体素被占据"  → L(n) += l_occ   (如 +0.85)
观测到"该体素是自由的" → L(n) += l_free  (如 -0.4)

多次观测后:
  L(n) > L_occ_threshold  → 判定为 occupied
  L(n) < L_free_threshold → 判定为 free
  否则 → 仍为 unknown

clamping（截断）机制——为了让体素"能被遗忘"（适应动态环境），OctoMap 对 log-odds 施加上下界：

\[ L_{\min} \leq L(n \mid z_{1:t}) \leq L_{\max} \]

典型值：$L_{\min} = -2.0$，$L_{\max} = 3.5$。这意味着一个体素被观测到"占据"足够多次后，其 log-odds 到达 $L_{\max}$，不再增加。之后如果观测到"自由"，log-odds 开始下降——体素可以从 occupied 回到 free。如果不做 clamping，一个被观测到障碍 100 次的体素，需要 100 次以上的"自由"观测才能翻转——对动态环境来说响应太慢。clamping 把响应延迟限制在有限次数内。

读到这里你可能会问："$l_{\text{occ}}$ 和 $l_{\text{free}}$ 的具体值怎么选？" 这取决于传感器模型。$l_{\text{occ}} = 0.85$ 对应 $P(\text{occ}|z_{\text{hit}}) \approx 0.7$，$l_{\text{free}} = -0.4$ 对应 $P(\text{occ}|z_{\text{miss}}) \approx 0.4$。注意 $|l_{\text{occ}}| > |l_{\text{free}}|$——这是故意的：障碍物比自由空间更"危险"，需要更少的观测就确认，更多的观测才翻转。

OctoMap 射线投射更新的完整逻辑：

// OctoMap 概率更新核心逻辑
void updateNode(OcTreeNode* node, float log_odds_update) {
    // 1. 加法更新
    float new_log_odds = node->getLogOdds() + log_odds_update;

    // 2. clamping 截断
    new_log_odds = std::clamp(new_log_odds, 
                              clamping_thres_min,    // 如 -2.0
                              clamping_thres_max);   // 如 +3.5

    // 3. 写回
    node->setLogOdds(new_log_odds);
}

// 射线投射: 从传感器原点到观测点
void insertPointCloud(const Pointcloud& scan, const point3d& origin) {
    for (auto& endpoint : scan) {
        // 射线上的所有体素标记为 free
        KeyRay ray;
        computeRayKeys(origin, endpoint, ray);
        for (auto& key : ray) {
            updateNode(key, prob_miss_log);   // l_free, 如 -0.4
        }
        // 端点体素标记为 occupied
        updateNode(endpoint, prob_hit_log);   // l_occ, 如 +0.85
    }
}

八叉树结构与压缩¶

**八叉树（Octree）**将 3D 空间递归地八等分。每个内部节点有 8 个子节点，对应空间被沿 x、y、z 三个轴中点切分后的 8 个象限。叶节点存储实际的占据概率（log-odds 值）。

八叉树递归分割:

         ┌──────────────┐
         │  根节点 (深度 0) │  ← 整个空间 (如 100m^3)
         └──────┬───────┘
        ┌───────┼───────┐
   ┌────┤  8 个子节点    ├────┐
   │    └───────┼───────┘    │
   ▼            ▼            ▼
[子节点1]  [子节点2]  ... [子节点8]    ← 深度 1, 每个 50m^3
   │
   ▼
  ...                                 ← 深度 d, 每个 (100/2^d)m^3

对于分辨率 0.1m 的地图覆盖 100m 范围，需要深度 $d = \log_2(100/0.1) = 10$。

剪枝（Pruning）压缩——八叉树的核心压缩能力来自剪枝：当一个内部节点的 8 个子节点具有相同的占据状态时，可以删除子节点，用父节点代表整个区域。递归剪枝后，大片自由空间可能被一个很高层的节点表示，压缩率可达 25-100 倍。

实际内存对比:

场景: 80m x 80m x 20m 办公楼, 0.1m 分辨率
均匀 3D 网格: 800 x 800 x 200 = 128,000,000 体素 x 4 bytes = 512 MB
OctoMap(八叉树): 约 5-20 MB (取决于场景复杂度)
压缩率: 25-100x

OctoMap 的局限性——为什么需要下一代方法¶

虽然 OctoMap 是概念基石，但在无人机实时规划中有明显不足：

局限	原因	后续方案如何解决
查询慢：O(log N)	八叉树需要逐层遍历指针	体素哈希 O(1)（voxblox）；扁平数组 O(1)（ROG-Map）
不提供 ESDF	需要额外库 dynamicEDT3D	voxblox/FIESTA 内建 ESDF
缓存不友好	指针式树结构，内存随机跳跃	体素哈希的 Block 内连续内存（voxblox）
不支持高效滑窗	删除旧区域需要递归遍历	环形缓冲 O(1) 覆盖（Ewok/ROG-Map）
GPU 移植困难	指针不兼容 GPU 地址空间	体素哈希 + 数组，nvblox 直接搬

教学定位：OctoMap 是理解概率建图和八叉树的**最佳起点**——它的概念足够简单、数学足够优雅。但在实际无人机系统中，它已被 voxblox / ROG-Map 替代。后续方法保留了 OctoMap 的概率融合思想，但用完全不同的数据结构实现了更高的性能。

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：忘记 clamping 导致体素"永远不会变"

错误做法：自己实现 log-odds 更新时不加 clamping
现象/后果：一个体素被观测到障碍 1000 次后，$L = 850$；之后即使连续观测到 2000 次"自由"也无法翻转（$850 - 2000 \times 0.4 = 50$，仍然 > 0）
根本原因：没有 clamping，log-odds 可以无限增长，导致遗忘速度与历史观测次数成正比
正确做法：始终加 clamping：new_L = std::clamp(old_L + update, L_min, L_max)

陷阱 2（概念误区）：以为八叉树查询是 O(1)

错误做法：在性能分析中把 OctoMap 查询当作常数时间
现象/后果：在高频循环（如 1kHz 碰撞检查）中，OctoMap 查询成为瓶颈
根本原因：八叉树的查询是 O(深度) = O(log N)，且每层都是指针解引用（可能 cache miss）。10 层深度、每层 200 cycles cache miss = 2000 cycles 最坏情况。对比 voxblox 的 O(1) 哈希查找 + 数组访问 $\approx$ 100 cycles
正确做法：如果查询频率高于 100 Hz，优先选择 O(1) 查询的数据结构

练习¶

手推 log-odds（实现题）：一个体素初始 $L = 0$（$P = 0.5$）。传感器观测到它是障碍（$l_{\text{occ}} = 0.85$）三次，然后观测到自由（$l_{\text{free}} = -0.4$）两次。设 $L_{\max} = 3.5$，$L_{\min} = -2.0$。
(a) 逐步计算每次更新后的 $L$ 值（注意 clamping）
(b) 最终 $L$ 对应的概率 $P = \frac{1}{1+e^{-L}}$ 是多少？
(c) 如果取消 clamping，最终 $L$ 和 $P$ 有什么不同？
(d) 如果要让这个体素重新回到 $P < 0.5$（判定为 free），至少还需要多少次"自由"观测？
压缩率分析（设计扩展题）：一个 1000 x 1000 x 100 的体素空间，95% 是连续自由空间、4% 是连续障碍、1% 是混合边界。估算八叉树的节点数量和内存占用，对比均匀网格。提示：连续同值区域可以被高层节点代表；混合边界必须细分到叶节点。
跨章综合：log-odds 的加法更新和 MPPI（Ch2）的 $\rho$ 减法都是"在对数域中操作以获得数值稳定性"。它们各自解决什么数值问题？有没有更深层的数学联系？（提示：指数函数族的充分统计量）

§D6.3 TSDF——从计算机视觉到机器人规划的桥梁 ⭐⭐¶

动机：为什么需要 TSDF¶

OctoMap 的 log-odds 更新直接产出"占据概率"——一个体素要么是障碍、要么是自由、要么未知。但规划器需要的不是"是/否"的判断，而是"到表面多远"的连续值。从概率占据到欧氏距离，需要一个中间桥梁——这就是 TSDF（Truncated Signed Distance Field，截断有符号距离场）。

TSDF 来自计算机视觉领域（Curless & Levoy, 1996），最初用于三维重建——把多帧深度图融合成一个连续的隐式表面。Oleynikova 等（2017）的关键洞见是：TSDF 不仅是重建工具，还是 ESDF 的最佳前置数据结构。从 TSDF 构建的 ESDF 比从占据网格构建的 ESDF 更精确（平均误差小 30-50%），因为 TSDF 本身就包含了连续距离信息。

核心概念：SDF 与 TSDF¶

SDF（Signed Distance Field，有符号距离场）：空间中每个点存储到最近表面的**有符号距离**——表面外为正，表面内为负。零等值面（$d = 0$）就是物体表面。

TSDF（Truncated SDF）：将 SDF 截断到 $[-\delta, +\delta]$ 的范围内。超出截断距离 $\delta$ 的体素不存储——远离表面的距离值对规划和重建意义不大，截断可以节省大量内存和计算。

TSDF 截断示意:

         +delta ========================
          /                             |
d(x)     /                              |
        /                               |
  0 ---X--------------- 物体表面         |  ← 零等值面 = 物体表面
        \                               |
         \                              |
         -delta ========================

截断范围 [-delta, +delta] 外: 不存储 (节省内存)
截断范围内: 存储精确的有符号距离

读到这里你可能会问："TSDF 和 OctoMap 的 log-odds 有什么关系？它们解决同一个问题吗？" 关键区别是：log-odds 回答"这个体素是障碍的概率是多少"（离散分类问题），TSDF 回答"这个体素离最近表面多远"（连续回归问题）。TSDF 包含了更丰富的信息，因此更适合作为 ESDF 的前置。

TSDF 集成方法¶

voxblox 使用**投影集成（Projective Integration）**构建 TSDF。对于每个深度帧，遍历视锥（Frustum）内的体素，将每个体素反投影到深度图上，读取对应的深度值：

投影集成核心逻辑:

for each voxel v in frustum(camera_pose, depth_image):
    # 1. 将体素中心投影到图像平面
    pixel = K * T_cw * v.position      # K: 相机内参, T_cw: 世界到相机

    # 2. 读取深度值
    measured_depth = depth_image[pixel.u, pixel.v]

    # 3. 计算沿视线方向的距离
    voxel_depth = (T_cw * v.position).z

    # 4. TSDF 值 = 测量深度 - 体素深度
    sdf = measured_depth - voxel_depth

    # 5. 截断
    tsdf = clamp(sdf, -truncation_dist, +truncation_dist)

    # 6. 加权平均融合 (多帧平滑噪声)
    v.distance = (v.distance * v.weight + tsdf * w_new) / (v.weight + w_new)
    v.weight = min(v.weight + w_new, max_weight)

这里的加权平均融合是 TSDF 的核心优势之一：传感器噪声被自然平滑。单帧深度图可能有 $\pm$2cm 的噪声，但 10 帧融合后噪声降低到 $\pm$0.6cm（$\sqrt{10}$ 倍降噪）。

voxblox 的 merged integrator 在此基础上做了一个关键优化——先将落入同一体素的所有像素合并为一根加权射线，避免同一体素被多个像素重复更新：

Merged Integrator 优化:

原始: 1000 像素各自射线投射 → 同一体素被更新约 50 次
优化: 1000 像素按体素分组 → 20 个体素各收到 1 根合并射线 → 更新 20 次

TSDF vs 占据网格：对规划的影响¶

方面	TSDF	占据网格（OctoMap）
存储值	float 距离 + float 权重	float log-odds
表面表达	隐式连续（零等值面）	离散二值化阈值
噪声处理	加权平均（自然平滑）	log-odds 累积（需要 clamping）
ESDF 构建精度	高——TSDF 已含连续距离信息	低——从二值化出发误差更大
内存	8 bytes/体素（distance + weight）	4 bytes/体素（log-odds）

不是 TSDF 而是 ESDF：很多初学者混淆 TSDF 和 ESDF。TSDF 只在截断距离 $\delta$（通常 0.3-0.5m）内有精确值——远离表面的体素没有距离信息。规划器需要的是**完整的 ESDF**——每个体素到最近表面的距离，无截断。TSDF 是 ESDF 的前置输入，不能直接用于规划。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为 TSDF 可以直接给规划器用

错误做法：把 TSDF 的 distance 字段直接作为碰撞距离输入 Fast-Planner
现象/后果：截断距离外的体素 TSDF 值为 0（未存储），规划器以为那里是表面→大量虚假碰撞
根本原因：TSDF 的截断范围远小于规划器需要的安全距离范围（$d_{\text{safe}}$ 通常 0.5-1.0m，而 $\delta$ 通常 0.2-0.3m）
正确做法：必须从 TSDF 构建 ESDF（通过波前传播扩展距离值到全空间），然后把 ESDF 给规划器

陷阱 2（编程陷阱）：截断距离 $\delta$ 设置不当

错误做法：$\delta$ 设得太小（如 0.1m）
现象/后果：TSDF 带太窄，ESDF 波前传播的起点（零穿越面）不够精确，最终 ESDF 质量下降
根本原因：$\delta$ 太小时，体素网格的离散化误差在窄带内占比更大
正确做法：$\delta$ 通常设为 4-8 倍体素大小。体素 0.1m → $\delta$ 约 0.4-0.8m

TSDF 的三种集成模式对比¶

voxblox 提供三种 TSDF 集成模式，适用于不同场景：

模式	速度	精度	实现策略	适用场景
`simple`	慢	最高	逐像素独立射线投射，不合并	教学/离线重建
`merged`	中等	高	按体素分组合并射线，减少重复更新	推荐默认
`fast`	快	中等	跳过远体素，只更新截断带附近	实时/嵌入式

三种模式的差异本质上是"精度-速度"权衡的不同取舍点。simple 模式对每个像素独立做射线投射——概念最清晰但计算量最大（同一体素可能被 50 个像素重复更新）。merged 模式先将落入同一体素的像素分组合并——用一次哈希分组操作换取大幅减少的重复计算。fast 模式在 merged 基础上进一步跳过离表面太远的体素——但可能导致远处体素的 TSDF 值不够精确。

练习¶

TSDF 融合推导（实现题）：一个体素的初始 TSDF 值 $d_0 = 0.15$m，权重 $w_0 = 5$。新观测的 TSDF 值 $d_{\text{obs}} = 0.12$m，权重 $w_{\text{obs}} = 1$。
(a) 融合后的 TSDF 值和权重是多少？
(b) 再来 4 次 $d_{\text{obs}} = 0.12$m 的观测，最终值收敛到多少？
(c) 如果 $w_{\max} = 20$，权重饱和后再来 10 次观测，TSDF 值还会变吗？为什么设 $w_{\max}$？
TSDF vs 占据网格精度（思考题）：为什么 voxblox 论文说"从 TSDF 构建的 ESDF 比从占据网格构建的更精确"（平均误差小 30-50%）？提示：占据网格将表面二值化为"占据/自由"，引入了体素大小量级的阶梯状近似；TSDF 的零等值面在体素内部连续插值，表面定位精度高于体素分辨率。

§D6.4 voxblox TSDF→ESDF 完整流水线 ⭐⭐⭐¶

动机：为什么 voxblox 是无人机规划的里程碑¶

voxblox（Oleynikova 等, ETH ASL, IROS 2017）是第一个在单个 CPU 核心上实时运行的 TSDF→ESDF 系统。它的里程碑意义在于：把 ESDF（轨迹优化器真正需要的输出）从理论概念变成了可部署的实时模块。在 voxblox 之前，获取 ESDF 要么离线计算（不实时）、要么用 dynamicEDT3D 补丁（不高效）。voxblox 之后，Fast-Planner、FUEL 等一系列依赖 ESDF 梯度的规划器才有了可用的基础设施。

系统架构总览¶

voxblox 的数据流是一条清晰的流水线：

深度图 + 位姿 (来自外部 SLAM)
    |
    | 分组射线投射 (merged integrator)
    v
TSDF Layer<TsdfVoxel>   (每体素: distance + weight + color)
    |
    | 准欧氏双波前 (raise + lower BFS)
    v
ESDF Layer<EsdfVoxel>   (每体素: distance + parent_direction)
    |
    | 三线性插值
    v
d(x), nabla_d(x)  → 送给轨迹优化器

注意 voxblox 本身**不做位姿估计**——它需要外部 SLAM 系统（VINS-Mono / FAST-LIO2 / ORB-SLAM3）提供每帧的位姿。

核心数据结构：体素哈希¶

voxblox 不使用八叉树，而是使用**体素哈希（Voxel Hashing）**——这个选择是 voxblox 性能优势的核心来源。体素哈希源自 Niessner 等（2013）的实时 3D 重建工作，其组织结构是：

两级索引：全局哈希表 → Block（$16^3$ 的稠密体素块）→ 单个体素。

全局哈希表:
  std::unordered_map<BlockIndex, Block<VoxelT>::Ptr>

     BlockIndex(3,1,2) --> Block<TsdfVoxel>  [16x16x16 稠密数组]
     BlockIndex(3,1,3) --> Block<TsdfVoxel>
     BlockIndex(4,1,2) --> Block<TsdfVoxel>
     ...

查询路径: 世界坐标 → BlockIndex(哈希查找 O(1)) → 局部索引(数组访问 O(1))

为什么不用八叉树？ 这是一个经典的设计权衡：

方面	八叉树（OctoMap）	体素哈希（voxblox）
查询	O(log N)，逐层遍历指针	O(1)，哈希查找 + 数组索引
缓存友好性	差——指针跳跃，空间局部性差	好——Block 内 16^3 体素连续存储
内存碎片	严重——大量小节点各自 malloc	少——Block 为 4KB 对齐单元
GPU 兼容性	困难——指针不兼容 GPU 地址空间	容易——哈希表 + 数组，nvblox 直接搬
多分辨率	天然支持（八叉树层次结构）	不支持（需要多层 Layer）
压缩	优秀（剪枝合并同值节点）	无（Block 内全部稠密存储）

本质洞察：voxblox 牺牲了多分辨率和压缩能力，换取了 O(1) 查询和缓存友好性。对实时规划而言，这是正确的权衡——规划器在每个优化迭代中要查询数百个 ESDF 值，O(1) 和 O(log N) 的差异在高频循环中被放大数百倍。

C++ 实现的核心模板设计：

// 体素类型
struct TsdfVoxel {
    float distance = 0.0f;   // TSDF 值
    float weight = 0.0f;     // 融合权重
    Color color;             // 可选: 颜色
};

struct EsdfVoxel {
    float distance = 0.0f;          // ESDF 值(有符号)
    bool observed = false;           // 是否被观测过
    bool in_queue = false;           // 是否在 BFS 队列中
    bool fixed = false;              // 是否已收敛
    Eigen::Vector3i parent;          // 最近障碍的方向向量 ← 这就是梯度!
};

// Block: 16^3 的稠密体素块
template <typename VoxelType>
class Block {
    static constexpr int kVoxelsPerSide = 16;
    VoxelType voxels_[kVoxelsPerSide * kVoxelsPerSide * kVoxelsPerSide];

    VoxelType& getVoxelByVoxelIndex(const VoxelIndex& idx) {
        return voxels_[idx.x() * kVoxelsPerSide * kVoxelsPerSide +
                       idx.y() * kVoxelsPerSide + idx.z()];
    }
};

// Layer: 全局体素哈希表
template <typename VoxelType>
class Layer {
    std::unordered_map<BlockIndex, typename Block<VoxelType>::Ptr,
                       BlockIndexHash> block_map_;

    // O(1) 体素查询
    VoxelType* getVoxelPtrByCoordinates(const Point& coords) {
        BlockIndex bi = computeBlockIndex(coords);
        auto it = block_map_.find(bi);
        if (it == block_map_.end()) return nullptr;  // 该区域无数据
        VoxelIndex vi = computeVoxelIndex(coords, bi);
        return &(it->second->getVoxelByVoxelIndex(vi));
    }
};

ESDF 生成——准欧氏双波前算法¶

TSDF 只在截断距离 $\delta$ 内有精确值。但规划器需要的是**完整的 ESDF**——每个体素到最近表面的距离，无截断。考虑 Fast-Planner 的碰撞代价：安全距离 $d_{\text{safe}}$ 通常是 0.5-1.0m，而 TSDF 截断距离 $\delta$ 通常只有 0.2-0.4m——TSDF 覆盖范围远不够。因此需要一个波前传播算法，从 TSDF 的零穿越面（物体表面）出发，向外扩展距离值到全空间。

这就像在水面上扔一块石头：石头落水点是"零穿越面"，涟漪向外扩散——距离值就是涟漪到达每个位置时的传播距离。不同的是，这里的"涟漪"不是圆形，而是沿着 3D 网格的 26 个方向传播。

voxblox 使用**增量式双波前（Raise + Lower）算法**。为什么需要**两个**波前？因为地图是动态更新的——每一帧深度图都可能带来两种变化：（1）新障碍出现（距离减小）；（2）旧障碍消失（距离需要重新计算）。这两种变化需要不同的处理策略。

RAISE 阶段（上波前）：处理"障碍移走"的情况。当一个体素的 TSDF 显示它不再是障碍时，需要通知所有以它为最近障碍的体素——"你的距离值失效了，你需要重新找最近障碍"。这些失效的体素被放入 raise queue，沿 parent 方向传播失效信号。

为什么叫"raise"？因为障碍消失后，受影响体素的距离值会**增大**（最近障碍更远了）——距离值被"抬升"了。

LOWER 阶段（下波前）：处理"新障碍出现"和"重新计算"。从零穿越面出发，用 BFS 向外传播距离值。如果新计算的距离比旧值更小（有更近的障碍），则更新。

为什么叫"lower"？因为新障碍出现后，附近体素的距离值会**减小**——距离值被"降低"了。

两个阶段的执行顺序：voxblox 先执行所有 RAISE（清除失效值），再执行所有 LOWER（填入正确值）。这个顺序很重要——如果反过来，LOWER 填入的值可能会被随后的 RAISE 错误地清除。

ESDF 增量更新算法:

输入: 上一帧 ESDF + 本帧 TSDF 更新的体素集合

1. 识别变化体素:
   changed = {v : v 的 TSDF 在本帧发生了变化}

2. RAISE 阶段:
   for each v in changed:
     if v 的新 ESDF > 旧 ESDF:     // 障碍移走了
       raise_queue.push(v)

   while raise_queue 非空:
     v = raise_queue.pop()
     for each neighbor n of v (26-connected):
       if n.parent 指向 v 的方向:   // n 的最近障碍就是 v 方向
         n.distance = INF           // 失效
         raise_queue.push(n)        // 继续传播

3. LOWER 阶段:
   for each v in changed:
     if v 的新 ESDF < 旧 ESDF:     // 新障碍出现
       lower_queue.push(v)

   while lower_queue 非空:
     v = lower_queue.pop()
     for each neighbor n (26-connected):
       d_new = v.distance + dist(v, n)   // 准欧氏: 1, sqrt(2), sqrt(3)
       if d_new < n.distance:
         n.distance = d_new
         n.parent = direction(n -> v)
         lower_queue.push(n)

准欧氏距离（Quasi-Euclidean Distance）——voxblox 的 ESDF 不是精确欧氏距离。沿网格传播时，面邻居距离 1，棱邻居距离 $\sqrt{2}$，角邻居距离 $\sqrt{3}$，最大误差约 8%。对规划而言，8% 通常可以接受——安全裕度 $d_{\text{safe}}$ 本身就留有余量。但 nvblox 使用 PBA 算法消除了这个误差（见 §D6.7）。

ESDF 的规划接口：d(x) 和 $\nabla d(\mathbf{x})$¶

ESDF 是离散体素数据。轨迹优化器需要在**任意连续坐标**处查询距离和梯度。voxblox 通过**三线性插值**提供——找到包围查询点的 8 个体素，用三个方向的线性权重插值距离值，梯度通过对插值公式求偏导获得。

与 Fast-Planner 的接口——碰撞代价函数：

\[ J_{\text{collision}} = \int_0^T c(\mathbf{p}(t))\,dt, \quad c(\mathbf{p}) = \begin{cases} (d_{\text{safe}} - d(\mathbf{p}))^3 & \text{if } d(\mathbf{p}) < d_{\text{safe}} \\ 0 & \text{otherwise} \end{cases} \]

梯度：

\[ \frac{\partial J_{\text{collision}}}{\partial \mathbf{p}} = -3(d_{\text{safe}} - d(\mathbf{p}))^2 \nabla d(\mathbf{p}) \]

这就是为什么需要 $d(\mathbf{p})$ 和 $\nabla d(\mathbf{p})$——它们直接参与梯度下降，告诉优化器"把轨迹往哪个方向推，能远离障碍"。

与 EGO-Planner 的对比——EGO-Planner（Zhou 等, 2020）提出了**不需要 ESDF** 的碰撞梯度计算。核心观察是：轨迹优化只用到轨迹附近的 ESDF 值，但 ESDF 更新在整个地图范围内传播波前——大量计算被浪费了。EGO-Planner 的替代方案是：当控制点进入障碍时，从该点出发做 BFS 找到最近自由空间参考点，碰撞梯度 = 控制点到参考点的方向。代价是精度略低（梯度方向不一定最优），收益是省掉了整个 ESDF 构建过程。

设计决策：如果规划器是 Fast-Planner / FUEL 系列 → 需要 ESDF（用 voxblox / FIESTA）；如果是 EGO-Planner 系列 → 不需要 ESDF（直接用占据栅格）；如果是 GCOPTER / SUPER 系列 → 不需要 ESDF（用安全走廊）。

voxblox 的多线程架构¶

voxblox 的线程模型对理解其工程实现至关重要：

voxblox 线程模型:

ROS 回调线程             集成线程                 ESDF 更新线程
    |                       |                         |
    |  收到深度图 + 位姿     |                         |
    | --------------------> |                         |
    |                       |  TSDF 集成               |
    |                       |  (merged integrator)     |
    |                       |  ---------------------> |
    |                       |                         | ESDF 双波前
    |                       |                         | (raise + lower)
    |                       |                         |
    |                       |                         | 发布 ESDF
    |                       |                         | (rviz 可视化)

ThreadSafeIndex 管理多线程对同一 Layer 的并发访问。核心策略是**按 Block 粒度加锁**——不同 Block 可以并行处理，同一 Block 内串行。这个粒度选择是一个经典的锁粒度权衡： - 太粗（整个 Layer 一把锁）→ 没有并行度 - 太细（每个体素一把锁）→ 锁的数量和开销太大 - Block 粒度（4096 体素一把锁）→ 兼顾并行度和锁开销

衍生项目——voxblox 衍生出一系列子图管理方案： - voxgraph（Reijgwart 等, IROS 2020）：在 voxblox 上叠加位姿图，子图拼接 + 回环优化后的地图一致性维护 - c-blox（Schmid 等, RA-L 2022）：voxblox 的子图管理器——每个子图独立建 TSDF，回环后重新集成 - panoptic mapping（Schmid 等, 3DV 2022）：在 voxblox 基础上加入语义实例分割——每个物体一个独立的 TSDF 子图

这些衍生项目展示了体素哈希的可扩展性——相同的 Layer/Block 抽象可以承载 TSDF、ESDF、语义标签等多种类型的体素数据。

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：查询未分配 Block 区域返回 nullptr 未检查

错误做法：auto* voxel = layer.getVoxelPtrByCoordinates(point); 然后直接读 voxel->distance
现象/后果：查询体素哈希中尚未分配的区域（从未被观测到），返回 nullptr，解引用段错误
根本原因：体素哈希只为有观测数据的区域分配 Block，大量空间没有 Block
正确做法：if (voxel != nullptr) { ... } 或 getDistanceAtPosition 返回 bool

陷阱 2（思维陷阱）：以为 ESDF 精度只取决于体素大小

错误做法：只关注体素分辨率，忽略准欧氏 vs 精确欧氏的差异
现象/后果：即使体素足够小（0.05m），voxblox ESDF 在对角方向仍有约 8% 误差
根本原因：准欧氏距离沿网格路径累积步长（1/$\sqrt{2}$/$\sqrt{3}$），对角方向路径不是直线
正确做法：如果需要精确 ESDF，用 nvblox（PBA 算法）或 FIESTA（精确欧氏）

练习¶

ESDF 传播手推：一个 5x5 的 2D 网格，中心 (2,2) 是障碍（ESDF=0）。用准欧氏距离（面=1，角=$\sqrt{2}$）手动传播 ESDF 值到所有格子。对比精确欧氏距离，最大误差出现在哪里？
梯度计算：对于上题的 ESDF 网格，在 (3,3)（对角方向）用有限差分计算 $\nabla d$。方向是否指向远离障碍的方向？

§D6.5 FIESTA——增量 ESDF 的极致优化 ⭐⭐⭐¶

动机：voxblox ESDF 的低效之处¶

voxblox 的 ESDF 更新有一个效率瓶颈：即使只有少数体素的占据状态发生变化，波前传播仍会触及大量体素。RAISE 阶段需要沿 parent 方向逐层传播失效信号——如果一个深处的障碍消失了，失效信号要传播到所有以它为最近障碍的体素，可能波及大片区域。

FIESTA（Han, Gao 等, HKUST, IROS 2019）针对这个问题做了两个算法层面的创新：双队列分离和双向链表索引。

核心创新一：双队列分离¶

FIESTA 将 ESDF 更新分为两个**独立的 BFS 队列**——InsertQueue（障碍出现，距离减小）和 DeleteQueue（障碍消失，需要重新找最近障碍）。

在 voxblox 中，raise 和 lower 是交替执行的：先 raise 所有失效体素，再 lower 所有更新。当同时有障碍出现和消失时，两个波前会互相干扰。FIESTA 证明：当插入和删除分离处理时，每个体素最多被访问常数次——均摊 O(1) 复杂度。

核心创新二：双向链表索引¶

每个障碍体素维护一个双向链表，链接所有以该障碍为最近障碍的体素。当障碍被删除时，不需要"盲目传播"失效信号——直接遍历链表就知道哪些体素受影响：

障碍体素 O 的双向链表:
  O.dependents → [V1] <-> [V2] <-> [V3] <-> [V4]

当 O 被删除(变为 free)时:
  遍历链表 → V1, V2, V3, V4 都需要重新计算最近障碍
  → 放入 DeleteQueue

这避免了 voxblox 中 raise 阶段的逐层传播——voxblox 需要沿 parent 方向一层一层扩散失效信号（类似广播"不确定"），FIESTA 直接精确定位受影响的体素。

性能对比¶

指标	voxblox ESDF	FIESTA
平均更新时间（室内 30m x 30m x 5m, 0.1m）	约 15 ms	约 3 ms（5 倍加速）
最大更新时间	约 80 ms	约 10 ms（8 倍加速）
距离精度	准欧氏（约 8% 误差）	精确欧氏（0 误差）

FIESTA 不仅更快，还更准——它计算精确欧氏距离，而非 voxblox 的准欧氏近似。

FIESTA 的局限：（1）双向链表增加了每体素的内存开销（两个额外指针）；（2）不提供 TSDF——直接从占据网格构建 ESDF，没有 Marching Cubes 网格重建能力；（3）与 HKUST 的 Fast-Planner 系列深度耦合。

选型建议：只需要 ESDF 而不需要 TSDF 网格 → FIESTA；还需要表面重建/可视化 → voxblox。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为 FIESTA 是 voxblox 的"升级版"

错误做法：在需要 TSDF 网格的项目中用 FIESTA 替代 voxblox
现象/后果：FIESTA 不提供 TSDF 层，无法做表面重建和 Marching Cubes 可视化
根本原因：FIESTA 跳过了 TSDF，直接从占据网格构建 ESDF——它和 voxblox 是互补的，不是替代的
正确做法：根据是否需要 TSDF 选择：需要 → voxblox，不需要 → FIESTA

练习¶

双向链表分析：画出一个 3x3 网格中，中心是障碍，周围 8 格各自的 closest_obstacle 指向和 DLL 链接关系。当中心障碍消失时，哪些体素需要重新计算？
复杂度对比：在一个 $N \times N \times N$ 的网格中，有 $M$ 个障碍物。voxblox 的 RAISE 阶段最坏情况下需要遍历多少体素？FIESTA 的 DeleteQueue 呢？

§D6.6 Ewok 环形缓冲——嵌入式友好的局部地图 ⭐⭐¶

动机：零拷贝滑窗¶

无人机向前飞行时，需要一个跟随它的局部地图窗口。新体素不断进入前方，旧体素从后方离开。如果使用普通数组，移动窗口需要**拷贝整个数组**——3D 数据量巨大时代价不可接受。

Ewok（Usenko 等, TUM, IROS 2017）引入了**3D 环形缓冲**——通过"改变起始偏移量而非移动数据"实现零拷贝滑窗。

核心机制：1D 环形缓冲到 3D¶

**1D 环形缓冲**的核心思想：物理数组不动，逻辑起点通过偏移量移动。

1D 环形缓冲(大小 8):

逻辑: [0] [1] [2] [3] [4] [5] [6] [7]
物理: [5] [6] [7] [0] [1] [2] [3] [4]    ← offset = 3
                   ^
                   逻辑位置 0 从物理位置 3 开始

映射: physical = (logical + offset) % 8
如果大小是 2 的幂: physical = (logical + offset) & 7   ← 位与!

滑窗移动: offset += 1   ← O(1), 无需拷贝!
物理位置 3+1=4 被新数据覆盖(旧的逻辑位置 0 的数据被丢弃)

Ewok 将这个思想推广到 3D——三个方向各自维护一个偏移量：

// Ewok 3D 环形缓冲核心 (简化版)
template <int POW>   // POW = log2(N), 如 POW=7 -> N=128
class RaycastRingBuffer {
    static constexpr int N = (1 << POW);        // 128
    static constexpr int N_MASK = N - 1;         // 127 = 0b01111111
    static constexpr int N3 = N * N * N;         // 2,097,152

    std::array<int16_t, N3> data_;   // 占据计数
    Eigen::Vector3i offset_;         // 当前偏移

    // O(1) 索引: 位与替代取模, 无分支
    int getLinearIndex(int ix, int iy, int iz) const {
        return ((ix + offset_.x()) & N_MASK) * N * N +
               ((iy + offset_.y()) & N_MASK) * N +
               ((iz + offset_.z()) & N_MASK);
    }
};

编译期常量的好处：（1）N_MASK 的位与替代取模——避免了昂贵的除法指令；（2）N3 编译期已知——编译器可以优化循环展开；（3）数组大小固定——栈上分配或编译期确定的堆分配。

滑窗移动：当无人机移动 1 个体素（如沿 x 轴正方向），只需 offset_x += 1，然后清零新进入窗口的 N x N 切片——整个滑窗移动 O($N^2$)，远好于 O($N^3$) 的完整拷贝。

可分离 EDT¶

Ewok 在环形缓冲之上计算 EDT（Euclidean Distance Transform），使碰撞检查变为一次 O(1) 的距离值查询。

可分离变换的核心思想：3D EDT 可以分解为三次独立的 1D 变换（Maurer 等, 2003）。这个分解之所以成立，是因为欧氏距离的平方可以按维度分解：

\[ d^2(\mathbf{p}, \mathbf{q}) = (p_x - q_x)^2 + (p_y - q_y)^2 + (p_z - q_z)^2 \]

每个维度的贡献是独立的。因此可以先沿 x 轴对每一列计算 1D EDT，再沿 y 轴对结果计算 1D EDT，最后沿 z 轴做第三次——三步合成精确的 3D EDT。总复杂度 O($N^3$)——线性于体素总数（每个体素被访问恰好 3 次，每次 O(1)）。

可分离 EDT 算法 (Maurer 2003):

Step 1: 沿 x 轴, 对每一条 (y,z) 固定的列计算 1D EDT
        → 得到每个体素在 x 方向上到最近障碍的距离平方

Step 2: 沿 y 轴, 对 Step 1 结果的每一条 (x,z) 固定的列计算 1D EDT
        → 得到 xy 平面内到最近障碍的距离平方

Step 3: 沿 z 轴, 对 Step 2 结果做最后一次 1D EDT
        → 得到完整 3D 到最近障碍的距离平方

最后: 对所有体素取平方根 → 精确欧氏距离

Ewok EDT 在环形缓冲上的特殊处理：由于环形缓冲的"环绕"特性，1D 变换需要处理物理地址的不连续——标准 EDT 假设数组元素在内存中连续排列，但环形缓冲中逻辑上相邻的元素可能在物理上被偏移量"环绕"到数组另一端。Ewok 的解法是在 EDT 循环中用 (i + offset) & mask 做索引转换，确保按逻辑顺序访问数据。

与 voxblox ESDF 的关键区别：Ewok 的 EDT 计算的是**精确欧氏距离**（通过可分离变换），没有 voxblox 准欧氏距离约 8% 的误差。但 EDT 只提供**无符号**距离（不区分障碍内外），而 voxblox ESDF 提供**有符号**距离。对于简单碰撞检查（距离 < 阈值 → 碰撞），EDT 足够；对于需要区分"自由空间"和"穿透深度"的轨迹优化器，需要有符号距离。

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：环形缓冲大小不是 2 的幂

错误做法：设 $N = 100$
现象/后果：位与运算 & (N-1) 不等价于取模 % N（只有 $N$ 是 2 的幂时才等价），索引错乱
根本原因：$(100-1) = 99 = 0b01100011$，位与结果不等于模 100
正确做法：$N$ 必须是 2 的幂（64, 128, 256 等），此时 $N-1$ 的二进制全为 1

陷阱 2（思维陷阱）：以为环形缓冲的 EDT 和 voxblox 的 ESDF 等价

错误做法：把 Ewok 的 EDT 当作 ESDF 输入 Fast-Planner
现象/后果：EDT 只有正值（不区分障碍内外），而 Fast-Planner 需要有符号距离
根本原因：EDT = |ESDF|，丢失了符号信息——无法区分"距障碍 0.3m 的自由空间"和"深入障碍 0.3m 的穿透区域"
正确做法：如果规划器需要有符号距离，用 voxblox/FIESTA；Ewok 的 EDT 适合简单的碰撞检查（距离 < 阈值 → 碰撞）

练习¶

位运算验证：手动计算 $N=8$（$\text{MASK}=7=0b111$）时，逻辑索引 3 + 偏移 5 的物理索引。用取模和位与两种方式验证结果一致。
滑窗效率：$N=128$ 的 3D 环形缓冲，每个体素 2 bytes。总内存是多少 MB？滑窗移动一步清零一个 128x128 的切片需要写多少 bytes？

§D6.7 nvblox——GPU 加速的体素建图 ⭐⭐⭐¶

动机：把 voxblox 搬到 GPU¶

nvblox（Millane 等, NVIDIA/ETH, ICRA 2024）可以理解为 voxblox 的 CUDA 移植 + 算法改进。核心加速来自两个方面：（1）TSDF 投影集成并行化——每个 CUDA thread 处理一个体素；（2）用 Parallel Banding Algorithm（PBA）替代 BFS 波前计算精确 ESDF。

GPU 体素哈希¶

nvblox 把体素哈希搬到 GPU 上，线程组织方式是：每个 CUDA block 处理一个 VoxelBlock（$8^3$ 体素），每个 CUDA thread 处理一个 voxel。

为什么八叉树不适合 GPU？ GPU 的执行模型要求大量线程执行相同指令（SIMT）。八叉树的指针式递归遍历会导致 warp divergence（同一 warp 内的 32 个线程走不同分支），且指针跳跃导致内存访问不规则（无法合并访问）。体素哈希的 Block 内连续内存天然支持 GPU 的 coalesced access。

PBA 精确 ESDF¶

nvblox 不使用 voxblox 的 BFS 波前，而是使用基于 PBA 的方法计算**精确** ESDF——消除了准欧氏距离的约 8% 误差。PBA 的核心思想类似可分离 EDT：先在每个 Block 内部沿三个轴扫描，再通过 Block 间边界交换传播信息。

性能数字¶

voxblox (CPU i9)  vs  nvblox (GPU RTX 3090):

                      voxblox      nvblox      加速比
TSDF 集成 (0.05m):   56.2 ms      0.32 ms     177x
ESDF 更新 (0.1m):    12.1 ms      0.39 ms      31x
ESDF 精度:           准欧氏(~8%)  精确欧氏     更准

Isaac ROS 集成与人体分割¶

nvblox 作为 NVIDIA Isaac ROS 生态的核心组件，提供以下输出： - Nav2 兼容的 2D costmap（通过 ESDF 投影到水平面） - Marching Cubes 三角网格（用于 RViz 可视化） - 3D 占据网格（用于碰撞检查） - ESDF Layer（用于轨迹优化）

人体分割与动态障碍处理——nvblox 的 Isaac ROS 集成包含一个独特功能：

动态障碍处理流程:

1. RGB 图像 → PeopleSemSegNet (DNN) → 人体分割掩码
2. 深度图 x 人体掩码 → 分离为:
   - 静态环境深度图 (无人区域)
   - 动态物体深度图 (人体区域)
3. 静态深度 → 主 TSDF (永久地图)
4. 动态深度 → 人体 TSDF (短期地图, 快速衰减)
5. 两个 TSDF 合并 → 统一 ESDF → costmap

优势: 人走开后, 人体 TSDF 快速衰减 → costmap 自动清除
     无需 voxblox 的 raise 波前来"擦除"旧障碍

这种"语义分割驱动的双 TSDF"架构是 nvblox 相比 voxblox 的一个重要差异化功能——它利用 GPU 同时运行 DNN 推理和体素建图，把语义理解和几何建图融合到同一条流水线中。

Jetson 平台性能¶

Jetson AGX Orin (嵌入式):

TSDF 集成 (0.01m):  ~33 ms → 30 Hz   ← 可用
ESDF 更新 (0.02m):  ~15 ms            ← 可用
总计:               ~48 ms → 约 20 Hz

对比 voxblox (CPU):
  同等分辨率: 无法实时 (>100 ms/帧)

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：在无 NVIDIA GPU 的嵌入式平台上使用 nvblox

错误做法：在 ARM CPU 上编译运行 nvblox
现象/后果：nvblox 的核心算法是 CUDA kernel，没有 GPU 回退路径，无法运行
根本原因：nvblox 是 GPU-first 设计，不是 CPU 库的 GPU 加速版
正确做法：无 GPU → 用 voxblox（CPU）或 FIESTA（CPU）；有 Jetson → nvblox

练习¶

GPU 线程映射：假设有 500 个 VoxelBlock 需要更新，每个 Block $8^3=512$ 体素。CUDA kernel 应该用多少 grid blocks？每个 block 多少 threads？总共启动多少 threads？
加速比分析：voxblox TSDF 集成 56.2ms，nvblox 0.32ms，加速比 177x。这个加速比合理吗？RTX 3090 有多少 CUDA 核心？理论峰值加速比是多少？为什么实际加速比小于理论值？

§D6.8 ROG-Map——2024 年 LiDAR 无人机事实标准 ⭐⭐⭐¶

动机：0.05m 分辨率下 50 Hz 维护 30m x 30m x 12m 局部地图¶

ROG-Map（Ren 等, HKU MaRS, IROS 2024）的设计目标极其具体：在笔记本级 CPU（Intel i7-1265U）上，以 0.05m 分辨率、50 Hz 频率维护一个 30m x 30m x 12m 的局部地图，包括正确的增量膨胀/去膨胀。这个指标是 SUPER 系统（Science Robotics 2025）在 20 m/s 下安全飞行的地图层保证。

三大设计¶

设计一：零拷贝滑窗（继承 Ewok 思想）。 ROG-Map 使用与 Ewok 类似的环形缓冲滑窗，但增加了多层支持。SlidingMap 基类为 ProbMap（概率层，0.1m 分辨率）和 InfMap（膨胀层，0.2m 分辨率）提供统一索引。细层和粗层按 2 的幂对齐：ProbMap[i,j,k] 对应 InfMap[i/2, j/2, k/2]。

设计二：计数器增量膨胀——核心创新。 这是 ROG-Map 最值得深入理解的算法，也是它与所有前序方法的本质区别。

为什么朴素膨胀无法正确"去膨胀"？回顾 §D6.1 的例子——两个障碍 A 和 B 共同膨胀了体素 X，当 A 消失时，朴素方案清除了 X 的膨胀，但 B 仍在膨胀范围内。

ROG-Map 的解决方案是：每个膨胀体素存一个 uint8_t 计数器。新占据体素出现时，其 $k^3$ 邻居计数全部 +1；消失时全部 -1。计数器 > 0 → 膨胀有效；计数器 = 0 → 撤销膨胀。

正确性证明：设膨胀半径为 $r$，体素 $v$ 的膨胀计数器 $C(v) = |\{o \in \text{occupied} : \|v - o\|_\infty \leq r\}|$。则 $C(v) > 0 \iff \exists\, o \in \text{occupied} : \|v - o\|_\infty \leq r \iff v$ 应该被膨胀。计数器恰好等于"有多少个占据体素贡献了 $v$ 的膨胀"。

增量性：每帧只处理状态变化的体素。典型 LiDAR 帧约 100-500 个变化体素，每个体素膨胀代价 $k^3$（$k=5$ → 125 次 uint8_t 加法），总计约 62500 次操作。实测：0.05m 分辨率，30m x 30m x 12m 地图，膨胀时间仅 0.66 ms。

让我们用一个详细的数值例子走一遍完整的计数器膨胀流程：

计数器增量膨胀完整示例 (2D 简化, 膨胀半径 = 2 格):

初始状态: 所有计数器 = 0

Step 1: 障碍 A 出现在 (5,5)
  for dx in [-2, +2]:
    for dy in [-2, +2]:
      counter[5+dx, 5+dy] += 1

  → (3,3) to (7,7) 的 5x5=25 个体素计数器各 +1
  → 所有值 = 1

Step 2: 障碍 B 出现在 (7,5)
  for dx in [-2, +2]:
    for dy in [-2, +2]:
      counter[7+dx, 5+dy] += 1

  → (5,3) to (9,7) 的 25 个体素计数器各 +1
  → 重叠区域 (5,3)-(7,7) 的 3x5=15 个体素计数器 = 2
  → 其余 A 独有区域计数 = 1, B 独有区域计数 = 1

Step 3: 障碍 A 消失
  for dx in [-2, +2]:
    for dy in [-2, +2]:
      counter[5+dx, 5+dy] -= 1

  → 重叠区域计数从 2 降到 1  → 膨胀保留! 正确!
  → A 独有区域计数从 1 降到 0 → 膨胀撤销! 正确!
  → B 独有区域不受影响, 计数仍 = 1  → 正确!

本质洞察：计数器膨胀的本质是**引用计数**（Reference Counting）——和 C++ 的 shared_ptr 是同一个思想。每个障碍物对膨胀体素"加一个引用"，消失时"释放引用"。引用计数降为零时才真正释放（撤销膨胀）。这个看似简单的设计，是所有 O($k^3$) 方案中**唯一**能正确处理动态去膨胀的方案。替代方案——每帧从头对所有 $N$ 个障碍重新膨胀——是 O($Nk^3$) 的全量操作，在 $N=10000$ 时约需 20+ ms，无法满足 50 Hz 要求。

设计三：双分辨率。 ProbMap 用 0.1m 分辨率做概率占据估计和射线投射更新；InfMap 用 0.2m 分辨率做膨胀——粗分辨率降低了膨胀的计算量和内存。两层通过 $2\times$ 降采样对齐。对齐保证：InfMap[i,j,k] 覆盖 ProbMap[2i:2i+1, 2j:2j+1, 2k:2k+1]，只要 8 个 ProbMap 格子中任一个是 occupied → InfMap 对应格子也是。

ROG-Map 与 SUPER 系统的集成¶

ROG-Map 在 SUPER 系统中的角色非常具体：

SUPER 系统数据流:

FAST-LIO2 --> 位姿 + 世界坐标系点云
    |
    v
ROG-Map --> 膨胀占据栅格 + 前沿信息
    |
    +---> A* / RRT* 路径搜索 (在 ProbMap 上)
    |       |
    |       v
    |   CIRI 安全飞行走廊 (构型空间膨胀)
    |       |
    |       v
    |   MINCO 轨迹优化 (primary + backup 双轨迹)
    |
    +---> ikd-Tree (FAST-LIO2 内部, 共享)
            |
            v
        kNN 碰撞查询 (Bubble Planner 安全检查)

注意一个关键细节：ROG-Map 的输入点云**必须是世界坐标系**。ROG-Map 不使用 ROS 的 frame_id 或 /tf——它假设所有输入点云已经被 FAST-LIO2 变换到世界系。如果你把 LiDAR 原始点云（传感器坐标系）直接送入，地图会建在错误位置。

ROG-Map 实测性能¶

硬件: Intel i7-1265U (笔记本级 CPU)
分辨率: 0.05m, 地图: 30m x 30m x 12m

                   平均时间    占 20ms 帧时间
点云插入:          2.81 ms     14.1%
概率更新:          1.62 ms      8.1%
膨胀更新:          0.66 ms      3.3%   ← 计数器膨胀的极致效率
前沿提取:          0.87 ms      4.4%
总计:              5.96 ms     29.8%

剩余 70.2%: 留给规划器 (CIRI + MINCO)

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：输入点云不在世界坐标系

错误做法：把 LiDAR 原始点云（传感器坐标系）直接送入 ROG-Map
现象/后果：地图建在错误位置，障碍物漂移
根本原因：ROG-Map 不使用 ROS 的 frame_id 或 /tf，它假设所有输入点云已被 FAST-LIO2 变换到世界系
正确做法：确保点云经过 FAST-LIO2 的位姿变换后再输入

陷阱 2（概念误区）：以为计数器膨胀等价于重新膨胀

错误做法：每帧从头对所有障碍物重新计算膨胀
现象/后果：$N$ 个占据体素 x $k^3$ 邻居 = O($Nk^3$)，当 $N=10000$ 时约 $10^6$ 次操作（20+ ms）
根本原因：重新膨胀是 O($Nk^3$) 全量操作，计数器膨胀是 O($\Delta N \cdot k^3$) 增量操作（$\Delta N$ 约 100-500）
正确做法：只处理状态变化的体素——新出现的 +1，消失的 -1

练习¶

计数器验证：在一个 7x7 的 2D 网格中（膨胀半径 = 2 格），按顺序插入障碍 A=(3,3)、B=(5,3)。画出每步的计数器值。然后删除 A，验证重叠区域的计数器是否正确。
内存估算：ROG-Map 在 0.05m 分辨率下，30m x 30m x 12m 的 ProbMap（每体素 2 bytes）和 InfMap（每体素 1 byte, 0.2m 分辨率）各占多少 MB？

§D6.9 ikd-Tree——SLAM 与规划的共享数据结构 ⭐⭐⭐¶

动机：一棵树服务两个子系统¶

ikd-Tree（Cai, Xu, Zhang, HKU MaRS, RA-L 2022）是**唯一在 SLAM 和规划中同时被复用的数据结构**。传统做法是 SLAM 维护自己的 kd 树（用于 ICP 最近邻匹配），规划器维护自己的占据栅格（用于碰撞检查）——两者之间需要数据拷贝和格式转换。ikd-Tree 打破了这个壁垒：

传统架构:
  SLAM 前端 → [SLAM kd 树] → 点云地图 → [拷贝+转换] → [规划器数据结构]
  开销: 2x 内存, 数据拷贝延迟

ikd-Tree 架构:
  FAST-LIO2 → [ikd-Tree] ← SLAM kNN 查询 (ICP)
                    ^
                    +---- 规划器 kNN 碰撞查询 (Bubble Planner)
  开销: 1x 内存, 零拷贝, 零延迟

本质洞察：ikd-Tree 的双重角色揭示了一个更深层的设计原则——SLAM 和规划不应该维护各自独立的世界模型。当它们共享同一个数据结构时，不仅节省内存和拷贝开销，还避免了两个世界模型之间的不一致（SLAM 已知有障碍但规划器还没收到更新）。

在 FAST-LIO2 中的角色¶

ikd-Tree 在 FAST-LIO2 的主循环中扮演两个角色：（1）为迭代卡尔曼滤波提供 ICP 最近邻匹配——每个新点找 5 个最近邻，用 PCA 拟合平面法线，构建点到平面残差；（2）增量维护点云地图——新点插入、旧点通过箱式删除实现滑窗裁剪。

在规划器中的角色¶

规划器直接复用 SLAM 的 ikd-Tree 做碰撞检查：

// 碰撞检查 (Bubble Planner / SUPER)
bool is_collision(PointType query, double safety_margin) {
    PointVector nearest(1);
    vector<float> sq_dists(1);
    ikd_tree.Nearest_Search(query, 1, nearest, sq_dists);
    return (sqrt(sq_dists[0]) < safety_margin);
}

增量操作¶

增量插入：从根节点遍历到叶节点，在叶节点插入新点。如果叶节点过大则分裂，然后向上检查平衡性。

惰性删除：不立即从树中移除节点，而是标记为"已删除"——O(log n) 操作。当已删除节点占比超过阈值（如 30%）时，触发子树重建，此时才物理移除。

箱式删除：删除一个 axis-aligned box 内的所有点。如果节点的包围盒完全在 box 内，标记整个子树为已删除（O(1)）；如果不相交则跳过（O(1)）；否则递归处理——这是 FAST-LIO2 实现滑窗裁剪的关键操作。

并行再平衡（$\alpha$-balanced）¶

ikd-Tree 的 $\alpha$ 平衡条件：对每个内部节点，$\max(|\text{left}|, |\text{right}|) \leq \alpha \cdot |\text{node}|$，典型 $\alpha = 0.7$。

违反时需要重建该子树。小子树在前台同步重建（通常 < 1ms）；大子树派发后台线程——主线程继续运行，新操作进入日志队列，对旧快照的查询仍然有效。后台线程重建完毕后，原子拼接新子树、回放日志中累积的操作。

时序:
  主线程: --[检测]--[快照]-----------[拼接+回放]--→
                         ^                    ^
  后台线程:              [---重建---]---------→

已知问题：双重释放 bug（Issue #20）¶

ikd-Tree 的多线程实现存在一个 use-after-free bug：后台重建线程和主线程同时访问同一个节点，后台释放旧节点后主线程仍持有旧指针。这是 C++ 并发编程中"引用计数缺失"的经典案例——没有使用 shared_ptr 追踪引用，快照释放时机不对。

教学价值：ikd-Tree 是展示"数据结构设计如何影响系统架构"的绝佳案例——一棵树服务两个子系统（SLAM + 规划），减少了系统复杂度，但也带来了并发控制的挑战。

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：在重建期间不加保护地修改树

错误做法：在后台重建线程运行时，主线程直接调用 Add_Points 修改树结构
现象/后果：偶发 segfault（后台线程读到半修改的节点）
根本原因：ikd-Tree 的并发保护依赖日志队列——新操作应进入日志而非直接修改树
正确做法：通过 ikd-Tree 的公开接口操作（内部已处理日志），不直接访问内部节点

陷阱 2（概念误区）：以为 ikd-Tree 提供 ESDF

错误做法：期望 ikd-Tree 像 voxblox 一样返回到最近障碍的距离场
现象/后果：ikd-Tree 提供的是 kNN 查询，不是全空间距离场
根本原因：kNN 查询是"给定一个点，找最近的 k 个邻居"（按需查询）；ESDF 是"预计算全空间每个体素到最近障碍的距离"（预计算全量）
正确做法：需要 ESDF 梯度 → 用 voxblox/FIESTA；只需碰撞检查 → ikd-Tree 的 kNN 足够

练习¶

架构对比：画出"传统架构"（SLAM kd 树 + 规划器占据栅格分离）和"ikd-Tree 架构"（共享）的数据流图。标出数据拷贝发生的位置和延迟。
$\alpha$ 平衡分析：一棵有 1000 个节点的 ikd-Tree，$\alpha = 0.7$。一个子树有 left=600, right=400。是否违反 $\alpha$ 平衡？如果 left=750 呢？

§D6.10 3DGS 用于无人机规划——Splat-Nav 与 SAFER-Splat ⭐⭐⭐⭐¶

动机：不需要体素化的全新范式¶

传统环境表示（OctoMap / voxblox / ROG-Map）都基于**规则网格**——空间被划分为体素，每个体素存储占据或距离值。3D Gaussian Splatting（3DGS）提出了完全不同的表示方式：空间由一组各向异性高斯椭球 $\{(\boldsymbol{\mu}_i, \boldsymbol{\Sigma}_i, \alpha_i)\}$ 表示，障碍是椭球的集合，自由空间是椭球之外的一切。

类比:
  传统表示 ≈ 光栅图像 (像素网格, 规则采样)
  3DGS 表示 ≈ 矢量图形 (基元集合, 参数化描述)

像的部分: 两者都能表示任意几何
不像的部分: 光栅/体素的精度受分辨率限制, 矢量/椭球没有分辨率概念
           但矢量/椭球的基元数量决定了表达能力和查询效率

椭球碰撞检测数学¶

点-椭球距离（Mahalanobis 距离）——给定点 $\mathbf{x}$ 和高斯椭球 $(\boldsymbol{\mu}, \boldsymbol{\Sigma})$：

\[ d_M(\mathbf{x}, \boldsymbol{\mu}, \boldsymbol{\Sigma}) = \sqrt{(\mathbf{x} - \boldsymbol{\mu})^\top \boldsymbol{\Sigma}^{-1} (\mathbf{x} - \boldsymbol{\mu})} \]

当 $d_M < k$（如 $k=3$，对应 99.7% 置信区间）时，认为点在椭球内部——碰撞。

椭球-椭球碰撞——两个椭球的碰撞可以转化为广义特征值问题：$\det(\mathbf{A}_1 - \lambda \mathbf{A}_2) = 0$，这是一个 O(1) 的矩阵运算——不需要迭代。

与 GJK 的关系：GJK 算法计算任意两个凸体的最小距离，需要 5-20 次迭代。椭球碰撞是 GJK 在椭球特例下的闭式解——利用了椭球的二次型代数结构。

Splat-Nav 安全走廊：从 splat 中心建 kd 树 → A* 搜索初始路径 → 沿路径膨胀凸多面体（切面约束来自最近椭球表面）→ Bezier 曲线优化（凸包性质保证轨迹在走廊内）。

SAFER-Splat：在线 3DGS + CBF 安全控制¶

SAFER-Splat（Chen 等, arXiv 2024）将 3DGS 与控制屏障函数（Control Barrier Function, CBF）结合。安全函数 $h(\mathbf{x}) = \min_i d_E(\mathbf{x}, E_i) - r_{\text{robot}}$，CBF 约束 $\dot{h} + \alpha(h) \geq 0$ 保证机器人始终与椭球保持安全距离。K-NN 剪枝将约束从 $10^5$ 降到 $10^2$-$10^3$，使 QP 实时可解。

范式转变的哲学意义¶

3DGS 展示的设计哲学:

  "学习型地图不意味着学习型规划器"

  传统偏见: 地图是神经网络(NeRF/3DGS) → 规划也必须用学习方法
  Splat-Nav 的洞见: 只要表示是数学规则的基元(椭球),
                    经典方法(凸走廊, CBF, QP)可以直接嫁接

  椭球 → Mahalanobis 距离 (闭式)
  椭球集合 → kd 树 + A* + 凸走廊 (经典算法)
  安全保证 → CBF-QP (控制理论)

  没有一步需要梯度下降或策略学习!

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为 3DGS 可以替代 voxblox/ROG-Map 用于在线建图

错误做法：在在线飞行系统中用 3DGS 替代体素地图
现象/后果：3DGS 训练需要大量 GPU 算力（通常离线训练数分钟），无法实时更新
根本原因：当前 3DGS 主要用于预建场景的规划（先建图后规划），在线增量训练仍是研究前沿
正确做法：在线飞行 → 体素地图（voxblox/ROG-Map）；预建场景导航 → 3DGS（Splat-Nav）

陷阱 2（思维陷阱）：以为截断阈值 $k=3$ 对所有椭球都安全

错误做法：对极端拉伸的椭球（一个主轴远大于其他两个）使用 $k=3$
现象/后果：沿长轴方向，$k=3$ 的截断距离可能远小于安全距离，导致碰撞泄漏
根本原因：$k=3$ 意味着覆盖 99.7% 的概率质量——但对极端各向异性的椭球，剩余 0.3% 的概率质量可能覆盖很长的空间范围
正确做法：对极端拉伸的椭球增大 $k$ 值，或引入额外的安全裕度

练习¶

Mahalanobis 距离计算：给定 $\boldsymbol{\mu} = (0,0,0)$，$\boldsymbol{\Sigma} = \text{diag}(1, 4, 0.25)$（沿 y 轴拉伸、沿 z 轴压缩的椭球）。计算点 $(1,0,0)$、$(0,2,0)$、$(0,0,0.5)$ 的 Mahalanobis 距离。哪些点在 $k=2$ 的椭球内？
设计思考：Splat-Nav 用 kd 树索引 splat 中心来做碰撞查询。如果场景中有 $10^5$ 个 splat，kNN 查询 50 个最近 splat 的复杂度是多少？和 voxblox 的 O(1) ESDF 查询相比，哪个更适合轨迹优化器的高频查询？

§D6.11 选型决策矩阵与接口对接 ⭐⭐¶

选型决策树¶

开始
  |
  +-- 传感器类型?
  |   +-- RGB-D 相机
  |   |   +-- 有 NVIDIA GPU (Jetson/桌面)?
  |   |   |   +-- 是 → nvblox (GPU 加速, Isaac ROS 集成)
  |   |   |   +-- 否 → 需要 ESDF 梯度?
  |   |   |       +-- 是 → voxblox 或 FIESTA
  |   |   |       |   +-- 需要 TSDF 网格重建 → voxblox
  |   |   |       |   +-- 只需 ESDF → FIESTA (更快更准)
  |   |   |       +-- 否 → OctoMap (简单够用)
  |   |
  |   +-- 3D LiDAR
  |   |   +-- 已有 FAST-LIO2 前端?
  |   |   |   +-- 是 → ROG-Map (滑窗 + 正确膨胀)
  |   |   |   |   +-- 规划器需要 kNN 碰撞查询 → ikd-Tree 直接复用
  |   |   |   +-- 否 → ROG-Map (需配合其他 SLAM)
  |   |
  |   +-- 预建 3DGS 场景
  |       +-- Splat-Nav (椭球碰撞, 无需体素化)
  |
  +-- 特殊约束?
      +-- 嵌入式极致内存 → Ewok 环形缓冲
      +-- 教学/原型验证 → OctoMap
      +-- 多机协同/大规模 → ROG-Map + ikd-Tree

综合性能对比表¶

方面	OctoMap	voxblox	FIESTA	Ewok	ROG-Map	nvblox	ikd-Tree
传感器	通用	RGB-D	RGB-D	RGB-D	LiDAR	RGB-D	LiDAR
分辨率(m)	0.1	0.1	0.1	0.1	0.05	0.01-0.1	点云
查询时间	O(log N)	O(1)	O(1)	O(1)	O(1)	O(1)	O(log n)
更新频率(Hz)	~10	~10	~20	~30	~50	~30	~100
ESDF 输出	外部	内建	内建	EDT	外部	内建	无(kNN)
滑窗	无	无(全局)	无	有	有	无	箱式删除
动态障碍	log-odds	raise波前	双队列	覆盖	计数器	人体分割	惰性删除
GPU 加速	无	无	无	无	无	有	无

与规划器的接口¶

接口 1：ESDF → Fast-Planner——voxblox/FIESTA 输出 $d(\mathbf{p})$ 和 $\nabla d(\mathbf{p})$，Fast-Planner 用于碰撞代价 $J_c = \int \max(0, d_{\text{safe}} - d)^3\,dt$ 及其梯度。

接口 2：占据栅格 → EGO-Planner——OctoMap/ROG-Map 输出占据状态。碰撞控制点通过 BFS 找最近自由空间参考点，碰撞梯度 = 控制点到参考点的方向。

接口 3：占据栅格 → 安全走廊 → MINCO——ROG-Map 膨胀栅格 → A*/RRT* 搜索引导路径 → FIRI/CIRI 安全走廊 → MINCO 轨迹优化（Bezier 凸包性质保证轨迹在走廊内）。

接口 4：ikd-Tree → kNN 碰撞 → SUPER——MINCO 生成轨迹后，对采样点做 kNN 碰撞查询验证安全性。碰撞点触发 backup 轨迹激活。

⚠️ 常见陷阱¶

陷阱 1（思维陷阱）：不看规划器就选地图表示

错误做法：先选"最好的"地图表示（如 nvblox），再选规划器
现象/后果：选了 nvblox（ESDF 输出）但规划器是 GCOPTER（不需要 ESDF，需要占据栅格构建走廊）——nvblox 的 ESDF 完全没用上，还浪费了 GPU 资源
根本原因：地图表示是为规划器服务的——应该先确定规划器需要什么查询接口，再选匹配的地图表示
正确做法：先确定规划器（Fast-Planner 需要 ESDF、EGO-Planner 需要占据、GCOPTER 需要走廊），再选地图

陷阱 2（编程陷阱）：混用不同坐标系的距离查询

错误做法：voxblox 在世界坐标系建图，但查询用的是机体坐标系的点
现象/后果：距离值完全错误——查询点和体素不在同一坐标系
根本原因：所有距离查询必须在同一坐标系中进行
正确做法：统一使用世界坐标系查询；或在查询前把机体坐标系的点变换到世界系

练习¶

选型实践：你的无人机装备 Ouster OS1-64 3D LiDAR + Pixhawk 飞控 + Intel NUC 机载电脑（无 GPU），使用 FAST-LIO2 做状态估计，规划器是 GCOPTER + MINCO。选择你的环境表示方案并论证。
跨章综合：画出 SUPER 系统的完整数据流图：从 LiDAR 原始数据 → FAST-LIO2 → ikd-Tree → ROG-Map → CIRI 安全走廊 → MINCO 轨迹 → 碰撞验证 → 执行。标出每个模块的输入/输出接口和数据格式。

概念地图（全章关联）¶

D6 概念关联图:

                    +-------------------------------------------+
                    |         传感器输入                            |
                    |  RGB-D 深度图  /  3D LiDAR 点云              |
                    +----------+--------------+-----------------+
                               |              |
                    +----------v--------+ +---v--------------+
                    |  体素化表示         | |  点云/基元表示      |
                    +-------------------+ +------------------+
                    | OctoMap (八叉树)   | | ikd-Tree         |
                    | voxblox (TSDF)    | |  +-- SLAM kNN    |
                    | FIESTA (ESDF)     | |  +-- 规划碰撞查询  |
                    | nvblox (GPU)      | |                  |
                    | Ewok (环形缓冲)    | | 3DGS (椭球)      |
                    | ROG-Map (计数器)   | |  +-- Splat-Nav   |
                    +--------+----------+ |  +-- SAFER-Splat |
                             |            +-------+----------+
                    +--------v--------------------v----------+
                    |           查询接口                        |
                    +----------------------------------------+
                    | d(x):    距离值    ← ESDF / EDT / kNN  |
                    | grad_d:  距离梯度  ← ESDF 三线性插值    |
                    | occ(x):  是否占据  ← 占据栅格           |
                    | inflate: 膨胀?    ← ROG-Map 计数器     |
                    +--------+-----------------+-------------+
                             |                 |
                    +--------v--------+ +------v--------------+
                    | D4 B样条轨迹优化  | | D5 MINCO+安全走廊    |
                    +-----------------+ +---------------------+
                    | Fast-Planner:   | | GCOPTER:            |
                    |  J_c = f(d,grad)| |  走廊 <-- occ       |
                    |                 | |  MINCO <-- 走廊     |
                    | EGO-Planner:    | |                     |
                    |  BFS 参考点     | | SUPER:              |
                    |  <-- occ        | |  CIRI + 双轨迹      |
                    +-----------------+ +---------------------+

时间线：各方法的历史演进¶

2013 ------ OctoMap (Hornung)
            |  八叉树 + log-odds + 剪枝压缩
            |  第一个通用 3D 建图框架
            |
2013 ------ Voxel Hashing (Niessner)
            |  实时 3D 重建 → 启发 voxblox
            |
2017 ------ voxblox (Oleynikova, ETH ASL)
            |  TSDF→ESDF; 体素哈希; CPU 实时
            |
2017 ------ Ewok (Usenko, TUM)
            |  环形缓冲 + EDT; 嵌入式
            |
2018 ------ Fast-Planner (Zhou, HKUST)
            |  首个实时使用 ESDF 梯度的轨迹优化器
            |  依赖 voxblox/FIESTA 提供 ESDF
            |
2019 ------ FIESTA (Han, HKUST)
            |  双队列增量 ESDF; 比 voxblox 更快更准
            |
2020 ------ EGO-Planner (Zhou, HKUST)
            |  绕开 ESDF! BFS 伪梯度
            |
2021 ------ ikd-Tree (Cai, HKU MaRS)
            |  SLAM+规划共享; FAST-LIO2 核心
            |
2022 ------ GCOPTER (Wang, ZJU FAST Lab)
            |  MINCO + 安全走廊 → 不需要 ESDF 梯度
            |
2024 ------ ROG-Map (Ren, HKU MaRS)
            |  计数器膨胀; 50 Hz; SUPER 标配
            |
2024 ------ nvblox (Millane, NVIDIA/ETH)
            |  GPU 加速 177x; Isaac ROS
            |
2025 ------ SUPER (Ren, HKU MaRS)
            |  ROG-Map + ikd-Tree + MINCO; 20 m/s
            |
2025 ------ Splat-Nav (Chen, Stanford)
            |  3DGS 椭球碰撞; 无需 ESDF
            |
            v
        未来: 语义 ESDF? 多机共享 3DGS? 在线增量训练?

关键概念深度辨析¶

辨析 1：TSDF vs ESDF vs EDT——三种距离场的本质区别¶

这三个概念是初学者最容易混淆的。它们都和"距离"有关，但来源不同、范围不同、用途不同：

TSDF (Truncated Signed Distance Field):
  来源: 直接从深度传感器数据构建 (投影集成+加权平均)
  范围: 截断到 [-delta, +delta], 只在表面附近有值
  用途: 表面重建 (Marching Cubes), ESDF 的前置数据结构
  特点: 多帧融合平滑噪声; 正值=表面外, 负值=表面内

ESDF (Euclidean Signed Distance Field):
  来源: 从 TSDF 或占据网格通过波前传播构建
  范围: 整个空间, 无截断
  用途: 轨迹优化的碰撞代价和梯度 (Fast-Planner 直接使用)
  特点: 有符号(区分内外); 每个体素存距离+梯度方向

EDT (Euclidean Distance Transform):
  来源: 从二值化占据网格通过可分离变换构建
  范围: 整个空间
  用途: 碰撞检查 (距离 < 阈值 -> 碰撞)
  特点: 无符号(不区分内外); 数学上 EDT = |ESDF|

关系链:
  深度图 --> TSDF -->(波前传播)--> ESDF  [voxblox/FIESTA]
  占据网格 -->(可分离变换)--> EDT        [Ewok/dynamicEDT3D]
  EDT ≈ |ESDF|  (EDT 丢失了符号信息)

如何选择：规划器需要有符号距离和梯度 → ESDF（voxblox/FIESTA）；只需碰撞检测 → EDT（Ewok）或 kNN（ikd-Tree）；需要表面重建/可视化 → TSDF + Marching Cubes（voxblox）。

辨析 2：体素大小的选择——经验法则¶

体素大小直接影响内存、速度和精度，是环境表示最重要的配置参数：

0.01m (1cm): 高精度桌面操作 → 必须用 GPU (nvblox)
  内存: 30m^3 空间 = 2.7x10^10 体素 → 只能稀疏存储
  适用: 机械臂抓取, 精密导航

0.05m (5cm): 高速飞行 → ROG-Map / SUPER
  内存: 30x30x12m = 4.3x10^7 体素 ≈ 170 MB (float)
  适用: 20 m/s 高速林地飞行 (需要精确避障)

0.1m (10cm): 标准室内/室外 → voxblox / FIESTA
  内存: 30x30x12m = 5.4x10^6 体素 ≈ 21 MB (float)
  适用: 2-5 m/s 中速飞行, 大部分教学/研究场景

0.2m (20cm): 大场景粗略 → OctoMap
  内存: 100x100x50m = 1.25x10^6 体素 ≈ 5 MB
  适用: 大范围探索, 初始全局规划

经验法则:
  体素大小 ≈ 机器人尺寸 / 10
  四旋翼直径 ~0.5m → 体素 ~0.05m
  安全距离 ~0.5m → 体素 ≤ 0.1m (至少 5 个体素的安全带)

辨析 3：全局地图 vs 局部地图——何时用哪个¶

全局地图 (OctoMap / voxblox):
  +-- 优: 完整环境信息, 支持全局规划和探索
  +-- 劣: 内存持续增长, SLAM 漂移影响一致性
  +-- 适用: 探索任务 (FUEL/TARE), 离线建图

局部滑窗地图 (Ewok / ROG-Map):
  +-- 优: 内存固定, 无漂移累积, 查询恒定时间
  +-- 劣: 无全局信息, 不适合全局规划
  +-- 适用: 反应式避障, 高速飞行 (SUPER)

混合策略 (典型实际系统):
  全局: ikd-Tree (FAST-LIO2 点云地图, 为全局路径提供粗略信息)
  局部: ROG-Map (膨胀占据栅格, 为局部轨迹优化提供精确信息)

本章常见误解汇总¶

误解	正确理解
TSDF 可以直接用于规划	TSDF 有截断范围（约 0.3m），规划器需要全空间 ESDF
voxblox ESDF 是精确欧氏距离	准欧氏距离，最大约 8% 误差；精确欧氏需要 FIESTA 或 nvblox
八叉树查询是 O(1)	O(log N)，且指针跳跃导致缓存不友好
环形缓冲大小可以任意设定	必须是 2 的幂，否则位与运算不等价于取模
ROG-Map 每帧从头膨胀所有障碍	增量操作：只处理状态变化的体素（计数器 +1 或 -1）
ikd-Tree 提供 ESDF 距离场	提供 kNN 查询（按需），不是全空间预计算距离场
3DGS 可以替代体素地图做在线建图	当前 3DGS 主要用于预建场景，在线增量训练仍是研究前沿
"分辨率越高越好"	分辨率、内存、速度是三角权衡；体素减半 → 体素数 8 倍

本章小结¶

术语速查表¶

术语	英文全称	一句话定义
TSDF	Truncated Signed Distance Field	截断有符号距离场：表面附近存储精确距离，截断范围外不存储
ESDF	Euclidean Signed Distance Field	欧氏有符号距离场：全空间每个体素到最近障碍的距离 + 梯度方向
EDT	Euclidean Distance Transform	欧氏距离变换：从二值占据图计算到最近障碍的距离（无符号）
SFC	Safe Flight Corridor	安全飞行走廊：一序列凸多面体，轨迹约束在其中
PBA	Parallel Banding Algorithm	并行带算法：GPU 上计算精确 EDT/ESDF
CBF	Control Barrier Function	控制屏障函数：保证安全集前向不变的约束
3DGS	3D Gaussian Splatting	3D 高斯溅射：用各向异性椭球集合表示场景
BFS	Breadth-First Search	广度优先搜索：ESDF 波前传播的实现方式
kNN	k-Nearest Neighbors	k 最近邻：点云碰撞查询的基本操作
NTTP	Non-Type Template Parameter	非类型模板参数：C++ 编译期常量，Ewok 用它固定缓冲区大小
DLL	Doubly Linked List	双向链表：FIESTA 的索引结构，追踪每个障碍的依赖体素
log-odds	Log-Odds Representation	对数几率表示：概率的对数比，把乘法变加法

知识点总表¶

编号	知识点	核心要点	对应节	难度
1	无人机 vs 地面环境表示	五个根本挑战（3D/ESDF/内存/滑窗/去膨胀）	§D6.1	⭐
2	OctoMap log-odds 更新	贝叶斯递推→log-odds 加法→clamping→三态	§D6.2	⭐⭐
3	TSDF 融合与截断	加权平均平滑噪声；零等值面=表面；ESDF 的前置	§D6.3	⭐⭐
4	voxblox 体素哈希 + 双波前 ESDF	O(1) 查询；RAISE + LOWER；准欧氏距离	§D6.4	⭐⭐⭐
5	FIESTA 双队列 + DLL	增量 ESDF 5x 加速；精确欧氏；DLL 精确定位受影响体素	§D6.5	⭐⭐⭐
6	Ewok 环形缓冲	位与零拷贝滑窗；可分离 EDT	§D6.6	⭐⭐
7	nvblox GPU 加速	TSDF 177x 加速；PBA 精确 ESDF；Isaac ROS	§D6.7	⭐⭐⭐
8	ROG-Map 计数器膨胀	增量去膨胀正确性；0.66ms 膨胀更新；50 Hz	§D6.8	⭐⭐⭐
9	ikd-Tree SLAM+规划共享	一棵树双角色；惰性删除；$\alpha$ 平衡并行重建	§D6.9	⭐⭐⭐
10	3DGS 椭球碰撞	Mahalanobis 距离；闭式碰撞；凸走廊 + CBF	§D6.10	⭐⭐⭐⭐
11	选型决策与接口对接	传感器→规划器→地图表示的决策链	§D6.11	⭐⭐

累积项目：本章新增模块¶

项目背景：贯穿无人机规控全流程的四旋翼自主飞行系统。前置章节已实现状态估计（FAST-LIO2）和轨迹生成（MINCO）。

本章新增——环境表示模块：

阶段 A：集成 voxblox，配置 TSDF→ESDF 流水线，在 RViz 中可视化 ESDF 切片
阶段 B：实现基于 ESDF 的碰撞代价梯度查询接口（提供给下一章的轨迹优化器）
阶段 C：集成 ROG-Map 替换 voxblox，比较两者在 LiDAR 场景下的更新频率和内存占用
阶段 D：实现 ikd-Tree 碰撞查询接口，与 ROG-Map 共存——ESDF 梯度来自 ROG-Map（或 FIESTA），kNN 碰撞验证来自 ikd-Tree

源码精读引导¶

精读优先级 A：核心代码文件¶

项目	文件路径	关键内容	预计时间
voxblox	`include/voxblox/core/layer.h`	`Layer<VoxelT>` 模板化体素层；体素哈希的完整 C++ 实现	2h
voxblox	`src/integrator/esdf_integrator.cc`	准欧氏双波前 ESDF；RAISE/LOWER 队列逻辑	3h
voxblox	`include/voxblox/core/block.h`	`Block<VoxelT>` 16^3 稠密数组；线性索引计算	1h
ROG-Map	`src/rog_map/`	`SlidingMap` 基类 + 计数器膨胀逻辑	3h
ikd-Tree	`ikd_Tree.h` + `ikd_Tree.cpp`	全部核心（约 2000 行）；Build/Add_Points/Delete_Point_Boxes/并行再平衡	4h
OctoMap	`include/octomap/OcTree.h`	八叉树节点 + log-odds 更新	2h
Ewok	`include/ewok/raycast_ring_buffer.h`	环形缓冲的 NTTP 模板；3D 索引的位与映射	2h

精读优先级 B：系统集成文件¶

项目	文件路径	关键内容	预计时间
voxblox	`src/integrator/tsdf_integrator.cc`	三种集成模式（simple/merged/fast）的实现差异	2h
FIESTA	`src/FIESTA.cpp`	双队列 BFS 主循环；DLL 操作	2h
nvblox	`nvblox/src/integrators/esdf_integrator.cu`	PBA ESDF 的 CUDA kernel	3h
SUPER	`src/super_planner/`	ROG-Map + MINCO + CIRI 的集成代码	2h

如何阅读 voxblox 源码¶

推荐阅读顺序:

1. 数据结构层:
   voxblox/include/voxblox/core/
   +-- common.h     ← 基础类型 (Point, BlockIndex, VoxelIndex)
   +-- voxel.h      ← TsdfVoxel, EsdfVoxel 的结构体定义
   +-- block.h      ← Block<VoxelT>: 16^3 稠密体素块
   +-- layer.h      ← Layer<VoxelT>: 全局体素哈希表

   关键问题:
   - BlockIndex 是怎么从世界坐标计算的?
   - Block 内的线性索引是 x-major 还是 z-major?
   - Layer 的 unordered_map 用了什么 hash 函数?

2. TSDF 集成:
   voxblox/src/integrator/tsdf_integrator.cc
   +-- SimpleTsdfIntegrator     ← 逐像素射线投射(最慢但最清晰)
   +-- MergedTsdfIntegrator     ← 分组合并(推荐理解)
   +-- FastTsdfIntegrator       ← 跳过远体素(最快)

   关键问题:
   - merged integrator 如何按 BlockIndex 分组像素?
   - TSDF 截断距离 delta 对结果有什么影响?
   - weight 的更新策略和 max_weight 的作用?

3. ESDF 生成:
   voxblox/src/integrator/esdf_integrator.cc
   +-- updateFromTsdfLayer()     ← 主入口
   +-- processRaiseSet()         ← RAISE 阶段(失效传播)
   +-- processLowerSet()         ← LOWER 阶段(距离传播)

   关键问题:
   - raise 和 lower 是先后执行还是交替?
   - parent_direction 是怎么更新的?
   - 准欧氏距离的具体计算方式(1/sqrt(2)/sqrt(3))?

延伸阅读¶

核心论文（按阅读优先级排序）¶

论文	阅读建议	难度
Oleynikova 2017 (voxblox)	精读 ESDF 双波前算法和体素哈希设计	⭐⭐⭐
Ren 2024 (ROG-Map)	精读计数器膨胀正确性论证和滑窗设计	⭐⭐⭐
Cai 2021 (ikd-Tree)	精读并行再平衡策略和箱式删除	⭐⭐⭐
Hornung 2013 (OctoMap)	速读概率模型和八叉树压缩	⭐⭐
Han 2019 (FIESTA)	精读双队列分离的复杂度证明	⭐⭐⭐
Chen 2025 (Splat-Nav)	速读椭球碰撞和凸走廊构建	⭐⭐⭐⭐

前沿工作¶

MAGiC-SLAM（CVPR 2025）：多代理高斯子图 + 中心服务器回环优化——多机 3DGS SLAM 的雏形
HAMMER（RA-L 2025）：异构机队共训 CLIP 语义 3DGS → 语言引导导航
VDB-EDT（Zhong 等, RA-L 2023）：基于 OpenVDB 的 3D EDT——利用 VDB 的稀疏层次结构
Voxblox++（Schmid 等, 2022）：全景分割叠加 voxblox——每个物体独立 TSDF 子图

本章与后续章节的关系¶

后续章节	关系	铺垫知识点
D7 感知引导规划与自主探索	本章的环境表示是 D7 探索策略的基础设施	ESDF 梯度、占据栅格、前沿提取
D4 B 样条轨迹优化	本章的 ESDF 接口是 Fast-Planner 碰撞代价的输入	$d(\mathbf{p})$、$\nabla d(\mathbf{p})$、三线性插值
D5 MINCO 与安全走廊	本章的占据栅格是安全走廊生成的输入	ROG-Map 膨胀栅格、ikd-Tree kNN

故障排查手册¶

故障 1：ESDF 距离值全为 0 或 NaN¶

项目	内容
症状	voxblox 的 ESDF 查询返回全 0 或 NaN，规划器报碰撞
可能原因	(a) TSDF 未收到深度数据——ESDF 无法从空 TSDF 生成；(b) 坐标系不一致——位姿和深度图不匹配
排查步骤	1. 检查 TSDF 的 block 数量是否非零；2. 在 RViz 中可视化 TSDF 切片；3. 检查 `/tf` 中传感器到世界系的变换是否正确
相关章节	§D6.3（TSDF 集成）、§D6.4（ESDF 生成）

故障 2：ROG-Map 膨胀层出现"残留膨胀"¶

项目	内容
症状	障碍物已经移走，但膨胀层仍显示该区域被膨胀
可能原因	(a) 占据概率未更新——射线投射没有覆盖该区域；(b) 计数器溢出——`uint8_t` 上溢后减法结果错误
排查步骤	1. 检查 ProbMap 中该体素的 log-odds 是否已降到 free 阈值以下；2. 打印 InfMap 中该体素的计数器值——如果异常大可能是溢出
相关章节	§D6.8（ROG-Map 计数器膨胀）

故障 3：ikd-Tree kNN 查询间歇性返回错误结果¶

项目	内容
症状	碰撞检查偶尔返回"安全"但实际会碰撞；或查询 segfault
可能原因	(a) 并行再平衡期间的 use-after-free（Issue #20）；(b) 箱式删除后树不平衡导致查询路径错误
排查步骤	1. 在 Debug 模式编译，检查是否有 AddressSanitizer 报告；2. 检查 $\alpha$ 平衡条件是否被频繁违反（日志中 rebuild 频率）；3. 临时禁用后台重建（改为同步重建），确认问题是否消失
相关章节	§D6.9（ikd-Tree 并行再平衡、已知 bug）

故障 4：voxblox 内存持续增长导致系统 OOM¶

项目	内容
症状	长时间飞行后内存超过系统限制，进程被 kill
可能原因	voxblox 是全局地图，不自动删除旧 Block
排查步骤	1. 监控 Block 数量随时间的增长曲线；2. 检查是否开启了 `clear_sphere_for_planning`（局部清除旧体素）；3. 考虑换用 ROG-Map 或 Ewok（滑窗地图，内存固定）
相关章节	§D6.4（voxblox 架构）、§D6.6（Ewok 环形缓冲）、§D6.8（ROG-Map 滑窗）

故障 5：nvblox ESDF 在 Jetson 上帧率不足¶

项目	内容
症状	nvblox 在 Jetson AGX Orin 上 ESDF 更新低于 10 Hz
可能原因	(a) 体素分辨率太高（0.01m）导致计算量过大；(b) GPU 内存不足触发页面交换；(c) 其他 CUDA 程序（如 DNN 推理）占用 GPU
排查步骤	1. `tegrastats` 查看 GPU 利用率和内存；2. 增大体素分辨率（0.02m → 0.05m）；3. 检查 `nvidia-smi` 或 `tegrastats` 中其他 GPU 进程
相关章节	§D6.7（nvblox 性能）

API 速查表¶

voxblox 核心 API¶

// 创建 ESDF 服务器
voxblox::EsdfServer esdf_server(nh, nh_private);

// 查询距离
double distance;
bool success = esdf_server.getEsdfMapPtr()
    ->getDistanceAtPosition(Eigen::Vector3d(x, y, z), &distance);

// 查询距离和梯度
double distance;
Eigen::Vector3d gradient;
bool success = esdf_server.getEsdfMapPtr()
    ->getDistanceAndGradientAtPosition(
        Eigen::Vector3d(x, y, z), &distance, &gradient);

// 获取 TSDF Layer
auto tsdf_layer = esdf_server.getTsdfMapPtr()->getTsdfLayerPtr();

// 保存/加载地图 (Protobuf)
esdf_server.saveMap("map.vxblx");
esdf_server.loadMap("map.vxblx");

ikd-Tree 核心 API¶

KD_TREE<PointType> ikd_tree;

// 构建
ikd_tree.Build(point_vector);

// 最近邻查询
PointVector nearest(k);
vector<float> sq_dists(k);
ikd_tree.Nearest_Search(query_point, k, nearest, sq_dists);

// 增量插入
ikd_tree.Add_Points(new_points, /* downsample */ true);

// 箱式删除 (滑窗裁剪)
BoxPointType box;
box.vertex_min = {x_min, y_min, z_min};
box.vertex_max = {x_max, y_max, z_max};
ikd_tree.Delete_Point_Boxes(box);

ROG-Map 关键接口¶

// 查询占据状态
bool is_occupied = rog_map.isOccupied(Eigen::Vector3d(x, y, z));

// 查询膨胀状态
bool is_inflated = rog_map.isInflated(Eigen::Vector3d(x, y, z));

// 获取前沿点 (探索用)
vector<Eigen::Vector3d> frontiers = rog_map.getFrontiers();

关键公式汇总¶

编号	公式	来源
1	$L(n \mid z_{1:t}) = L(n \mid z_{1:t-1}) + L(n \mid z_t) - L_0$	OctoMap log-odds
2	$d_{\text{new}} = \frac{d_{\text{old}} \cdot w_{\text{old}} + d_{\text{obs}} \cdot w_{\text{obs}}}{w_{\text{old}} + w_{\text{obs}}}$	TSDF 加权平均融合
3	$d(v,n) \in \{1, \sqrt{2}, \sqrt{3}\}$（面/棱/角邻居）	voxblox 准欧氏 ESDF
4	$C(v) =	{o \in \text{occupied} : \|v-o\|_\infty \leq r}
5	$d_M(\mathbf{x}) = \sqrt{(\mathbf{x}-\boldsymbol{\mu})^\top \boldsymbol{\Sigma}^{-1} (\mathbf{x}-\boldsymbol{\mu})}$	3DGS Mahalanobis
6	$J_c = \int_0^T \max(0, d_{\text{safe}} - d(\mathbf{p}))^3\,dt$	Fast-Planner 碰撞代价
7	$\frac{\partial J_c}{\partial \mathbf{p}} = -3\max(0, d_{\text{safe}}-d)^2 \nabla d(\mathbf{p})$	Fast-Planner 碰撞梯度
8	$h(\mathbf{x}) = \min_i d_E(\mathbf{x}, E_i) - r_{\text{robot}}$，$\dot{h} + \alpha(h) \geq 0$	SAFER-Splat CBF
9	`physical = (logical + offset) & (N-1)`（$N$ 为 2 的幂）	环形缓冲索引
10	$\max(	\text{left}

研究实践建议¶

初级（刚接触无人机环境表示）¶

跑通 OctoMap 的 ROS demo，用 RViz 可视化八叉树地图，理解三态语义
跑通 voxblox + Gazebo 的 ESDF 可视化，观察距离场的连续性和梯度方向
手推 log-odds 更新公式，理解 clamping 对动态环境的意义

中级（需要集成到系统中）¶

精读 voxblox 的 layer.h + esdf_integrator.cc，理解体素哈希和双波前 ESDF
精读 ROG-Map 的计数器膨胀代码，构造重叠膨胀测试验证正确性
集成 voxblox 或 ROG-Map 到你的规划系统，实测更新频率和内存占用

高级（从事环境表示研究）¶

精读 FIESTA 的双队列 BFS 复杂度证明，理解增量 ESDF 的理论下界
精读 nvblox 的 PBA CUDA kernel，理解 GPU 上精确 ESDF 的并行策略
研究 3DGS 在线增量训练的最新进展（MAGiC-SLAM、SplaTAM）
思考"统一碰撞抽象层"——能否设计一个接口同时兼容 ESDF 查询和 kNN 查询？

版本信息速查¶

库/框架	常用版本	依赖
OctoMap	1.9.x	PCL, ROS 1/2
voxblox	master (ROS 1) / foxy+ (ROS 2)	Eigen3, Protobuf, ROS
FIESTA	master	Eigen3, PCL, ROS 1
Ewok	master	Eigen3, PCL, ROS 1
ROG-Map	master (ROS 1) / SUPER (ROS 2)	Eigen3, PCL
nvblox	Isaac ROS 3.x	CUDA 11.8+, ROS 2 Humble+
ikd-Tree	FAST-LIO2 内置	无额外依赖

预计学习时间¶

模块	内容	时间
§D6.1-§D6.2	五个挑战 + OctoMap 概率模型	3h
§D6.3-§D6.4	TSDF + voxblox ESDF 流水线	5h
§D6.5	FIESTA 增量 ESDF	2h
§D6.6	Ewok 环形缓冲	2h
§D6.7	nvblox GPU 加速	3h
§D6.8	ROG-Map 计数器膨胀	3h
§D6.9	ikd-Tree SLAM+规划共享	3h
§D6.10	3DGS 规划化	2h
§D6.11	选型决策与接口	1h
练习与精读	A 型 + B 型 + 思考题	4h
总计		28h（约 2 周）

开放问题¶

开放问题	现状	难点
3DGS 在线增量训练	SplaTAM/MAGiC-SLAM 初步尝试	如何在飞行中实时训练高斯场？GPU 资源与规划竞争
ESDF 与 kNN 的统一碰撞接口	ROG-Map 用栅格、ikd-Tree 用 kNN，接口不同	能否设计统一的碰撞抽象层？
语义 ESDF	nvblox 有人体分割；更丰富语义（可降落区域）未解决	语义标签与距离场的融合方式
可变分辨率 ESDF	OctoMap 天然多分辨率；voxblox/nvblox 固定分辨率	多分辨率 ESDF 的波前传播一致性
多机共享地图	MAGiC-SLAM 初步尝试；带宽是瓶颈	高斯参数传输 vs 压缩特征
3DGS 无界高斯截断	$k=3$ → 99.7%；极端拉伸椭球仍可能泄漏	如何自适应选择截断阈值？

延伸阅读¶

本节按主题分类列出推荐文献和学习资源，帮助读者根据自身方向选择性深入。每条资源附简要说明和推荐阅读顺序。

体素地图与距离场基础¶

序号	资源	说明	推荐顺序
1	Hornung et al., "OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees," Autonomous Robots, 2013	OctoMap 的开山论文，概率占据模型与多分辨率八叉树的完整推导。读完后对照 §D6.2 的 log-odds 更新公式，理解 clamping 的工程选择	第 1 篇
2	Oleynikova et al., "Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning," IROS, 2017	voxblox 的核心论文，详细描述了 TSDF 融合和准欧氏 ESDF 的双波前传播算法。配合 §D6.3-§D6.4 精读，重点关注 Bucket Queue 的复杂度分析	第 2 篇
3	Han et al., "FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots," IROS, 2019	增量 ESDF 的双队列 BFS 算法，证明了 $O(\text{changed voxels})$ 的理论复杂度。对照 §D6.5 阅读，注意 insertionList 和 deletionList 的维护逻辑	第 3 篇

滑窗与高效地图¶

序号	资源	说明	推荐顺序
4	Usenko et al., "Real-Time Trajectory Replanning for MAVs using Uniform B-splines and a 3D Circular Buffer," IROS, 2017	Ewok 的环形缓冲区设计与 B-spline 轨迹优化的联合框架。理解 §D6.6 中 `physical = (logical + offset) & (N-1)` 的位运算索引	第 4 篇
5	Ren et al., "ROG-Map: An Efficient Robocentric Occupancy Grid Map for Large-scene and High-resolution LiDAR-based Motion Planning," 2024	ROG-Map 的计数器膨胀与自中心滑窗设计。重点关注 InfMap 中 $O(1)$ 膨胀/去膨胀的实现细节和正确性证明	第 5 篇

GPU 加速与大规模地图¶

序号	资源	说明	推荐顺序
6	Millane et al., "nvblox: GPU-Accelerated Incremental Signed Distance Field Mapping," ICRA, 2024	nvblox 的 PBA（Parallel Banding Algorithm）在 GPU 上实现精确 ESDF 的完整描述。对照 §D6.7 理解为什么 GPU 并行特别适合距离场计算	第 6 篇
7	Cao & Payandeh, "GPU-Accelerated Real-Time 3D TSDF Reconstruction," 相关工作综述	理解 TSDF 的 GPU 并行化范式，包括体素级并行和射线级并行的权衡	补充阅读

点云数据结构¶

序号	资源	说明	推荐顺序
8	Cai et al., "ikd-Tree: An Incremental KD Tree for Robotic Applications," 2022	ikd-Tree 的增量构建、并行再平衡和 $\alpha$-平衡条件的完整推导。配合 §D6.9 阅读，特别注意已知 bug 和并发问题	第 7 篇
9	Xu et al., "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry," TRO, 2022	ikd-Tree 的主要应用场景，理解 SLAM 系统如何利用增量 KD-Tree 进行在线点云管理	补充阅读

神经隐式与高斯表示¶

序号	资源	说明	推荐顺序
10	Kerbl et al., "3D Gaussian Splatting for Real-Time Radiance Field Rendering," SIGGRAPH, 2023	3DGS 的原始论文，理解高斯椭球参数化和 $\alpha$-blending 渲染。这是 §D6.10 所有工作的基础	第 8 篇
11	Chen et al., "SAFER-Splat: A Control Barrier Function for Safe Navigation with Online Gaussian Splatting Maps," 2024	将 3DGS 用于安全规划的 CBF 方法。对照 §D6.10 理解碰撞代价的 Mahalanobis 距离表达	第 9 篇
12	Tosi et al., "How NeRFs and 3D Gaussian Splatting are Reshaping SLAM," 综述 2024	全面梳理神经表示在 SLAM 中的应用现状，帮助把握研究前沿和发展方向	综述阅读

系统集成与规划¶

序号	资源	说明	推荐顺序
13	Zhou et al., "FUEL: Fast UAV Exploration using Incremental Frontier Structure and Hierarchical Planning," RAL, 2021	前沿探索与环境表示的协同，理解 ROG-Map 中 `getFrontiers()` 的应用背景	系统集成
14	Zhou et al., "EGO-Planner: An ESDF-free Gradient-based Local Planning Framework," RAL, 2021	无 ESDF 的碰撞梯度方案，对照 §D6.11 理解"是否一定需要 ESDF"	系统集成
15	Zhou et al., "Ego-Planner Revisited: Integrating a Robocentric Occupancy Grid Map," 2024	ROG-Map + EGO-Planner 的集成实例，完整展示选型决策如何落地	系统集成

在线教程与代码库¶

序号	资源	说明
16	OctoMap GitHub	官方实现，包含 ROS 集成示例和可视化工具
17	voxblox GitHub	ETH ASL 维护，附带 Gazebo 仿真 demo
18	nvblox GitHub	NVIDIA 官方 GPU 地图库，Isaac ROS 集成
19	ROG-Map GitHub	港大 MARS 实验室维护，含 ROS 1/2 接口
20	FAST-LIO2 GitHub	内含 ikd-Tree 实现，可独立提取使用

阅读建议：初学者按编号 1→5 顺序阅读体素地图线路；如果研究方向偏向 LiDAR SLAM，则优先阅读 8→9→5；如果关注神经隐式表示前沿，则优先阅读 10→11→12。无论哪条路线，建议最后都阅读 13→15 的系统集成文献，理解环境表示如何服务于上层规划。

本章小结¶

核心知识点回顾¶

本章从无人机自主飞行的实际需求出发，系统梳理了环境表示（Environment Representation）的核心问题、主流方案和前沿方向。回顾全章内容，我们可以提炼出以下关键认识：

第一，环境表示是规划的基础设施，而非独立模块。 本章开篇提出的五大挑战——体素分辨率与内存的矛盾、距离场的实时维护、动态障碍物的处理、计算资源约束、以及表示精度与规划效率的权衡——本质上都是"如何让规划器高效地获取碰撞信息"这一根本问题的不同切面。选择环境表示方案，本质上是在回答"规划器需要什么样的世界模型"。

第二，概率占据模型是理解所有方案的共同起点。 无论是 OctoMap 的八叉树、voxblox 的体素哈希、还是 ROG-Map 的三层结构，底层都建立在"传感器观测 → 占据概率更新 → 障碍物判定"这一概率框架之上。log-odds 的对数形式将乘法转化为加法，clamping 限制了历史观测的过度积累——这些设计选择贯穿了所有基于栅格的方案。

第三，ESDF 是连接离散栅格与连续优化的关键桥梁。 规划器（特别是基于梯度优化的方法）需要的不仅是"这里是否有障碍物"，更需要"离最近障碍物有多远"以及"远离障碍物的方向是什么"。ESDF 恰好提供了这两个信息（距离值和梯度），这也解释了为什么 voxblox、FIESTA、nvblox 都将 ESDF 生成作为核心功能。

第四，滑窗策略是资源受限平台的必然选择。 无人机的计算和内存资源有限，维护全局地图既不必要也不现实。Ewok 的环形缓冲、ROG-Map 的自中心滑窗、以及 ikd-Tree 的箱式删除，都是"只维护机器人周围局部环境"这一思想的不同实现。滑窗的代价是丢失全局信息，但对于反应式规划而言这通常是可接受的。

第五，GPU 并行为实时性开辟了新维度。 nvblox 的 PBA 算法表明，ESDF 计算本质上是高度可并行的——每个体素的距离更新只依赖于其邻域信息。在 GPU 硬件越来越普及的趋势下，计算瓶颈正在从"算法复杂度"转向"数据传输带宽"。

第六，3DGS 代表了环境表示从离散到连续的范式转换。 传统方法将世界离散化为体素再计算距离，3DGS 则直接用连续的高斯椭球描述几何。这种转换带来了全新的碰撞检测范式（Mahalanobis 距离）和安全约束形式（CBF），但也引入了无界高斯截断、在线训练等新挑战。

知识点总表¶

编号	知识点	难度	核心要点	典型应用
§D6.1	五大挑战	⭐	分辨率-内存矛盾、ESDF 实时性、动态环境、算力约束、精度-效率权衡	方案选型的评估框架
§D6.2	OctoMap 概率占据	⭐⭐	log-odds 更新、三态语义、clamping、多分辨率八叉树	全局建图、探索
§D6.3	TSDF 融合	⭐⭐	加权平均、截断距离、体素哈希	voxblox/nvblox 的输入层
§D6.4	voxblox ESDF	⭐⭐	准欧氏双波前、Bucket Queue、$O(n)$ 传播	梯度规划
§D6.5	FIESTA 增量 ESDF	⭐⭐⭐	双队列 BFS、$O(\text{changed})$ 增量更新	高频 ESDF 更新
§D6.6	Ewok 环形缓冲	⭐⭐	位运算索引、固定内存、平移高效	内存受限平台
§D6.7	nvblox GPU ESDF	⭐⭐⭐	PBA 并行、CUDA kernel、精确 ESDF	Jetson 平台
§D6.8	ROG-Map 计数器膨胀	⭐⭐	InfMap 计数器、$O(1)$ 膨胀/去膨胀、自中心滑窗	LiDAR 规划系统
§D6.9	ikd-Tree	⭐⭐⭐	增量 KD-Tree、$\alpha$-平衡、并行再平衡、箱式删除	SLAM+规划共享
§D6.10	3DGS 规划化	⭐⭐⭐⭐	Mahalanobis 碰撞、CBF 安全约束、神经隐式表示	视觉导航前沿
§D6.11	选型决策	⭐⭐	决策树、传感器-表示-规划器匹配、统一接口设计	系统集成

方案选型速查¶

以下决策矩阵帮助快速定位适合你场景的环境表示方案：

场景	推荐方案	理由
仿真环境快速原型	OctoMap	生态最成熟，ROS 集成完善，调试方便
视觉（RGBD）+ 梯度规划	voxblox / nvblox	TSDF→ESDF 流水线成熟，梯度信息丰富
有 GPU（Jetson）	nvblox	GPU 并行 ESDF，精确且实时
无 GPU + 需要 ESDF	FIESTA	增量更新高效，CPU 友好
LiDAR + 内存受限	ROG-Map	滑窗 + 计数器膨胀，内存固定
LiDAR SLAM 系统	ikd-Tree	与 FAST-LIO2 天然集成，增量维护高效
高精度视觉重建 + 规划	3DGS (SAFER-Splat)	连续表示，渲染质量高，但需离线训练
探索任务	ROG-Map + 前沿检测	内置前沿点提取，适合探索规划

全章思维导图¶

无人机环境表示
├── 基础概念
│   ├── 五大挑战（§D6.1）
│   └── 概率占据模型（§D6.2）
│       └── log-odds → clamping → 三态语义
├── ESDF 流水线
│   ├── TSDF 融合（§D6.3）→ 加权平均
│   ├── voxblox 准欧氏 ESDF（§D6.4）→ 双波前 + Bucket Queue
│   ├── FIESTA 增量 ESDF（§D6.5）→ 双队列 BFS
│   └── nvblox GPU ESDF（§D6.7）→ PBA 并行
├── 高效地图结构
│   ├── Ewok 环形缓冲（§D6.6）→ 固定内存
│   ├── ROG-Map 三层结构（§D6.8）→ 计数器膨胀
│   └── ikd-Tree 增量 KD-Tree（§D6.9）→ 点云管理
├── 前沿方向
│   └── 3DGS 规划化（§D6.10）→ Mahalanobis + CBF
└── 系统集成
    └── 选型决策（§D6.11）→ 传感器-表示-规划器匹配

后续章节关系¶

本章是无人机自主系统的"感知-表示"环节，为后续的规划与控制提供了必要的世界模型。以下说明本章与前后章节的关系：

方向	相关章节	衔接方式
前置	传感器与状态估计	本章假设传感器数据（深度图、点云）和位姿估计已经可用。TSDF 融合需要逐帧的深度图和传感器位姿；ikd-Tree 需要 SLAM 输出的点云
后续	运动规划（路径搜索与轨迹优化）	规划器从本章的环境表示中查询碰撞信息：ESDF 提供距离和梯度用于梯度优化，占据栅格提供二值碰撞检测用于采样规划，kNN 提供最近障碍物点用于安全约束
后续	安全与避障	CBF（控制屏障函数）需要实时距离查询——ESDF 的 `getDistanceAtPosition()` 或 3DGS 的 Mahalanobis 距离正是 CBF 的 $h(\mathbf{x})$
后续	探索与覆盖	前沿检测依赖占据栅格中"已知自由"与"未知"的边界。ROG-Map 的 `getFrontiers()` 直接服务于探索规划器
并行	多机协同	多机场景需要地图共享与融合。本章的开放问题"多机共享地图"直接关联多机协同章节

衔接提示：进入运动规划章节时，你需要回顾本章的 ESDF 查询接口（API 速查表）和碰撞梯度公式（公式 6-7）。规划器的碰撞代价函数 $J_c$ 直接依赖于 ESDF 的距离值 $d(\mathbf{p})$ 及其梯度 $\nabla d(\mathbf{p})$。如果你的系统使用 3DGS 表示，则碰撞约束变为 Mahalanobis 距离形式（公式 5、8），对应的规划器需要支持非欧氏距离度量。

术语索引¶

术语（中文）	术语（英文）	首次出现	简要定义
占据栅格地图	Occupancy Grid Map	§D6.1	将空间离散化为栅格，每个栅格存储占据概率
对数几率	Log-odds	§D6.2	概率的对数比 $L = \log\frac{p}{1-p}$，将乘法更新转化为加法
截断有符号距离场	TSDF	§D6.3	每个体素存储到最近表面的带符号距离，截断到 $[-\delta, \delta]$
欧氏有符号距离场	ESDF	§D6.4	每个体素存储到最近障碍物的精确欧氏距离及符号
准欧氏距离	Quasi-Euclidean Distance	§D6.4	voxblox 使用面/棱/角邻居的 $\{1, \sqrt{2}, \sqrt{3}\}$ 权重近似欧氏距离
双波前传播	Two-wave-front Propagation	§D6.4	分别从障碍物表面向外（正距离）和向内（负距离）传播的 BFS
桶队列	Bucket Queue	§D6.4	距离值离散化后按桶分组的优先队列，$O(1)$ 出队
环形缓冲	Circular/Ring Buffer	§D6.6	用模运算实现的固定大小缓冲区，平移时只需更新偏移量
并行带状算法	PBA (Parallel Banding Algorithm)	§D6.7	将 3D EDT 分解为三次 1D 扫描，每次扫描 GPU 并行
计数器膨胀	Counter-based Inflation	§D6.8	用计数器追踪膨胀核内的占据体素数，实现 $O(1)$ 增量膨胀
$\alpha$-平衡条件	$\alpha$-balanced Criterion	§D6.9	KD-Tree 子树大小比不超过 $\alpha$ 的平衡约束
马氏距离	Mahalanobis Distance	§D6.10	考虑协方差矩阵的广义距离 $d_M = \sqrt{(\mathbf{x}-\boldsymbol{\mu})^\top \Sigma^{-1} (\mathbf{x}-\boldsymbol{\mu})}$
控制屏障函数	CBF (Control Barrier Function)	§D6.10	保证安全集正不变性的约束 $\dot{h}(\mathbf{x}) + \alpha(h(\mathbf{x})) \geq 0$
前沿点	Frontier	§D6.8	已知自由空间与未知空间的边界体素，用于探索规划
体素哈希	Voxel Hashing	§D6.3	用哈希表索引体素块，避免预分配全局数组，支持稀疏表示

本章自测答案提示¶

以下为章首前置自测题的简要答案提示，帮助读者在学完本章后自我检验。

题号	提示
自测 1	log-odds 将贝叶斯更新的乘法转化为加法（§D6.2 公式 1），clamping 限制了 $L$ 的上下界，防止过度自信
自测 2	TSDF 存储的是到表面的截断距离（带符号），ESDF 存储的是到最近障碍物的精确欧氏距离。TSDF 是局部信息（截断范围内），ESDF 是全局信息
自测 3	规划器需要连续的距离值和梯度来构建碰撞代价函数并求解梯度优化。占据栅格只提供二值信息（占据/自由），无法直接提供梯度方向
自测 4	滑窗策略（如环形缓冲、箱式删除）只维护机器人周围的局部地图，内存开销固定。代价是丢失全局信息，不适合需要全局规划的场景
自测 5	3DGS 用连续高斯椭球表示几何，碰撞检测使用 Mahalanobis 距离而非欧氏距离。优势是表示连续且渲染质量高，挑战是在线训练的实时性和无界高斯的截断问题

编号	公式	来源
1	\(L(n \mid z_{1:t}) = L(n \mid z_{1:t-1}) + L(n \mid z_t) - L_0\)	OctoMap log-odds
2	\(d_{\text{new}} = \frac{d_{\text{old}} \cdot w_{\text{old}} + d_{\text{obs}} \cdot w_{\text{obs}}}{w_{\text{old}} + w_{\text{obs}}}\)	TSDF 加权平均融合
3	\(d(v,n) \in \{1, \sqrt{2}, \sqrt{3}\}\)（面/棱/角邻居）	voxblox 准欧氏 ESDF
4	$C(v) =	{o \in \text{occupied} : \|v-o\|_\infty \leq r}
5	\(d_M(\mathbf{x}) = \sqrt{(\mathbf{x}-\boldsymbol{\mu})^\top \boldsymbol{\Sigma}^{-1} (\mathbf{x}-\boldsymbol{\mu})}\)	3DGS Mahalanobis
6	\(J_c = \int_0^T \max(0, d_{\text{safe}} - d(\mathbf{p}))^3\,dt\)	Fast-Planner 碰撞代价
7	\(\frac{\partial J_c}{\partial \mathbf{p}} = -3\max(0, d_{\text{safe}}-d)^2 \nabla d(\mathbf{p})\)	Fast-Planner 碰撞梯度
8	\(h(\mathbf{x}) = \min_i d_E(\mathbf{x}, E_i) - r_{\text{robot}}\)，\(\dot{h} + \alpha(h) \geq 0\)	SAFER-Splat CBF
9	`physical = (logical + offset) & (N-1)`（\(N\) 为 2 的幂）	环形缓冲索引
10	$\max(	\text{left}

序号	资源	说明	推荐顺序
8	Cai et al., "ikd-Tree: An Incremental KD Tree for Robotic Applications," 2022	ikd-Tree 的增量构建、并行再平衡和 \(\alpha\)-平衡条件的完整推导。配合 §D6.9 阅读，特别注意已知 bug 和并发问题	第 7 篇
9	Xu et al., "FAST-LIO2: Fast Direct LiDAR-Inertial Odometry," TRO, 2022	ikd-Tree 的主要应用场景，理解 SLAM 系统如何利用增量 KD-Tree 进行在线点云管理	补充阅读

序号	资源	说明	推荐顺序
10	Kerbl et al., "3D Gaussian Splatting for Real-Time Radiance Field Rendering," SIGGRAPH, 2023	3DGS 的原始论文，理解高斯椭球参数化和 \(\alpha\)-blending 渲染。这是 §D6.10 所有工作的基础	第 8 篇
11	Chen et al., "SAFER-Splat: A Control Barrier Function for Safe Navigation with Online Gaussian Splatting Maps," 2024	将 3DGS 用于安全规划的 CBF 方法。对照 §D6.10 理解碰撞代价的 Mahalanobis 距离表达	第 9 篇
12	Tosi et al., "How NeRFs and 3D Gaussian Splatting are Reshaping SLAM," 综述 2024	全面梳理神经表示在 SLAM 中的应用现状，帮助把握研究前沿和发展方向	综述阅读

D6 无人机环境表示——从体素世界到高斯椭球¶

前置自测¶

本章目标¶

本章知识导航¶

前置知识桥接¶

如果跳过本章会怎样¶

预计阅读时间¶

科研发展脉络¶

§D6.1 为什么无人机需要特殊的环境表示 ⭐¶

动机：从地面 2D costmap 说起¶

如果不用三维环境表示会怎样¶

五个核心需求小结¶

⚠️ 常见陷阱¶

练习¶

§D6.2 OctoMap——概率八叉树与 log-odds 更新 ⭐⭐¶

动机：为什么从 OctoMap 讲起¶

如果不用概率融合会怎样¶

核心机制：贝叶斯占据滤波与 log-odds¶

八叉树结构与压缩¶

OctoMap 的局限性——为什么需要下一代方法¶

⚠️ 常见陷阱¶

练习¶

§D6.3 TSDF——从计算机视觉到机器人规划的桥梁 ⭐⭐¶

动机：为什么需要 TSDF¶

核心概念：SDF 与 TSDF¶

TSDF 集成方法¶

TSDF vs 占据网格：对规划的影响¶

⚠️ 常见陷阱¶

TSDF 的三种集成模式对比¶

练习¶

§D6.4 voxblox TSDF→ESDF 完整流水线 ⭐⭐⭐¶

动机：为什么 voxblox 是无人机规划的里程碑¶

系统架构总览¶

核心数据结构：体素哈希¶

ESDF 生成——准欧氏双波前算法¶

ESDF 的规划接口：d(x) 和 \(\nabla d(\mathbf{x})\)¶

voxblox 的多线程架构¶

⚠️ 常见陷阱¶

练习¶

§D6.5 FIESTA——增量 ESDF 的极致优化 ⭐⭐⭐¶

动机：voxblox ESDF 的低效之处¶

核心创新一：双队列分离¶

核心创新二：双向链表索引¶

性能对比¶

⚠️ 常见陷阱¶

练习¶

§D6.6 Ewok 环形缓冲——嵌入式友好的局部地图 ⭐⭐¶

动机：零拷贝滑窗¶

核心机制：1D 环形缓冲到 3D¶

可分离 EDT¶

⚠️ 常见陷阱¶

练习¶

§D6.7 nvblox——GPU 加速的体素建图 ⭐⭐⭐¶

动机：把 voxblox 搬到 GPU¶

GPU 体素哈希¶

PBA 精确 ESDF¶

性能数字¶

Isaac ROS 集成与人体分割¶

Jetson 平台性能¶

⚠️ 常见陷阱¶

练习¶

§D6.8 ROG-Map——2024 年 LiDAR 无人机事实标准 ⭐⭐⭐¶

动机：0.05m 分辨率下 50 Hz 维护 30m x 30m x 12m 局部地图¶

三大设计¶

ROG-Map 与 SUPER 系统的集成¶

ROG-Map 实测性能¶

⚠️ 常见陷阱¶

练习¶

§D6.9 ikd-Tree——SLAM 与规划的共享数据结构 ⭐⭐⭐¶

动机：一棵树服务两个子系统¶

在 FAST-LIO2 中的角色¶

在规划器中的角色¶

增量操作¶

并行再平衡（\(\alpha\)-balanced）¶

已知问题：双重释放 bug（Issue #20）¶

⚠️ 常见陷阱¶

练习¶

§D6.10 3DGS 用于无人机规划——Splat-Nav 与 SAFER-Splat ⭐⭐⭐⭐¶

动机：不需要体素化的全新范式¶

椭球碰撞检测数学¶