Hydra 教程（二）：从传感器观测到度量-语义地图

Hydra 教程（二）：从传感器观测到度量-语义地图

相机或激光雷达每一帧只看到局部表面，而且观测带有噪声。Hydra 若直接把每帧点云追加到世界里，会得到越来越密、带有重影、难以增量查询的点集合；高层模块也无法稳定判断表面、自由空间和语义实例。

因此，本篇要解决的不是"怎样显示一帧点云"，而是下面这条完整证据链：

上游位姿 + 传感器标定 + 深度/点云 + 颜色/标签
  -> 同一时刻、同一坐标约定下的射线观测
  -> 稀疏体素块中的 TSDF、颜色和语义状态
  -> 多帧融合后的零等值面
  -> 带颜色和语义的三角网格
  -> 只包含本轮变化的 active-window 输出

全文用"机器人观察一面墙"作为贯穿例子。读完后，读者应能回答：位姿和点云分别从哪里来，一个体素为什么得到正或负距离，传感器换到墙后为何可能产生符号冲突，语义是否直接标在每个三维点上，以及局部稠密地图怎样在长期运行中保持有界。

阅读层级： 第 1--8 节是"观测如何变成 TSDF、语义体素和 Mesh"的主线；第 9 节是长期运行的工程专题，包含 Active Window、Partial/Full update、归档和回访。若只想先建立重建心智模型，可以先读到第 8 节，再回头阅读第 9 节。

---

1. 第一个前提：Hydra 必须知道观测发生在哪里

一张深度图只说明某个表面相对当前传感器有多远。机器人移动后，下一张深度图使用了另一个传感器坐标系。若不先把这些观测放到共同参考系中，同一面墙会在地图里出现多份错位副本。

1.1 先约定坐标变换的方向

本文用 A T B {}^{A}\mathbf{T}_{B} ATB 表示"把 B B B 坐标系中的坐标变换到 A A A 坐标系"。它是一个刚体位姿：

A T B = A R B A t B 0 T 1 {}^{A}\mathbf{T}{B}= \begin{bmatrix} {}^{A}\mathbf{R}{B} & {}^{A}\mathbf{t}_{B} \\ \mathbf{0}^{T} & 1 \end{bmatrix} ATB=ARB0TAtB1

其中， A R B ∈ S O ( 3 ) {}^{A}\mathbf{R}{B}\in SO(3) ARB∈SO(3) 是三维旋转矩阵， A t B ∈ R 3 {}^{A}\mathbf{t}{B}\in\mathbb{R}^{3} AtB∈R3 是 B B B 原点在 A A A 中的位置，单位为米。对传感器坐标系 S S S 中的点 S p {}^{S}\mathbf{p} Sp，有：

W p = W T B B T S S p {}^{W}\mathbf{p} ={}^{W}\mathbf{T}{B} {}^{B}\mathbf{T}{S} {}^{S}\mathbf{p} Wp=WTBBTSSp

这里有三个不同来源：

量	物理意义	典型来源	更新频率
W T B ( t ) {}^{W}\mathbf{T}_{B}(t) WTB(t)	时刻 t t t 机体 B B B 在世界 W W W 中的位姿	视觉惯性里程计（VIO）、激光惯性里程计（LIO）、轮式里程计融合、外部定位或数据集真值	随机器人运动持续变化
B T S {}^{B}\mathbf{T}_{S} BTS	传感器 S S S 到机体 B B B 的外参	离线标定或机械设计值	通常固定
S p {}^{S}\mathbf{p} Sp	当前观测点在传感器系中的坐标	深度反投影或激光点云	每帧变化

Hydra 当前重建路径消费 W T B {}^{W}\mathbf{T}_{B} WTB，并不在这个模块里根据深度重新完成 VIO 或 LIO。所谓"把点云放进世界坐标系"，准确含义是：上游定位先给出机器人位姿，Hydra 再利用位姿和外参解释当前几何观测。

1.2 位姿、深度和标签必须属于同一时刻

输入包带有纳秒时间戳。理想情况下， W T B ( t ) {}^{W}\mathbf{T}_{B}(t) WTB(t) 应表示深度或点云采集时刻 t t t 的机体位姿，而不是消息到达处理线程时的最新位姿。

若机器人以 1   m/s 1\,\text{m/s} 1m/s 横向运动，而位姿相对深度晚了 50   ms 50\,\text{ms} 50ms，仅时间偏差就会造成约：

Δ x = 1   m/s × 0.05   s = 0.05   m \Delta x = 1\,\text{m/s}\times 0.05\,\text{s}=0.05\,\text{m} Δx=1m/s×0.05s=0.05m

的表面错位。多帧融合不会自动消除这种系统偏差，反而会把墙融合成约数厘米厚的带状结构。旋转时，误差还会随量程放大。因此"有时间戳"不等于"已经同步"，系统集成端仍需做硬件同步、时间插值或点云去畸变。点云去畸变是利用扫描期间的连续位姿，把同一帧内在不同时刻采集的点补偿到统一参考时刻。

这一节解决了观测的空间和时间归属。下一步仍有一个问题：相机给的是像素深度，激光雷达给的是离散点，它们怎样变成统一的射线距离？

2. 深度图和点云怎样变成可融合的射线观测

TSDF 更新需要知道：沿某个传感器方向，当前帧第一次观测到的表面离传感器多远。Hydra 因而把不同传感器输入整理成对齐的二维表示，其中最关键的是 range_image。

2.1 相机深度不是射线长度

针孔相机给定像素 ( u , v ) (u,v) (u,v)、相机内参 ( f x , f y , c x , c y ) (f_x,f_y,c_x,c_y) (fx,fy,cx,cy) 和平面深度 z z z。这里的 z z z 是点沿相机光轴的坐标，不是传感器原点到点的欧氏距离。

反投影得到相机坐标系中的三维点：

S p ( u , v , z ) = ( u − c x ) z / f x ( v − c y ) z / f y z {}^{S}\mathbf{p}(u,v,z)= \begin{bmatrix} (u-c_x)z/f_x \\ (v-c_y)z/f_y \\ z \end{bmatrix} Sp(u,v,z)= (u−cx)z/fx(v−cy)z/fyz

随后计算射线长度：

r ( u , v ) = ∥ S p ( u , v , z ) ∥ 2 r(u,v)=\left\|{}^{S}\mathbf{p}(u,v,z)\right\|_2 r(u,v)= Sp(u,v,z) 2

例如 f x = f y = 500 f_x=f_y=500 fx=fy=500， ( c x , c y ) = ( 320 , 240 ) (c_x,c_y)=(320,240) (cx,cy)=(320,240)，像素 ( 420 , 240 ) (420,240) (420,240) 的深度是 2   m 2\,\text{m} 2m。反投影结果为：

S p = 0.4 0 2.0 T m {}^{S}\mathbf{p}= \begin{bmatrix} 0.4 & 0 & 2.0 \end{bmatrix}^{T}\text{m} Sp=0.402.0Tm

所以该点的射线长度不是 2   m 2\,\text{m} 2m，而是：

r = 0.4 2 + 2.0 2 ≈ 2.040   m r=\sqrt{0.4^2+2.0^2}\approx2.040\,\text{m} r=0.42+2.02 ≈2.040m

这一区分很重要。后文的 SDF 比较使用射线长度 range_image；depth_image 主要是生成三维点和射线长度的原始输入。

2.2 激光点云怎样获得"像素"

激光雷达本来就给出传感器系中的三维点。Hydra 按每个点的方位角和俯仰角把它投影到二维球面网格：

列 -> 离散方位角
行 -> 离散俯仰角
格子值 -> 该角度方向上最近返回点的射线长度

若多个点落入同一个角度格，保留更近的点，同时使该格的颜色和标签与这个最近点对齐。这个二维网格看起来像图像，但其行列表示角度，不是普通相机像素。

2.3 一帧整理后实际维护什么

无论原始输入来自相机还是激光雷达，重建器最终关心以下对齐状态：

状态	含义	来源
range_image	每条离散射线到当前观测表面的欧氏距离，单位米	相机深度反投影或激光球面投影
vertex_map	每条射线对应的三维点	深度反投影或输入点云
color_image	与射线对应的 RGB 颜色	相机图像或点云颜色
label_image	与射线对应的整数语义类别 ID	外部语义分割或带标签点云
min_range、max_range	当前有效观测覆盖的距离范围	从当前帧几何统计
W T B {}^{W}\mathbf{T}_{B} WTB	采集时刻机体位姿	上游定位
传感器模型	内参、量程和 B T S {}^{B}\mathbf{T}_{S} BTS	配置和标定

"观测表面"现在可以精确定义为：当前离散射线在这一帧返回的可见表面位置。对 RGB-D 相机，它通常是该像素的深度表面；对激光雷达，它是该角度格保留的最近回波。它不是完整物体表面，也不是全局三维空间中离任意体素最近的表面。

至此，一帧观测已经可用于射线比较，但还没有跨帧记忆。下一步需要一个拥有稳定世界地址、可被重复更新的地图单元。

3. 为什么使用稀疏体素块，而不是不断追加点云

点云中的点只表示"这一帧在哪里打到了表面"。它没有为表面前方的自由空间提供稳定存储地址，也不便于把第 100 帧的证据融合回第 1 帧附近的同一位置。

3.1 体素是规则三维空间中的采样单元

体素 可以理解为三维像素。设体素边长为 h h h，世界被规则网格离散化；每个整数索引 ( i , j , k ) (i,j,k) (i,j,k) 对应一个固定空间位置。算法在体素中心保存距离场采样值，因此同一个世界位置在不同帧会访问同一体素状态。

体素不是点云点，也不是一小块实体材料：

• 点云点来自某次传感器返回，数量和位置随帧变化。
• 体素索引来自地图规则网格，先定义地址，再融合落到该地址的观测。
• TSDF 体素中心只是标量场的采样位置，不是最终 Mesh 顶点。

较小的 h h h 能表达更细表面，但体素数量大约按 1 / h 3 1/h^3 1/h3 增长。分辨率减半时，同一体积内的体素数约变成 8 倍。

3.2 Block 是分配、并行和归档单位

若每个方向把 n n n 个体素组成一个 block（体素块） ，则一个 block 含有 n 3 n^3 n3 个体素，边长为：

s b l o c k = n h s_{block}=n h sblock=nh

例如 h = 0.10   m h=0.10\,\text{m} h=0.10m、 n = 16 n=16 n=16，一个 block 有 4096 4096 4096 个体素，边长为 1.6   m 1.6\,\text{m} 1.6m。Hydra 只为当前观测可能影响的 block 分配内存，而不创建覆盖整个世界的稠密三维数组。

Block 同时服务四个工程目标：

1. 先按视锥筛选候选区域，避免扫描全地图。
2. 不同 block 可以交给不同线程并行更新。
3. Mesh 只需重算 TSDF 发生变化的 block。
4. Block 离开机器人附近参与高频更新的 active window 后可以整体移除；第 9 节会完整定义这个窗口。

3.3 "一个体素有什么字段"必须按对齐层回答

当前实现不是把所有信息塞进一个巨大结构，而是在相同网格索引上按需分配多层：

对齐层	字段	物理含义和来源
TSDF	distance	多帧融合后的投影式截断有符号距离，单位米
TSDF	weight	距离融合的累计可信权重；不是简单帧数
TSDF	color	表面附近颜色的累计结果
Semantic	semantic_label	当前最可能的语义类别 ID
Semantic	semantic_likelihoods	所选语义融合器维护的分数、对数似然或权重槽
Semantic	semantic_labels	FirstK 模式下各分数槽对应的类别 ID
Semantic	empty	该语义体素是否尚未初始化
Tracking	first_observed、last_observed	首次和最近一次被几何观测到的时间戳
Tracking	last_occupied、ever_free、active、to_remove	为更完整的时序占用管理预留的状态

这里有两个边界：

• weight=0 的 TSDF 体素是未观测体素，此时默认 distance 没有可用几何意义，不能把默认零值误当成表面。
• 当前普通投影融合路径明确更新 Tracking 层的 first_observed 和 last_observed；表中其余 Tracking 字段虽然存在，但不能据此断言该路径已经实现完整动态物体跟踪。

有了稳定体素地址以后，核心问题变成：当前帧怎样给某个候选体素产生一次距离测量？

4. 一个 TSDF 测量是怎样产生的

TSDF 是 Truncated Signed Distance Field，即截断有符号距离场。这里每个词都对应一个机制：

• Distance Field：在规则空间位置保存距离值。
• Signed：距离带正负号，用来区分当前射线上观测表面的前方和后方。
• Truncated：只保留有限距离范围，避免远离表面的数值主导融合。

Hydra 当前实现使用投影式 TSDF 。它不为体素搜索全局最近表面，而是把体素投影到当前 range_image，与同一离散射线的表面距离比较。

4.1 先选候选 block，再逐体素投影

对一帧观测，算法先根据传感器模型、 W T S {}^{W}\mathbf{T}_{S} WTS、当前量程和 block 尺寸找出与视锥相交的候选 block。随后对每个候选 block 的体素中心执行：

世界系体素中心
  -> 变换到传感器系
  -> 检查传感器量程
  -> 投影到相机像素或激光角度格
  -> 在 range_image 中插值表面距离
  -> 计算 SDF 并继续过滤

因此它不是"对每个像素，从传感器一直走到地图尽头并比较所有体素"。但也不是只处理零交叉附近已有体素：当前视锥候选 block 内的每个体素中心都会被尝试投影，再由后续条件快速拒绝。

若有 N b N_b Nb 个候选 block，每块边长含 n n n 个体素，则一帧至多尝试约：

N t r y = N b n 3 N_{try}=N_b n^3 Ntry=Nbn3

次体素投影。实际更新数会因量程、投影边界、无效深度、遮挡后截断和语义掩码显著减少；实现还按 block 并行处理。

4.2 SDF 来自同一射线上的两个距离

设候选体素中心变换到传感器系后为 S x {}^{S}\mathbf{x} Sx，它到传感器原点的距离为：

r v = ∥ S x ∥ 2 r_v=\left\|{}^{S}\mathbf{x}\right\|_2 rv= Sx 2

将 S x {}^{S}\mathbf{x} Sx 投影到图像位置 ( u , v ) (u,v) (u,v)，从 range_image 插值得到当前表面距离 r s r_s rs。本帧对该体素的投影式 SDF 测量为：

d m e a s = r s − r v d_{meas}=r_s-r_v dmeas=rs−rv

于是：

条件	射线上的顺序	d m e a s d_{meas} dmeas	当前帧能提供的解释
r v < r s r_v<r_s rv<rs	传感器 -> 体素 -> 表面	正	体素位于已观测自由射线段上
r v = r s r_v=r_s rv=rs	体素落在观测表面	零	表面零交叉候选
r v > r s r_v>r_s rv>rs	传感器 -> 表面 -> 体素	负	体素位于这次可见表面后方

"前方"和"后方"都相对于当前传感器射线，不是世界坐标某个固定方向，也不是由表面法向永久决定。

4.3 截断具体过滤了什么

μ \mu μ 表示 TSDF 截断距离。它是一个以米为单位的正数，用来规定：在当前观测表面附近多宽的范围内，系统保留 SDF 的实际数值。它不是墙的厚度，不是传感器量程，也不是机器人与障碍物之间的安全距离。

已知当前像素测得的表面射线距离为 r s r_s rs，则 μ \mu μ 在这条射线上定义了一个以表面为中心的截断带：

r s − μ ≤ r v ≤ r s + μ r_s-\mu\le r_v\le r_s+\mu rs−μ≤rv≤rs+μ

把 d m e a s = r s − r v d_{meas}=r_s-r_v dmeas=rs−rv 代入，上式等价于：

− μ ≤ d m e a s ≤ + μ -\mu\le d_{meas}\le+\mu −μ≤dmeas≤+μ

因此， μ \mu μ 可以理解为"TSDF 愿意精确描述零交叉附近多远的距离"：

• 从 r s − μ r_s-\mu rs−μ 到 r s r_s rs 是表面前方的正距离带，射线已经穿过这里，因而有自由空间证据。
• r s r_s rs 处对应 d m e a s = 0 d_{meas}=0 dmeas=0，是观测表面的位置。
• 从 r s r_s rs 到 r s + μ r_s+\mu rs+μ 是表面后方的负距离带，用来在零交叉另一侧形成有限的负值，但这部分位于遮挡区，证据弱于正侧。
• 更远离表面时，精确的距离值对提取零交叉已经没有必要，还可能让一次错误深度影响过大的空间范围，所以要饱和或拒绝。

为什么不能只保存表面上的零值？因为 Marching Cubes 等表面提取方法需要在相邻体素间看到"正值到负值"的变化，才能插值出零交叉。如果只更新恰好落在表面上的体素，离散体素中心通常不会与真实表面重合，也就很难稳定恢复表面。截断带给表面两侧留出若干体素，既容纳深度噪声和位姿误差，也为零交叉插值提供支撑。

当前 VolumetricMap::Config 的代码默认值是：

voxel_size = 0.10 m
truncation_distance = 0.30 m

也就是默认 μ = 0.30   m \mu=0.30\,\text{m} μ=0.30m，约为三个体素边长。实际运行时配置可以覆盖这个默认值，所以应以所加载的配置为准，而不能假定所有 Hydra 地图都使用 0.30   m 0.30\,\text{m} 0.30m。

一个沿射线的数值例子

假设某像素看到的第一个表面位于 r s = 2.00   m r_s=2.00\,\text{m} rs=2.00m，并设置 μ = 0.20   m \mu=0.20\,\text{m} μ=0.20m：

体素射线距离 r v r_v rv	原始 d m e a s = 2.00 − r v d_{meas}=2.00-r_v dmeas=2.00−rv	处理结果	物理含义
1.50   m 1.50\,\text{m} 1.50m	+ 0.50   m +0.50\,\text{m} +0.50m	截成 + 0.20   m +0.20\,\text{m} +0.20m	表面前方很远，但射线确实穿过，是已观测自由空间
1.90   m 1.90\,\text{m} 1.90m	+ 0.10   m +0.10\,\text{m} +0.10m	保留 + 0.10   m +0.10\,\text{m} +0.10m	位于表面前方截断带内
2.00   m 2.00\,\text{m} 2.00m	0 0 0	保留 0 0 0	当前观测表面，即零交叉候选
2.10   m 2.10\,\text{m} 2.10m	− 0.10   m -0.10\,\text{m} −0.10m	保留 − 0.10   m -0.10\,\text{m} −0.10m	位于表面后方截断带内
2.30   m 2.30\,\text{m} 2.30m	− 0.30   m -0.30\,\text{m} −0.30m	默认拒绝	已超过可见表面后的截断带，属于被遮挡的未知区域

第一行存成 + μ +\mu +μ 后，不再表示"体素到最近表面恰好为 μ \mu μ"，而只表示"它在这次观测中位于明确的自由侧，并且离零交叉至少已有一个截断距离"。截断后的 TSDF 是为稳定融合和提取表面服务的有限支持表示，不再是处处精确的欧氏距离场。

Hydra 为什么对两侧采用不同处理

当前实现对正侧和负侧的处理并不完全对称：

1. 若 d m e a s > μ d_{meas}>\mu dmeas>μ，把测量截成 + μ +\mu +μ。这些体素仍可作为已观测自由空间参与几何融合。
2. 若 − μ ≤ d m e a s ≤ μ -\mu\le d_{meas}\le\mu −μ≤dmeas≤μ，保留实际测量，用于形成零交叉及其邻域。
3. 若 d m e a s < − μ d_{meas}< -\mu dmeas<−μ，默认拒绝，因为第一可见表面之后太远的区域已被遮挡，传感器没有证据说明那里是什么。
4. 若显式配置了额外负侧积分距离，有限范围内的更深体素才可被接纳并截成 − μ -\mu −μ，还可按语义标签继续筛选。

这种不对称来自深度传感器的可见性：从传感器到第一个深度回波之间的射线段确实被观测为空，所以很远的正值可以安全地饱和为 + μ +\mu +μ；第一个表面之后则被遮挡，不能据此断言后方一直是实体。因此，这比简单写成 clamp ⁡ ( d , − μ , μ ) \operatorname{clamp}(d,-\mu,\mu) clamp(d,−μ,μ) 更准确。直接把所有表面后方体素都写成 − μ -\mu −μ，等于把被遮挡的未知空间武断地当成占用内部。

μ \mu μ 的取值是一项分辨率、噪声鲁棒性和表面分离能力之间的折中：取值过小，深度噪声或位姿误差可能让相邻体素难以形成稳定的正负带；取值过大，不同的邻近表面更容易在融合中互相影响，薄结构也更容易被抹厚。它通常应至少覆盖若干个体素，并结合深度噪声和定位误差选择，而不是脱离 voxel_size 单独设置。

4.4 一次体素更新要经过哪些门

一个候选体素最终形成有效测量，至少要通过以下条件：

阶段	接受条件	拒绝原因举例
Block 候选	Block 与当前传感器视锥和量程相交	在视野外或距离范围外
体素量程	r v r_v rv 位于传感器允许范围及当前帧最大有效范围内	太近、太远
投影	体素能投到有效图像区域	在相机后方、超出图像边界
插值	对应邻域能产生有效 range_image 值	深度无效、邻域不满足插值规则
遮挡后带宽	d m e a s ≥ − μ d_{meas}\ge-\mu dmeas≥−μ，或处于允许的额外积分带	位于可见表面后方太远
语义掩码	表面带内的标签不是被屏蔽类别	动态或无效表面类别
额外带标签	若启用额外负带，标签在允许集合中	该类别不允许扩大负带

一个容易忽略的实现细节是：无效/动态标签主要阻止表面截断带内 的观测写成静态表面；远在表面前方并被截成 + μ +\mu +μ 的已知自由射线段仍可融合。这样，一个人站在相机前时，人表面不会进入静态墙体地图，但相机到人之间确实被看见的空间仍可记作自由。

至此得到的只是"一帧对一个体素的意见"。稳定地图还必须回答：多帧意见如何组合，以及观察方向改变时正负号会不会互相矛盾？

5. 多帧怎样融合成一个 TSDF 状态

设同一地图体素当前保存距离 D o l d D_{old} Dold 和累计权重 W o l d W_{old} Wold，新测量为 d m e a s d_{meas} dmeas，新权重为 w m e a s w_{meas} wmeas。加权融合为：

D n e w = W o l d D o l d + w m e a s d m e a s W o l d + w m e a s D_{new}= \frac{W_{old}D_{old}+w_{meas}d_{meas}} {W_{old}+w_{meas}} Dnew=Wold+wmeasWoldDold+wmeasdmeas

权重状态更新为：

W n e w = min ⁡ ( W o l d + w m e a s , W m a x ) W_{new}=\min(W_{old}+w_{meas},W_{max}) Wnew=min(Wold+wmeas,Wmax)

因此 distance 不是原始深度，也不是到某个永久表面点的显式向量；它是许多投影式 SDF 测量在同一世界体素地址上的加权平均。

5.1 新测量权重怎样得到

当前实现先让传感器模型估计该深度处一个体素大约会接收多少射线，再按配置施加距离衰减、表面后方衰减和最小权重限制。可以概括为：

w m e a s = max ⁡  ⁣ ( w m i n , ρ ( h , z )   η z ( z )   η b a c k ( d m e a s ) ) w_{meas}=\max\!\left(w_{min},\rho(h,z)\,\eta_z(z)\,\eta_{back}(d_{meas})\right) wmeas=max(wmin,ρ(h,z)ηz(z)ηback(dmeas))

其中：

• ρ ( h , z ) \rho(h,z) ρ(h,z) 是传感器在深度 z z z、体素尺寸 h h h 下的射线密度估计。
• η z ( z ) \eta_z(z) ηz(z) 是可选的深度平方衰减；关闭时不再额外除以 z 2 z^2 z2。
• η b a c k \eta_{back} ηback 在表面负侧深处逐渐减小权重。
• w m i n w_{min} wmin 防止测量权重低于 Mesh 判断"已观测"所需的最小量级。

对针孔相机，当前射线密度近似为：

ρ ( h , z ) = f x f y ( h z ) 2 \rho(h,z)=f_x f_y\left(\frac{h}{z}\right)^2 ρ(h,z)=fxfy(zh)2

默认深度衰减开启时还会再除以 z 2 z^2 z2。这表达了远处观测覆盖更稀、通常也更不可靠。use_constant_weight 这个配置名容易让人以为权重必定等于 1；从当前计算路径看，它关闭的是额外 1 / z 2 1/z^2 1/z2 衰减，传感器射线密度项仍然存在。

5.2 一个可手算的融合例子

假设某体素已有：

D o l d = 0.04   m , W o l d = 2 D_{old}=0.04\,\text{m},\qquad W_{old}=2 Dold=0.04m,Wold=2

新视角认为它稍微位于表面后方：

d m e a s = − 0.02   m , w m e a s = 1 d_{meas}=-0.02\,\text{m},\qquad w_{meas}=1 dmeas=−0.02m,wmeas=1

则：

D n e w = 2 × 0.04 + 1 × ( − 0.02 ) 3 = 0.02   m D_{new}=\frac{2\times0.04+1\times(-0.02)}{3}=0.02\,\text{m} Dnew=32×0.04+1×(−0.02)=0.02m

新结果仍为正，但更靠近零。若后续观测持续把该体素放在表面附近，distance 会向稳定零交叉收敛；若测量因位姿偏差长期在正负两侧抖动，表面带就会变厚。

6. 传感器从墙前移动到墙后，符号冲突怎么办

这是理解投影式 TSDF 最关键的边界。考虑一维的无限薄表面 x = 0 x=0 x=0 和体素 x = − 0.05   m x=-0.05\,\text{m} x=−0.05m。

传感器 S 1 S_1 S1 位于 x = − 2   m x=-2\,\text{m} x=−2m 并朝正方向观察：

r s = 2.00 , r v = 1.95 , d 1 = + 0.05   m r_s=2.00,\qquad r_v=1.95,\qquad d_1=+0.05\,\text{m} rs=2.00,rv=1.95,d1=+0.05m

同一传感器移动到 S 2 : x = + 2   m S_2:x=+2\,\text{m} S2:x=+2m 并朝负方向观察同一无限薄表面：

r s = 2.00 , r v = 2.05 , d 2 = − 0.05   m r_s=2.00,\qquad r_v=2.05,\qquad d_2=-0.05\,\text{m} rs=2.00,rv=2.05,d2=−0.05m

同一个体素确实收到一正一负两个测量。若权重相同，平均值会变成零。这不是公式遗漏，而是用单个投影式标量场描述无限薄双面表面时的固有限制。

实际系统能缓解但不能从数学上彻底消除这一问题：

1. 真正的墙有厚度。墙两侧传感器通常分别看到两个不同物理表面，而不是同一个无限薄平面。
2. 负距离只在表面后方很窄的截断带内融合，更深的遮挡区被拒绝。
3. 多帧位姿、深度和权重一致时，真正可见表面的零交叉仍可稳定积累。
4. 动态或无效类别可以不形成静态表面。

但薄板、玻璃、栏杆、错误深度、穿透回波以及从两侧反复观察的薄物体仍可能产生相消、双层表面或表面变厚。增大体素尺寸或截断距离通常会加重这种混合。

还要注意，体素里没有保存"哪一个方向更空旷"的向量。方向在每次更新时由当前传感器位置和体素位置重新构造；融合后只保留标量 distance 和 weight。所以：

一个像素的距离 -> 只描述该像素对应的当前射线
许多像素 + 相机模型 -> 得到这一帧的多方向射线集合
许多位姿下的射线集合 -> 在三维体素网格中积累自由空间和表面证据

若任务需要查询"离最近障碍多远"或"自由空间向哪里延伸"，不能只读一个 TSDF 标量；下一篇会从整张已观测体素地图建立 ESDF 和自由空间骨架。

7. 语义信息从哪里来，又怎样与几何融合

TSDF 能告诉系统表面在哪里，却不能仅凭距离判断它是墙、桌子还是椅子。Hydra 因而消费与深度或点云对齐的语义标签。

7.1 标签由外部感知产生

对 RGB-D 输入，外部分割模型通常为每个像素输出类别 ID：

label_image(v,u) = wall / floor / chair / ... 对应的整数编号

该像素的深度负责产生三维位置，标签负责说明这个表面测量属于哪一类。对激光点云，可以为每个点提供类别，再在球面投影时把标签放到对应角度格。

Hydra 会把数据集或模型自己的类别编号重映射到当前统一 label space，即整套系统约定的类别字典和 ID 编码规则；但普通重建模块并不在内部运行语义分割网络。没有标签时仍可建立纯几何 TSDF、Mesh、ESDF、GVD 和 Places；只是不能从这条路径可靠地产生语义 Objects。

7.2 标签不是永久贴在原始点上

原始像素或点的标签只是一帧测量。对于落在表面截断带内的有效体素，Hydra 在与 TSDF 对齐的 Semantic 层中累计语义证据。不同融合器维护的状态不同：

融合器	内部状态	一次新标签怎样更新	当前类别怎样选
MLESemanticIntegrator	每个类别的对数似然	加上该观测标签对应的一列对数观测似然	最大对数似然类别
FirstKSemanticIntegrator	最多 K K K 个类别 ID 和各自累计权重	找到同类槽或空槽并增加测量权重	权重最大且超过阈值的槽
SingleLabelIntegrator	一个类别和一个支持权重	同类加权，异类相互抵消，强者可替换当前类	当前剩余权重对应类别
BinarySemanticIntegrator	两个计数槽	按标签是否非零增加计数	为二分类用途保留计数

以最大似然模式为例。设类别集合大小为 K K K，当前对数分数为 ℓ k \ell_k ℓk，观测标签为 y y y，配置的正确标签概率为 p p p。一次更新可以写成：

ℓ k n e w = ℓ k o l d + log ⁡ P ( y ∣ k ) \ell_k^{new}=\ell_k^{old}+\log P(y\mid k) ℓknew=ℓkold+logP(y∣k)

当前语义类别为：

k ^ = arg ⁡ max ⁡ k ℓ k \hat{k}=\arg\max_k\ell_k k^=argkmaxℓk

在当前实现中，匹配项使用 log ⁡ p \log p logp，非匹配项使用 log ⁡ ( 1 − p ) \log(1-p) log(1−p)。因为使用对数，连续观测的概率乘法变成分数加法，避免直接连乘很小概率。

7.3 动态标签过滤不等于动态物体识别

配置中的 invalid_labels 和 dynamic_labels 会合并成积分掩码。表面带内若命中这些标签，本次表面测量不会写入静态 TSDF 和语义层。

这条机制的能力边界是：

• 它依赖上游已经把人、车辆等标成动态类别。
• 它按类别过滤，不估计速度、运动模型或跨帧 Object ID。
• 一把被人搬动的椅子通常仍属于 chair，不会仅因发生移动就自动被检测为动态。
• 若分割漏检动态物体，其几何仍可能污染静态地图。

因此"动态类别屏蔽"解决的是静态重建污染的一部分，不是完整多目标跟踪。Object 实例怎样从语义 Mesh 中产生，将在第 4 篇解释。

8. TSDF 为什么还要转换成三角网格

TSDF 是隐式标量场。表面由满足 D ( x ) = 0 D(\mathbf{x})=0 D(x)=0 的位置定义，但系统并没有直接存下一组三角形。可视化、对象聚类、网格形变和表面连接都更适合使用显式 Mesh，因此 Hydra 使用 Marching Cubes 提取零等值面。

8.1 一个 Marching Cubes 单元来自 2 × 2 × 2 2\times2\times2 2×2×2 个角点采样

这里的"8 个相邻体素"容易让人误以为：先选一个中心体素，再从它的 26 个邻居中挑出 8 个。Marching Cubes 实际上没有这样做。它处理的基本对象不是"某个体素及其邻域"，而是规则采样网格中相邻坐标面围成的一个最小立方体单元。

设 TSDF 体素中心的离散索引为 ( i , j , k ) (i,j,k) (i,j,k)，体素间距为 s s s。以 ( i , j , k ) (i,j,k) (i,j,k) 作为当前立方体的最小索引角点，立方体的角点集合为：

C i , j , k = { v i + a , j + b , k + c ∣ a , b , c ∈ { 0 , 1 } } \mathcal{C}{i,j,k} =\left\{ v{i+a,j+b,k+c} \mid a,b,c\in\{0,1\} \right\} Ci,j,k={vi+a,j+b,k+c∣a,b,c∈{0,1}}

严格来说，这个 Marching Cubes 立方体的"角点"是 8 个 TSDF 采样位置，也就是 8 个体素中心，不是某一个体素自身的 8 个几何顶角。体素是保存 TSDF 数值的空间小格子；Marching Cubes 把相邻小格子的中心当作标量场采样点，再在这些采样点之间寻找零交叉。

每个坐标轴只有"当前层"和"下一层"两种选择，所以角点总数为：

2 × 2 × 2 = 8 2\times2\times2=8 2×2×2=8

Hydra 当前实现使用的 8 个索引偏移如下：

角点	相对索引 ( a , b , c ) (a,b,c) (a,b,c)	实际体素索引
v 0 v_0 v0	( 0 , 0 , 0 ) (0,0,0) (0,0,0)	( i , j , k ) (i,j,k) (i,j,k)
v 1 v_1 v1	( 1 , 0 , 0 ) (1,0,0) (1,0,0)	( i + 1 , j , k ) (i+1,j,k) (i+1,j,k)
v 2 v_2 v2	( 1 , 1 , 0 ) (1,1,0) (1,1,0)	( i + 1 , j + 1 , k ) (i+1,j+1,k) (i+1,j+1,k)
v 3 v_3 v3	( 0 , 1 , 0 ) (0,1,0) (0,1,0)	( i , j + 1 , k ) (i,j+1,k) (i,j+1,k)
v 4 v_4 v4	( 0 , 0 , 1 ) (0,0,1) (0,0,1)	( i , j , k + 1 ) (i,j,k+1) (i,j,k+1)
v 5 v_5 v5	( 1 , 0 , 1 ) (1,0,1) (1,0,1)	( i + 1 , j , k + 1 ) (i+1,j,k+1) (i+1,j,k+1)
v 6 v_6 v6	( 1 , 1 , 1 ) (1,1,1) (1,1,1)	( i + 1 , j + 1 , k + 1 ) (i+1,j+1,k+1) (i+1,j+1,k+1)
v 7 v_7 v7	( 0 , 1 , 1 ) (0,1,1) (0,1,1)	( i , j + 1 , k + 1 ) (i,j+1,k+1) (i,j+1,k+1)

可以先把它看成上下两个正方形：

z = k + 1                    z = k

v7 -------- v6              v3 -------- v2
|             |              |             |
|             |              |             |
v4 -------- v5              v0 -------- v1

再把 v 0 − v 4 v_0-v_4 v0−v4、 v 1 − v 5 v_1-v_5 v1−v5、 v 2 − v 6 v_2-v_6 v2−v6、 v 3 − v 7 v_3-v_7 v3−v7 分别连接起来，就得到一个三维立方体。它有 8 个角点、12 条边和 6 个面，边长为一个体素间距 s s s。注意，所谓"8 个相邻采样点"是说它们共同围成一个局部单元，并不是说这 8 个点彼此之间都属于 6 邻接关系。

为什么三维体素又会有 26 个邻居

"26 邻域"回答的是另一个问题：以一个体素为中心，周围哪些体素被当作与它相邻 。对中心体素 v i , j , k v_{i,j,k} vi,j,k，26 邻域定义为：

N 26 ( i , j , k ) = { v i + Δ i , j + Δ j , k + Δ k ∣ Δ i , Δ j , Δ k ∈ { − 1 , 0 , 1 } , ( Δ i , Δ j , Δ k ) ≠ ( 0 , 0 , 0 ) } \mathcal{N}{26}(i,j,k) =\left\{ v{i+\Delta i,j+\Delta j,k+\Delta k} \mid \Delta i,\Delta j,\Delta k\in\{-1,0,1\}, (\Delta i,\Delta j,\Delta k)\ne(0,0,0) \right\} N26(i,j,k)={vi+Δi,j+Δj,k+Δk∣Δi,Δj,Δk∈{−1,0,1},(Δi,Δj,Δk)=(0,0,0)}

三个方向各取 − 1 -1 −1、 0 0 0、 + 1 +1 +1，一共有 3 3 = 27 3^3=27 33=27 个位置；去掉中心本身，剩下 27 − 1 = 26 27-1=26 27−1=26 个邻居。其中包括：

• 6 个共面的邻居，即共享一个体素面。
• 12 个共边的对角邻居，即只共享一条边。
• 8 个共顶点的对角邻居，即只共享一个角点。

因此两种数字没有矛盾：

数字	描述对象	用来回答什么问题
8	一个最小立方体单元的角点	这个单元内部是否有 TSDF 零等值面穿过
26	一个中心体素周围的完整一圈邻居	体素之间按面、边和角是否连通

如果把中心体素连同 26 个邻居全部取出，会得到一个 3 × 3 × 3 3\times3\times3 3×3×3 的采样点集合。它在每个轴上覆盖 -1 -> 0 和 0 -> +1 两个网格间隔，因此内部实际包含：

2 × 2 × 2 = 8 2\times2\times2=8 2×2×2=8

个 Marching Cubes 小立方体，而不是一个立方体。Marching Cubes 会分别处理这 8 个小单元，不会把 27 个采样值一次塞进同一个情况编号。

遍历 8 个角点会不会漏掉其他方向

不会。当前单元只使用相对偏移为 0 0 0 或 + 1 +1 +1 的角点，是为了给这个单元一个唯一的最小索引 ( i , j , k ) (i,j,k) (i,j,k)。算法随后遍历其他起始索引：例如处理 ( i − 1 , j , k ) (i-1,j,k) (i−1,j,k) 时，就覆盖了当前体素负 x x x 方向的小立方体。内部体素通常会同时成为周围多个立方体的角点，但每个最小立方体单元只处理一次。

若 8 个角点跨越 TSDF block 边界，Hydra 会从相邻 block 读取越界角点；block 只是内存分块，不会把几何立方体截断。只有这 8 个角点的 weight 都达到 Mesh 最小权重，当前单元才被认为观测充分。

8 个距离值的正负组合形成一个 8 位情况编号。若一条立方体边两端距离异号，零等值面一定穿过这条边。三角形查找表再根据情况编号决定哪些交点应连接成面片。

8.2 边交点公式怎样推导

设一条边两端位置为 p 0 \mathbf{p}_0 p0、 p 1 \mathbf{p}_1 p1，TSDF 值为 d 0 d_0 d0、 d 1 d_1 d1。假设边上距离线性变化：

d ( t ) = ( 1 − t ) d 0 + t d 1 , 0 ≤ t ≤ 1 d(t)=(1-t)d_0+t d_1,\qquad 0\le t\le1 d(t)=(1−t)d0+td1,0≤t≤1

令表面处 d ( t ) = 0 d(t)=0 d(t)=0：

0 = d 0 + t ( d 1 − d 0 ) 0=d_0+t(d_1-d_0) 0=d0+t(d1−d0)

所以：

t = d 0 d 0 − d 1 t=\frac{d_0}{d_0-d_1} t=d0−d1d0

网格顶点位置为：

p s u r f a c e = p 0 + t ( p 1 − p 0 ) \mathbf{p}_{surface}=\mathbf{p}_0+t(\mathbf{p}_1-\mathbf{p}_0) psurface=p0+t(p1−p0)

例如 d 0 = 0.03   m d_0=0.03\,\text{m} d0=0.03m、 d 1 = − 0.01   m d_1=-0.01\,\text{m} d1=−0.01m，则 t = 0.75 t=0.75 t=0.75。若两体素中心相距 0.1   m 0.1\,\text{m} 0.1m，表面位于从 p 0 \mathbf{p}_0 p0 出发 0.075   m 0.075\,\text{m} 0.075m 处，而不是简单放在边中点。

颜色沿边按同一个 t t t 线性插值。语义类别不能像连续颜色一样做数值插值，当前实现选择更靠近交点的端点标签；恰在中点时选择权重更大的端点标签。

8.3 为什么跨 block 还要访问邻块

位于 block 最大边界上的 Marching Cubes 单元，其部分角点属于相邻 block。若只读取当前 block，网格会在块边界断裂。因此 Mesh 提取分为块内部和块外边界处理；边界单元需要相邻 TSDF block 存在且角点权重有效。

Mesh 只重算 mesh_updated 的 TSDF blocks，并把对应 Mesh block 标为更新。这样底层仍保留适合融合的隐式场，上层则获得带世界坐标、颜色、可选语义和时间信息的显式表面。

9. Active window 怎样限制长期运行成本

即使采用稀疏 block 分配，机器人只要持续向前探索，已经分配过的 TSDF、语义、Tracking 和 Mesh blocks 仍会不断累积。若这些高分辨率数据永远留在内存里，计算量和内存都会随行驶距离增长。

Hydra 的解决思路不是把整个世界都永久保存在同一张稠密体素地图中，而是把表示分成两个生命周期层次：

机器人附近
高分辨率、可继续融合和修改的局部稠密表示
TSDF / semantic / tracking / mesh / ESDF / GVD
                         │
                         │ 离开 Active Window
                         ▼
机器人远处
不再高频修改的历史压缩表示
全局 Mesh、Object、Place、Room、Building 和其他 DSG 信息

**Active window（活动窗口）**因此不是一种新的地图类型，而是一条生命周期规则：

哪些局部数据仍位于机器人附近，可以继续接受新观测并参与高频更新；哪些数据已经离开局部工作区，应停止维护昂贵的稠密状态，只保留适合长期运行的历史表示。

这个设计使 Hydra 能同时满足两个看似矛盾的目标：

• 机器人附近保留足够细的几何，支持持续融合、网格更新、自由空间和对象提取。
• 机器人走远后不再为所有历史区域维护完整体素场，但仍能在全局场景图中查询曾经见过的地点和对象。

9.1 三个名称相近但职责不同的对象

源码中有三个名称都包含 Active Window 的对象，初次阅读时很容易把它们当成同一个东西。

名称	本质	主要职责
ActiveWindowModule	流水线模块基类	持有局部 VolumetricMap、输入队列和输出队列；ReconstructionModule 继承它并执行实际重建
VolumetricWindow	判断策略接口	根据机器人位姿、数据位置和可选时间信息，判断某个 block 或视图是否仍在活动范围内
ActiveWindowOutput	模块间增量消息	携带时间戳、机器人位姿、本轮更新的地图块、归档索引和可选传感器数据，交给前端消费

也就是说：

ActiveWindowModule
    持有并更新局部地图
          │
          ├── 使用 VolumetricWindow 判断哪些块应离开窗口
          │
          └── 产生 ActiveWindowOutput，把增量和归档事件发给前端

不能把 ActiveWindowOutput::map() 理解成完整 Active Window，也不能把 VolumetricWindow 理解成真正存储体素的地图；前者只是消息，后者只是边界判断器。

9.2 Active window 不是什么

Active window 不是以下几种概念。

它不是当前传感器视锥。

视锥只回答"这一帧可能观测哪些 blocks"，用于候选块分配和投影积分。Active window 回答"跨越很多帧后，哪些已分配 blocks 仍值得留在局部地图中"。一个 block 当前不在相机视野内，只要仍在窗口范围内，就可以继续保留。

它不是固定数量的最新帧。

当前实现主要根据空间距离保留数据，而不是保留最近 N N N 帧。一个很久没有重新看到、但仍在机器人附近的 block 可以继续存在；一个时间很新、但已经远离机器人的 block 则可能被归档。

它不是不断重置坐标原点的滚动栅格。

TSDF block 的索引和体素位置仍锚定在同一个 world 坐标系中。窗口移动时，系统改变的是"哪些 world blocks 还留在局部地图"，而不是把所有体素坐标重新平移到机器人附近。

它不是第二张与 DSG 无关的世界地图。

局部 TSDF/Mesh 的增量会继续生成 Object、Place 和全局 Mesh，并同步到 DSG。Active Window 是全局表示的高分辨率前端工作区。

它也不表示超过半径后所有历史知识都会立即消失。

离开窗口首先影响的是昂贵的局部稠密表示。已经压缩到全局 Mesh 或 DSG 的 Place、Object 等历史信息通常会被归档或标成非活动状态，而不是与 TSDF block 一起无条件删除。

9.3 当前空间窗口怎样判断一个 block 是否在范围内

VolumetricWindow 是可扩展接口，参数中预留了时间戳和最后更新时间，因此理论上可以实现按时间、轨迹或其他规则变化的窗口。当前仓库实际注册并普遍使用的是 SpatialWindowChecker，它只检查三维空间距离。

设机器人机体在 world 坐标系中的位置为：

W t B {}^{W}\mathbf{t}_{B} WtB

某个 block 的中心为：

c b l o c k \mathbf{c}_{block} cblock

窗口半径为：

r w i n d o w r_{window} rwindow

则 block 保留条件为：

inBounds ⁡ ( b l o c k )    ⟺    ∥ c b l o c k − W t B ∥ 2 ≤ r w i n d o w \operatorname{inBounds}(block) \iff \left\| \mathbf{c}{block}-{}^{W}\mathbf{t}{B} \right\|2 \leq r{window} inBounds(block)⟺ cblock−WtB 2≤rwindow

各变量的物理含义如下：

• W t B {}^{W}\mathbf{t}_{B} WtB 来自当前输入包中的机器人位姿，只使用平移部分。
• c b l o c k \mathbf{c}_{block} cblock 由整数 block 索引和 block 尺寸换算得到。
• ∥ ⋅ ∥ 2 \|\cdot\|_2 ∥⋅∥2 是三维欧氏距离，因此当前窗口是以机器人机体位置为中心的球形区域。
• 机器人朝向不影响窗口判断；旋转原地不改变任何 block 是否在范围内。
• 当前空间策略也不使用 block 的更新时间，时间参数只为其他可能的窗口策略保留。

它检查的是整个 block 的中心 ，不是逐体素检查。设体素尺寸为 s v s_v sv，每条边有 N b N_b Nb 个体素，则 block 边长为：

s b l o c k = s v N b s_{block}=s_vN_b sblock=svNb

例如 Habitat 配置中：

s v = 0.1 m , N b = 16 s_v=0.1\ \mathrm{m},\qquad N_b=16 sv=0.1 m,Nb=16

因此：

s b l o c k = 1.6 m s_{block}=1.6\ \mathrm{m} sblock=1.6 m

该立方体从中心到最远角点的距离是半对角线：

r h a l f _ d i a g = 3 2 s b l o c k ≈ 1.386 m r_{half\diag} =\frac{\sqrt{3}}{2}s{block} \approx1.386\ \mathrm{m} rhalf_diag=23 sblock≈1.386 m

若窗口半径为 8 m 8\ \mathrm{m} 8 m：

• 中心距机器人 7.9 m 7.9\ \mathrm{m} 7.9 m 的 block 会完整保留，即使它的远端角点可能超过 8 m 8\ \mathrm{m} 8 m。
• 中心距机器人 8.1 m 8.1\ \mathrm{m} 8.1 m 的 block 会被判为越界，即使它的一部分仍与半径 8 m 8\ \mathrm{m} 8 m 的球相交。

所以实际窗口边界呈 block 级的阶梯形，而不是在体素层面精确裁出的光滑球面。这样做牺牲了一点边界精度，却把判断和删除单位统一为 block，显著降低管理成本。

9.4 Active window 为什么会同时影响多种表示

Active window 不是一个拥有所有数据的中央容器。多个模块会创建同一类 VolumetricWindow 判断器，并分别管理自己的活动数据。

表示	谁维护	离开窗口时的主要动作
TSDF、Semantic、Tracking、局部 Mesh blocks	ReconstructionModule 的 VolumetricMap	从局部体素地图中移除对应 block，并输出归档索引
压缩后的前端 Mesh	GraphBuilder / mesh compression	根据归档索引结束对应局部网格块的活动生命周期，历史网格由全局 Mesh 继续管理
ESDF/GVD blocks	GvdPlaceExtractor	删除越界 GVD block，并通知 Graph Extractor 其细粒度支撑已离开活动范围
Place 节点	Graph Extractor 与 GraphBuilder	不再属于活动 Place 集合的历史节点可在 DSG 中保留，但 is_active 被设为 false
Feature views	ViewDatabase	视图位置离开窗口后归档，不再作为当前活动视角参与节点特征分配
Frontiers	FrontierExtractor	结合刚归档 block 和归档 Place 更新边界；已离开局部探索范围的 frontier 不再按普通活动 frontier 处理

这意味着不同表示到达窗口边界的时刻可能略有差异：

• 重建地图只在 full update 时归档 TSDF blocks。
• GVD 提取器在收到一次前端重建输出后，再根据自己的 GVD blocks 做窗口检查。
• Place 的活动状态要等 GVD/Graph Extractor 更新完，再由 GraphBuilder 比较前后两轮活动节点集合。

因此"归档"不是某一行代码同时删除所有层，而是一条从稠密地图向稀疏全局图逐层传播的生命周期事件。

9.5 Partial update 和 full update 的完整执行顺序

这一对术语描述的是 ReconstructionModule 对同一帧输入执行到哪一步，不是两种不同的传感器，也不是两张不同的地图。

最简洁的定义是：

• Partial update（部分更新） ：本帧观测只被融合进模块内部的局部体素地图，先不生成网格，也先不向 GraphBuilder 提交前端更新。
• Full update（完整更新）：本帧观测先被融合进局部体素地图，然后把一段时间内积累的变化继续转换为 Mesh、归档事件和增量输出，提交给前端。

可以把它们理解成数据库中的两个阶段：

每个传感器输入
       │
       ▼
写入 ReconstructionModule::map_
       │
       ├── Partial update：只完成内部写入，暂不"提交"给前端
       │
       └── Full update：完成写入后，生成派生结果并"提交"一批增量

这里的"完整"有一个重要限定：Full update 不是重新处理整个世界，也不是扫描整个局部地图的每个体素。 当前实现仍然只重算发生变化的 Mesh blocks，并且只把 updated blocks 克隆到输出消息中。它相对于 Partial update "完整"的地方，是完成了重建模块到前端之间的一整套提交流程。

9.5.1 为什么不能每帧都做 Full update

传感器频率通常高于 Mesh 和场景图所需的更新频率。例如相机可能以 30 Hz 输入，意味着每约 33 m s 33\ \mathrm{ms} 33 ms 就有一帧深度；但 Marching Cubes、网格压缩、对象聚类、Places/GVD 更新和 DSG 同步都比一次 TSDF 体素融合昂贵。

如果每帧都执行：

深度融合 -> Mesh 重建 -> 对象分割 -> Places 更新 -> DSG 提交

系统会反复为还没有稳定的局部表面生成网格，并把大量中间结果传给前端。Hydra 因此把工作拆成两种频率：

高频：每帧融合观测，尽快让 TSDF / semantic / tracking 累积证据
低频：定期把累积变化生成 Mesh，并驱动下游场景图更新

这样做的核心取舍是：

• TSDF 仍然尽量使用每一帧观测，避免丢失几何和语义证据。
• Mesh 和 DSG 可以降低更新频率，减少三角形生成、压缩和图更新开销。
• 前端看到的是一批较稳定的增量，而不是每帧都变化的半成品网格。

9.5.2 两种更新分别更新什么

工作内容	Partial update	Full update
解析输入、坐标变换和语义掩码	是	是
TSDF 几何融合	是	是
颜色、语义、Tracking 体素融合	是，若相应层启用	是，若相应层启用
机器人 footprint 自由空间先验	是，若配置	是，若配置
从 TSDF 生成 Mesh	否	是，只处理 mesh_updated blocks
Active Window 归档检查	否	是
复制本轮增量地图	否	是，通过 cloneUpdated()
生成 ActiveWindowOutput	否，通常返回空指针	是，地图非空时生成
清除 block 的更新标志	否，保留给后续 full update	是，增量打包后清除
触发 GraphBuilder 的 Object/Place/Frontier 等更新	否	是，收到输出后触发

"Partial"并不意味着只更新一小部分体素。某一帧可能同时改变很多 blocks，但只要没有达到完整提交时间，它仍然是 Partial update。这里的"部分"指的是执行流程只完成了部分阶段，不是指空间范围一定较小。

9.5.3 Full update 什么时候触发

ReconstructionModule 维护一个 last_update_ns_，记录上一次成功完整提交的输入时间戳。设当前输入时间戳为 t k t_k tk，上一次 Full update 时间戳为 t l a s t t_{last} tlast，配置的完整更新间隔为 T f u l l T_{full} Tfull，则触发条件为：

doFull ⁡ ( t k ) = { true , 尚未发生过 Full update true , t k − t l a s t ≥ T f u l l false , t k − t l a s t < T f u l l \operatorname{doFull}(t_k) = \begin{cases} \text{true}, & \text{尚未发生过 Full update}\\ \text{true}, & t_k-t_{last}\geq T_{full}\\ \text{false}, & t_k-t_{last}<T_{full} \end{cases} doFull(tk)=⎩ ⎨ ⎧true,true,false,尚未发生过 Full updatetk−tlast≥Tfulltk−tlast<Tfull

对应源码中的逻辑可以读成：

如果 last_update_ns_ 为空：当前帧是 Full update
否则：比较当前输入时间戳与 last_update_ns_ 的差值
      差值 >= full_update_separation_s：Full update
      差值 <  full_update_separation_s：Partial update

这里比较的是 InputPacket 携带的传感器时间戳，不是函数实际执行所花的墙上时间。因此，即使程序因为排队而延迟处理，只要输入时间戳已经跨过间隔，下一帧仍可能直接触发 Full update。

两个边界情况也需要注意：

1. 第一个有效输入一定满足"尚未发生过 Full update"，因此会尝试执行 Full update。
2. 即使逻辑上判定为 Full update，如果 TSDF 地图仍为空，函数也会返回空输出，并不会把 last_update_ns_ 更新为当前时间；这样后续输入仍会继续尝试生成第一个有效完整结果。

如果配置：

T f u l l = 0 T_{full}=0 Tfull=0

则任意两次有序输入都满足间隔条件，基本上每帧都是 Full update。若配置为 1.0 s 1.0\ \mathrm{s} 1.0 s，则通常是每秒左右做一次完整提交，中间的输入执行 Partial update。

9.5.4 每个输入都会执行的共同阶段

无论最终属于 Partial 还是 Full，spinOnce() 在判断之后都会先处理当前帧的观测：

InputPacket
  -> 解析传感器模型、机器人位姿和观测
  -> 生成动态/无效语义掩码
  -> 把当前深度或点云融合到 TSDF
  -> 同步更新可选的颜色、语义和 Tracking 体素
  -> 可选地用机器人 footprint 标记机体附近自由空间

这一步是两种更新的共同前缀。也就是说，Full update 不是"跳过 Partial 阶段直接做另一套算法"，而是：

Full update = 当前帧的共同融合阶段 + 低频提交阶段
Partial update = 当前帧的共同融合阶段

只要某个 TSDF block 中至少一个体素真正获得有效测量，ProjectiveIntegrator 就调用 TsdfBlock::setUpdated()。该调用同时设置：

updated          本 block 的局部数据发生变化，需要进入下一次增量输出
mesh_updated     本 block 的 TSDF 变化需要重新生成 Mesh
esdf_updated     本 block 的几何变化需要被 ESDF/GVD 关注
tracking_updated 本 block 的 tracking 信息发生变化

因此，Partial update 的结果不是"什么都没有"，而是内部地图和这些标志发生了变化，只是这些变化暂时没有封装成 ActiveWindowOutput。

9.5.5 Partial update 的完整过程

假设当前帧没有达到 Full update 时间间隔，执行过程是：

1. 读取 InputPacket
2. 根据传感器名称查找内参、外参和量程
3. 把深度/点云转换成积分器需要的表示
4. 屏蔽 invalid / dynamic 语义观测
5. 投影并融合当前帧到 map_
6. 更新颜色、语义和 Tracking 层（若启用）
7. 执行可选的机器人 footprint 自由空间标记
8. 返回 nullptr，不生成前端输出

返回 nullptr 的含义是：

本次重建模块没有向输出队列发出一条新的 ActiveWindowOutput。

它不表示：

• 当前传感器数据无效；
• TSDF 没有更新；
• 当前帧被丢弃；
• 机器人位姿没有被读取。

当前帧的观测已经累积在 ReconstructionModule::map_ 中。例如连续 30 帧都属于 Partial update，TSDF 仍然可以融合这 30 帧；只是 Mesh 和前端 DSG 暂时没有收到这 30 帧的中间结果。

不过，"没有被丢弃"需要限定在重建模块内部 。按照当前默认连接方式，Partial update 返回空指针后，ActiveWindowModule 不会调用输出 sink，也不会把消息推入 output_queue。因此这些 Partial 帧不会直接进入 GraphBuilder，也就不会在这一条输出链路中单独触发：

• Agent 位姿图节点和里程计边的生成。
• 当前帧 FeatureView 到场景图节点的分配。
• Mesh、Object、Place、Frontier 和其他前端 DSG 更新。

当前帧的位姿仍然被 ReconstructionModule 用来计算传感器在 world 中的位置、投影 TSDF 和标记机器人 footprint；只是它不会以 ActiveWindowOutput 的形式向后续模块公开。于是，后端/前端看到的机器人轨迹采样频率也会受到 Full update 频率影响，除非系统另有独立的位姿输入通道。

9.5.6 Full update 的完整过程

当时间间隔达到后，当前帧仍然先执行上面的共同融合阶段，然后继续执行：

已经累积的局部 VolumetricMap + 当前帧新观测
              │
              ▼
1. 只对 mesh_updated 的 TSDF blocks 运行 MeshIntegrator
              │
              ▼
2. 根据 Active Window 归档越界 blocks
              │
              ▼
3. cloneUpdated() 克隆仍存在且 updated 的增量 blocks
              │
              ▼
4. clearUpdated() 清理已经打包的 block 更新标志
              │
              ▼
5. 构造 ActiveWindowOutput，发送给 GraphBuilder

这里每一步的作用不同。

第一步：生成 Mesh。

mesh_integrator_->generateMesh(map_, true, true) 的第一个 true 表示只处理 mesh_updated 为真的 blocks。因此 Full update 也不是从零遍历所有局部 blocks，而是把自上次网格更新以来发生变化的 TSDF 区域转换为三角网格。第二个 true 在网格处理完成后清除这些 blocks 的 mesh_updated 标志。

第二步：执行 Active Window 归档。

归档发生在清理 updated 标志之前。越界且没有待发送更新的 block 会从 TSDF、Mesh、Semantic 和 Tracking layers 移除，并把索引记录到 archived_mesh_indices。越界但刚更新的 block 会因为 skip_updated=true 暂时保留，确保最新观测先进入输出。

第三步：复制增量。

map_.cloneUpdated() 只复制 updated=true 的 blocks，而不是复制整个 map_。对于这些 blocks，如果有对应的 Mesh、Semantic 或 Tracking block，也一并复制到输出地图中。

第四步：清理标志。

只有在增量已经克隆到输出之后，代码才遍历内部 TSDF blocks 调用 clearUpdated()。这样下一个周期不会重复发送同一批已经提交的变化。

第五步：发送前端消息。

输出中的 world_t_body、world_R_body 和时间戳对应本次 Full update 的输入；map() 是本轮增量；archived_mesh_indices 是本轮删除事件；sensor_data 保存当前帧的解析结果，供 GraphBuilder 继续生成视图和位姿相关信息。

9.5.7 用具体时间线区分两者

假设：

输入频率：20 Hz，每 0.05 s 一帧
full_update_separation_s：1.0 s

处理过程可以简化为：

时间戳	是否 Full	做了什么	是否发送 ActiveWindowOutput
0.00 0.00 0.00 s	是	首帧融合、生成 Mesh、归档、复制增量、清标志	是
0.05 0.05 0.05 s	否	融合第 2 帧到 TSDF，保留更新标志	否
0.10 0.10 0.10 s	否	融合第 3 帧到 TSDF，继续累积	否
⋯ \cdots ⋯	否	每帧继续融合	否
0.95 0.95 0.95 s	否	仍未达到 1 秒	否
1.00 1.00 1.00 s 或更晚	是	融合当前帧，再处理此前累积的所有变化	是

在 1.00 1.00 1.00 s 的 Full update 中，Mesh 看到的不是只有 1.00 1.00 1.00 s 这一帧，而是从 0.05 0.05 0.05 s 到 1.00 1.00 1.00 s 期间写入 TSDF 的全部累积结果。也正因为这些变化在内部已经融合，Full update 不需要重新读取历史传感器帧。

可以用一个 block 的标志变化表示：

t = 0.00 full：
updated=true, mesh_updated=true
生成 Mesh 后 mesh_updated=false
克隆输出后 updated=false

t = 0.05 partial：
TSDF 有效更新
updated=true, mesh_updated=true

t = 0.10 partial：
再次更新同一 block
updated=true, mesh_updated=true

t = 1.00 full：
只重算 mesh_updated=true 的 block
cloneUpdated() 复制该 block 的最新累计状态
随后 clearUpdated()

9.5.8 这两个术语与"地图是否完整"的关系

最后需要避免三个常见误解：

1. Partial update 不是局部空间更新。 它可以改变视锥内很多 blocks，只是没有做低频提交阶段。
2. Full update 不是完整世界地图更新。 它只处理当前 map_ 中发生变化的 blocks，并输出增量。
3. Full update 不是重新建图。 它使用已经累积好的 TSDF，增量生成 Mesh 和前端结果。

因此更准确的中文翻译是：

Partial update：高频内部融合
Full update：低频完整提交

full_update_separation_s 控制的正是这两个阶段之间的时间间隔。它越小，前端看到的地图越及时，但 Mesh、归档、对象和 DSG 更新成本越高；它越大，TSDF 仍可高频融合，但前端场景图和显式 Mesh 的更新延迟会增加。

可选的机器人 footprint 积分器会在两种更新中都参与共同融合阶段，把机体附近配置区域标成自由空间，用于清除机器人自身造成的占用痕迹。这是系统根据"机器人本体当前所在区域应可通行"加入的先验，不是深度传感器测得的表面距离，理解数据来源时应与普通 TSDF 测量分开。

9.6 skip_updated 为什么让窗口成为"带保护的软边界"

VolumetricWindow::archiveBlocks() 默认使用：

skip_updated = true

其判断顺序可以写成：

遍历所有 TSDF blocks：
    如果 block 中心仍在窗口内：
        保留
    否则如果 block.updated == true：
        本轮暂时保留
    否则：
        加入待归档列表

从 VolumetricMap 的所有相关层删除待归档 blocks
返回这些 block 索引

这里的 updated 不只表示"当前这一帧刚更新"，而是表示：

自上一次 full update 末尾调用 clearUpdated() 以来，这个 block 是否获得过至少一次有效更新。

考虑一个已经移动到窗口外的 block A A A：

t0：上一次 full update 结束
    A.updated 被清零

t0 到 t1：某个 partial update 又看到了 A
    A.updated = true

t1：执行 full update
    A 虽然越界，但因为 updated=true，被 skip_updated 暂时保护
    A 的最新内容进入本轮增量输出
    full update 末尾 A.updated 再次清零

t1 到 t2：没有再次看到 A

t2：下一次 full update
    A 越界且 updated=false
    A 被正式归档并从局部 VolumetricMap 删除

这样做可以避免一个刚被传感器更新的 block 在同一轮尚未发给下游前就被删除。

但它也说明窗口半径不是绝对硬裁剪：如果传感器持续更新一个中心位于窗口外的 block，该 block 每次归档检查时都可能因为 updated=true 而继续保留。窗口半径明显小于传感器有效量程时，就容易出现"窗口外分配、更新、保护、稍后删除、随后又重新分配"的反复抖动。

9.7 归档时究竟删除了什么

当某个 block 被 VolumetricMap::removeBlock() 删除时，当前局部重建地图会移除同一索引下的：

• TSDF block。
• Mesh block。
• Semantic block，如果启用了语义层。
• Tracking block，如果启用了 tracking 层。

这些局部稠密数据会真正释放，不再参与下一帧 TSDF 加权平均。因此，"归档"对 ReconstructionModule::map_ 来说确实包含物理删除。

但是删除局部体素不等于删除所有全局历史。该 block 此前生成的网格增量、Object、Place 等信息可能已经进入前端全局 Mesh 和 DSG。archived_mesh_indices 会通知前端这些网格块不再活动；GVD/Place 流程也会把离开窗口的支撑和节点转入非活动生命周期。

可以把两者区别为：

层次	离开窗口后的状态
局部稠密重建层	block 被删除，不能继续直接融合旧 TSDF 值
全局压缩场景表示	历史 Mesh/Place/Object 等通常继续存在，但不再作为机器人附近的活动数据高频更新

这里也不能反向推断：全局 DSG 中还存在一个 Place，并不表示 ReconstructionModule 中还保留那个位置的 TSDF block。DSG 是压缩后的历史知识，不是稠密体素地图的无损备份。

9.8 内部地图、增量消息和归档索引有什么区别

一次 full update 结束时，至少存在四种相关状态：

状态	包含什么	是否完整	用途
ReconstructionModule::map_	当前仍保留的全部局部 TSDF、语义、Tracking 和 Mesh blocks	是，针对当前局部窗口	下一帧继续融合
ActiveWindowOutput::map()	cloneUpdated() 克隆出的 updated blocks	否，只是增量	GraphBuilder 和 GVD/Place 等前端模块消费
ActiveWindowOutput::archived_mesh_indices	本次被删除的 block 索引	否，只是删除事件	通知前端结束相应网格块的活动生命周期
全局 Mesh / DSG	历史与当前场景的压缩表示	是全局表示，但不是完整体素副本	长期查询、场景推理、闭环和后端优化

ActiveWindowOutput::map() 尤其容易被误认为"当前窗口内的完整地图"。实际上 cloneUpdated() 只遍历 TsdfBlock::updated == true 的 blocks，并克隆这些 block 的 TSDF、Mesh、Semantic 和 Tracking 数据。未变化但仍在窗口内的 block 继续存在于内部 map_，却不会在每条输出消息中重复发送。

如果前端处理速度低于重建输出速度，GraphBuilder 还可以合并多条 ActiveWindowOutput：各条增量地图通过 updateFrom() 合并，归档索引追加到同一个消息中，再统一执行一次前端更新。这进一步说明输出包是可合并的增量日志，而不是某一时刻局部地图的完整快照。

当前代码边界： ActiveWindowOutput::updateFrom() 只是追加归档索引，再把地图增量合并起来；源码中的待办注释已指出，如果同一个 block 在较早消息中更新、又在较晚消息中归档，简单追加可能无法完整表达事件先后顺序。另外，当前 VolumetricMap::updateFrom() 合并 TSDF、Mesh 和 Semantic layers，但没有合并 Tracking layer。因此，聚合后的输出应理解成供当前前端流程使用的批量增量，而不能当作能够无损重放全部局部地图生命周期的通用日志格式。

9.9 机器人重新进入归档区域时会发生什么

假设某个世界坐标区域的 block 已经离开窗口并从 ReconstructionModule::map_ 删除。之后机器人返回该区域时：

1. 当前传感器视锥再次覆盖这个 world block 索引。
2. ProjectiveIntegrator 为缺失 block 重新分配 TSDF、语义和可选 Tracking 内存。
3. 这些新体素从当前观测开始重新建立距离、权重、颜色和语义，而不是自动恢复归档前的旧 TSDF 数组。
4. 新生成的 Mesh、Object 和 Place 证据再次送入全局前端。
5. 闭环检测、数据关联和后端优化可以把当前观测与历史 DSG 对齐，但这属于上层关联和优化，不是 Active Window 自动恢复旧体素。

因此，Active Window 的长期运行策略本质上是有损压缩：远处区域保留高层历史结构，但不保证能够从 DSG 无损还原原来的每个 TSDF 体素。

9.10 窗口半径应怎样量化选择

窗口半径至少要与以下量有关：

• 传感器到机体原点的外参平移长度 ∥ B t S ∥ \|{}^{B}\mathbf{t}_{S}\| ∥BtS∥。
• 传感器实际参与积分的最大量程 R s e n s o r R_{sensor} Rsensor。
• block 半对角线 3 2 s b l o c k \frac{\sqrt{3}}{2}s_{block} 23 sblock。
• 两次 full update 间机器人可能移动的距离 v m a x Δ t f u l l v_{max}\Delta t_{full} vmaxΔtfull。
• 为位姿误差、时间同步误差和边界抖动预留的安全余量 m m m。

一个便于工程估算的下界是：

r w i n d o w ≳ ∥ B t S ∥ + R s e n s o r + 3 2 s b l o c k + v m a x Δ t f u l l + m r_{window} \gtrsim \|{}^{B}\mathbf{t}{S}\| +R{sensor} +\frac{\sqrt{3}}{2}s_{block} +v_{max}\Delta t_{full} +m rwindow≳∥BtS∥+Rsensor+23 sblock+vmaxΔtfull+m

这不是源码中的强制公式，而是根据窗口中心、传感器量程、block 判定粒度和归档周期得到的工程估算。若只希望维护比完整传感器量程更小的局部区域，也可以主动减小半径，但要接受更频繁的分配和归档。

半径过小通常会造成：

• 传感器刚分配的远端 block 很快又被归档。
• 机器人回头时，同一 world block 反复重新分配并重新融合。
• Mesh 边界频繁进入、离开活动状态。
• ESDF/GVD 的边界条件反复变化，Places 骨架在窗口边缘更容易出现短暂断裂。
• Frontier 可能更多地反映窗口裁剪边界，而不是纯粹的真实未知空间边界。

半径过大则会增加：

• TSDF、Semantic 和 Tracking 的内存。
• Marching Cubes 可能涉及的 Mesh blocks 数量。
• ESDF/GVD 更新和 Place 图维护范围。
• 前端增量消息和后端几何处理压力。

若窗口内部近似被三维空间填满，block 数量的量级为：

N b l o c k ≈ 4 3 π r w i n d o w 3 s b l o c k 3 N_{block}\approx \frac{\frac{4}{3}\pi r_{window}^{3}}{s_{block}^{3}} Nblock≈sblock334πrwindow3

因此半径翻倍时，最坏情况下活动 block 数可能接近增加到 8 倍。真实地图使用稀疏分配，通常不会填满整个球体；地面机器人场景更接近二维表面增长，但 r w i n d o w r_{window} rwindow 仍然是影响内存和实时性的关键参数。

每个体素 block 又包含：

N v o x e l = N b 3 N_{voxel}=N_b^3 Nvoxel=Nb3

个体素。例如 N b = 16 N_b=16 Nb=16 时，一个 block 有：

16 3 = 4096 16^3=4096 163=4096

个 TSDF 体素，还可能存在同尺寸的语义和 Tracking 数据。因此，窗口半径并不是"越大越准确"的单调参数，而是局部连续性、重访代价和实时预算之间的工程折中。

9.11 用一个 block 的完整生命周期串起来

假设 block A A A 的 world 中心固定为 c A \mathbf{c}_A cA，窗口半径为 8 m 8\ \mathrm{m} 8 m。

阶段 1：机器人靠近 A
距离 5 m
  -> A 在窗口内
  -> 深度持续融合进 A 的 TSDF
  -> A 生成 Mesh，并为 Object / Place 提供证据

阶段 2：机器人逐渐远离 A
距离 8.5 m，但 A 自上次 full update 后刚被再次观测
  -> A 已越界
  -> 因 updated=true，本次归档暂时跳过 A
  -> A 的最后增量先发送给前端

阶段 3：下一次 full update 前没有再看到 A
距离仍为 8.5 m，updated=false
  -> A 从局部 TSDF / Semantic / Tracking / Mesh layers 删除
  -> A 的索引写入 archived_mesh_indices
  -> GVD 支撑随后离开活动范围
  -> 对应历史 Place 等可在 DSG 中保留，但变成非活动状态

阶段 4：机器人以后返回 A
  -> 相同 world block 索引被重新分配
  -> TSDF 从新的传感器观测重新积累
  -> 新局部证据通过关联和后端优化与历史全局图重新建立一致性

这条时间线概括了 Active Window 的真正作用：它移动的不是世界坐标，而是高成本局部状态的活动边界。

10. 用一面墙串起完整更新过程

假设机器人在时刻 t 1 t_1 t1 位于墙前，输入如下：

位姿：上游 VIO 给出当前 world_T_body
外参：配置给出 body_T_camera
深度：某像素反投影后射线长度为 2.00 m
语义：同一像素标签为 wall
体素：某候选中心沿同一射线距离相机 1.96 m
截断距离：0.20 m

这一帧的处理链为：

1. 组合 W T B B T S {}^{W}\mathbf{T}{B}{}^{B}\mathbf{T}{S} WTBBTS，确定相机在世界中的位姿。
2. 根据视锥和量程分配墙附近及相机到墙之间的候选 blocks。
3. 把候选体素中心变到相机系并投影到该像素。
4. 计算 d m e a s = 2.00 − 1.96 = + 0.04   m d_{meas}=2.00-1.96=+0.04\,\text{m} dmeas=2.00−1.96=+0.04m，说明体素位于已观测自由侧且接近表面。
5. 由射线密度、深度和负侧衰减规则计算 w m e a s w_{meas} wmeas，更新 distance 和 weight。
6. 因为 ∣ d m e a s ∣ < 0.20   m |d_{meas}|<0.20\,\text{m} ∣dmeas∣<0.20m，在表面带内融合颜色和 wall 语义证据。
7. 多帧重复后，墙的位置表现为稳定的 TSDF 零交叉。
8. Full update 时，Marching Cubes 在距离异号的相邻体素之间插值出墙面三角形。
9. 只有发生变化的 blocks 被克隆给前端；远离机器人的旧 blocks 通过归档索引通知下游。

这条链中任何上游量出错，都会沿依赖关系传播：

错误	直接影响	后续表现
位姿漂移或时间不同步	不同帧访问错位世界体素	墙变厚、双层表面、Objects 重复
外参错误	所有观测带固定旋转或平移偏差	不同传感器地图不重合
深度尺度错误	射线表面距离整体缩放	Mesh 尺寸和 Place 清障距离错误
无效深度未过滤	产生错误 SDF 测量	空中碎片或错误自由空间
标签与深度未对齐	几何位置收到错误类别	语义边界漂移、Object 混合
动态类别漏检	运动表面进入静态 TSDF	拖影、残留障碍、GVD 偏移

11. 本篇最终产生了什么，又没有产生什么

经过本篇流水线，系统已经拥有：

• 世界坐标一致的局部稀疏 TSDF blocks。
• 与 TSDF 对齐的颜色、可选语义和有限时序状态。
• 从 TSDF 零交叉提取的增量三角网格。
• 更新 block 和归档 block 的生命周期信息。

但它仍然没有直接得到：

• 到全局最近障碍的欧氏距离。
• 自由空间中轴和稀疏 Places 图。
• 独立 Object 实例、Room 或 Building。
• 能考虑机器人外形和动力学的最终可执行轨迹。

TSDF 擅长融合"表面沿当前射线在哪里"，并不天然适合回答"任意自由位置离最近障碍多远"。因此下一篇会从本篇输出的已更新 TSDF blocks 出发，先构造 ESDF，再从不同最近障碍区域的交界提取 GVD，最后解释一个 Place 节点及其边到底怎样生长出来。

下一篇：从 ESDF、GVD 到 Places 图

---

关键实现证据

这里只保留决定本文主线的入口，避免把正文结尾变成重复的源码目录。能力边界和已知审计项见附录 A，逐函数阅读顺序见附录 C。

核心转换	当前实现证据
输入包提供时间戳、world_T_body、传感器身份和图像或点云	include/hydra/input/input_packet.h、include/hydra/input/sensor_input_packet.h
world_T_body * body_T_sensor 得到传感器世界位姿；相机和激光再生成顶点图或量程图	InputData::getSensorPose、src/input/camera.cpp、src/input/lidar.cpp
投影积分器只遍历候选 blocks，并用"测得表面量程减体素量程"形成 SDF 测量	src/reconstruction/projective_integrator.cpp::updateMap、computeSDF
无效或动态标签通过 mask 控制融合；几何、语义和时序状态分别写入对应体素字段	src/reconstruction/integration_masking.cpp、include/hydra/reconstruction/voxel_types.h、ProjectiveIntegrator::updateVoxel
Mesh 只重建更新 blocks，并在 8 个立方体角点之间寻找 TSDF 零交叉	src/reconstruction/mesh_integrator.cpp、src/reconstruction/marching_cubes.cpp
Partial update 继续融合体素；Full update 才生成 Mesh、归档窗口外 blocks 并输出增量	ReconstructionModule::shouldUpdate、ReconstructionModule::spinOnce、VolumetricMap::cloneUpdated