Hydra 完整案例：机器人穿过两间房和长走廊，怎样生成并校正认知地图

前九篇教程分别解释了 Hydra 的表示和算法，但真实系统不会按“今天只生成 TSDF，明天再生成 Place”的方式工作。机器人每向前运动一小段，输入、稠密重建、自由空间骨架、图实体和后端状态都在以不同频率更新。读者真正需要建立的是一段可以在脑中连续播放的过程：

机器人看到一帧
  -> 这帧怎样进入同一世界坐标
  -> 哪些体素被更新
  -> 表面和自由空间怎样出现
  -> 哪些密集证据会成为图节点
  -> 图节点怎样组成 Room 和 Building
  -> 旧区域怎样离开 Active Window
  -> 重访时怎样发现同一地点
  -> 回环残差怎样最终移动 Mesh 和场景图

本文用一个从头到尾不更换实体的案例，把这条链完整走一遍。它不是另一份概念词典，而是前九篇的“联调样例”：每个新状态都要回答它从哪里来、保存什么、何时更新、何时失效，以及下一步为什么需要它。文中的 DSG 是 Dynamic Scene Graph（动态场景图），即把几何、自由空间、物体、机器人轨迹和空间层级组织在同一张分层图中。

证据边界

• 当前代码确认：Hydra core 中可以直接核对的数据流、节点、更新器和后端行为。
• 依赖或论文方法：由 voxblox、Kimera-PGMO、TEASER++ 或 Hydra 论文定义，而不是当前仓库独立实现的全部细节。
• 案例设定：为手算和连贯叙述给出的尺寸、时间、位姿和噪声；它们不是项目默认参数。
• 外部系统职责：定位、语义网络、探索策略、路径规划和底盘控制等 Hydra core 之外的环节。

读完本文，应该能回答五个贯穿全局的问题：

1. 同一面墙怎样先后成为深度点、TSDF 零交叉、Mesh 顶点、障碍父索引和 Place 清障边界。
2. 同一个自由空间位置怎样先后成为 GVD 体素、细粒度图节点、压缩簇和持久 Place。
3. Object、Mesh Place、Agent、Frontier 和 Room 为什么不能因为都“有位置”就混成一类节点。
4. Active Window 离开某个房间后，历史地图为什么没有被删除。
5. 一条只连接两个 Agent 的回环测量，为什么最终可以校正稠密 Mesh、Places、Objects 和 Rooms。

---

1. 先固定世界：场景、坐标、传感器和实体名字

1.1 物理环境

案例位于单层办公区域。世界坐标系记为 $W$：

• $x$ 轴沿 Office A 指向 Meeting B。
• $y$ 轴指向平面图上方。
• $z$ 轴竖直向上。
• 地面近似位于 $z=0$。

平面区域采用下面的案例尺寸：

区域	世界坐标范围	物理含义
Office A	$x\in [0,6]\mathrm{m}$，$y\in [0,5]\mathrm{m}$	机器人起点所在办公室
Corridor	$x\in [6,14]\mathrm{m}$，$y\in [2,4]\mathrm{m}$	两个房间之间的长走廊
Meeting B	$x\in [14,20]\mathrm{m}$，$y\in [0,6]\mathrm{m}$	放有会议桌和投影仪
Side alcove	$x\in [9,12]\mathrm{m}$，$y\in [4,6]\mathrm{m}$	初次经过时尚未观测的储物间

Office A 中有桌子 O_desk、打印机 O_printer 和椅子 O_chair_A。Meeting B 中有会议桌 O_table、两把椅子和投影仪 O_projector。一名行人在机器人第一次通过走廊时横穿视野。

这里故意同时放入：

• 有语义的静态表面，用于形成 Object 和 Mesh Place；
• 没有可靠语义的墙面片段，用于说明没有语义也能参与 TSDF、ESDF、GVD 和 Places；
• 动态行人，用于说明语义屏蔽与真正动态跟踪的边界；
• 初始未知区域，用于生成 Frontier；
• 返回起点的轨迹，用于产生真正的 SLAM 回环。

图 1 与正文采用同一组实体：

图上元素	本文含义	后续怎样使用
浅蓝区域	当前已经观测并建立自由空间证据的区域	进入 ESDF、GVD 和 Place 提取
黄色斜线区域	还没有足够 TSDF 权重的储物间	在已知 Place 球旁形成 Frontier
紫色实体	静态语义对象候选	从语义 Mesh 聚类成 Objects
橙色人	动态类别 person	案例中从静态 TSDF 融合掩码中排除
红色虚线路径	机器人运动轨迹	被抽样成 Agent 位姿图
A_0、A_8、A_16	三个示意 Agent 采样点	进入前端 DSG 和后端位姿图

打开图 1 的 HTML 预览

1.2 传感器与上游输入

为了让输入链具体，案例使用：

• 一台 RGB-D 相机，30 Hz 输出彩色图和深度图；
• 外部 VIO（Visual-Inertial Odometry，视觉-惯性里程计）或 LIO（LiDAR-Inertial Odometry，激光雷达-惯性里程计），100 Hz 输出机器人机体位姿；
• 外部语义分割网络，为图像像素输出类别 ID；
• 已标定的相机内参和传感器到机体外参。

Hydra core 不应被理解成“拿到裸相机图像后自己完成全部定位和语义推理”。在这条案例链中：

RGB-D camera  -> color + depth
VIO / LIO     -> world_T_body(t)
semantic net  -> per-pixel labels
calibration   -> intrinsics + body_T_sensor
                         |
                         v
                  Hydra input packet

若改用激光雷达，深度图反投影可以替换为点云输入，但“每个点必须带上时间、传感器身份、外参和对应机体位姿”这一约束不变。

1.3 案例参数只为手算，不代表默认配置

后文采用下列教学值：

参数	案例值	为什么这样选
体素边长 $s_v$	$0.10\mathrm{m}$	便于把索引差换成物理距离
每个 block 的边长	$16$ 个体素	一个 block 约 $1.6\mathrm{m}$
TSDF 截断距离 $\mu$	$0.30\mathrm{m}$	表面前后各保留有限更新带
机器人安全半径	$0.35\mathrm{m}$	用于解释清障需求，不是 Hydra 控制参数
Full update 间隔	$0.5\mathrm{s}$	展示多次 Partial update 后再发布
Agent 最小采样位移	$0.5\mathrm{m}$	把高频定位压成稀疏轨迹

实际工程必须根据传感器噪声、地图分辨率、机器人尺寸和计算预算调节这些值。

---

2. 时间线：同一场景在什么时候产生哪些状态

把全过程压缩成六个阶段：

阶段	时间	机器人所在位置	新信息	关键地图变化
S0	$t=0$–$8\mathrm{s}$	Office A	首批深度、语义和位姿	TSDF、Mesh、Office 内 Places、Objects、Agent 出现
S1	$t=8$–$20\mathrm{s}$	长走廊	狭窄空间的墙面和自由空间	走廊 GVD 中轴和 Place 链向前生长
S2	$t=20$–$32\mathrm{s}$	Meeting B	新房间与对象	Meeting Objects、Places 和第二个 Room 出现
S3	$t=32$–$38\mathrm{s}$	储物间门口	已知空间旁仍有未知体素	Frontier f_0 形成，随后被新观测替换
S4	$t=38$–$55\mathrm{s}$	返回 Office A	位姿累计漂移	同一办公室在前端坐标中出现偏移副本
S5	$t>55\mathrm{s}$	Office A	历史子图与当前子图匹配	LCD 产生 Agent 测量，后端连续校正轨迹和地图

这张表最重要的不是时间数字，而是表示的依赖顺序：

没有世界位姿
  -> 不同帧不能融合到同一体素
没有 TSDF / Mesh
  -> 没有稳定表面、Object 和 Mesh Place
没有 ESDF / GVD
  -> 不知道自由空间的净空中轴
没有 Places
  -> Agent、Object、Frontier 和 Room 缺少空间骨架
没有历史图与重访
  -> 不存在可验证的长期回环
没有回环因子与形变耦合
  -> 优化 Agent 也不能自动移动稠密地图

---

3. S0 的第一步：一帧观测先回答“点在世界哪里”

3.1 定位和点云分别从哪里来

在 $t=t_8$，机器人位于走廊入口附近。系统收到的不是一个已经完成的“世界点云”，而是几类同时刻证据：

1. 深度图给出每个有效像素沿相机射线看到的表面距离。
2. 相机内参把像素和深度反投影成相机坐标点。
3. 标定外参把相机坐标点变换到机器人机体系。
4. VIO/LIO 位姿把机体系点放入世界坐标。
5. 语义图给同一像素附加类别证据。

图 2 中的数据来源不能互换：

量	来源	回答的问题
$D_t(u,v)$	RGB-D 深度相机	该像素沿当前相机射线多远遇到表面
$\mathbf{K}$	相机标定	像素方向对应哪条三维射线
${\mathbf{T}}_{BS}$	传感器外参	相机安装在机器人机体的什么位置和朝向
${\mathbf{T}}_{WB}(t)$	VIO、LIO、轮速融合或其他定位系统	当前机体在世界中的位置和朝向
$L_t(u,v)$	外部语义网络或数据集标注	像素更像 wall、desk、person 还是其他类别

打开图 2 的 HTML 预览

3.2 从像素得到相机坐标点

设像素为 $(u,v)$，深度为 $z$，针孔相机内参为 $(f_x,f_y,c_x,c_y)$。反投影为：

$$x_s=\frac{(u-c_x)z}{f_x},y_s=\frac{(v-c_y)z}{f_y},z_s=z$$

于是：

$${\mathbf{p}}_S={\left[\begin{array}{ccc}{x}_s& y_s& z_s\end{array}\right]}^T$$

其中下标 $S$ 表示传感器坐标系。以图 2 的像素为例，设：

$$(u,v)=(640,360),(c_x,c_y)=(620,350),f_x=f_y=500,z=2.4\mathrm{m}$$

则：

$$x_s=\frac{20\times 2.4}{500}=0.096\mathrm{m}$$

$$y_s=\frac{10\times 2.4}{500}=0.048\mathrm{m}$$

因此该表面点在相机坐标中约为：

$${\mathbf{p}}_S={\left[\begin{array}{ccc}0.096& 0.048& 2.4\end{array}\right]}^T\mathrm{m}$$

这个结果只说明点相对当前相机在哪里，还不能与前一秒或下一个房间的点融合。

3.3 从相机坐标进入世界坐标

定义：

• ${\mathbf{T}}_{BS}$：把传感器坐标系中的点变换到机体系。
• ${\mathbf{T}}_{WB}(t)$：把时刻 $t$ 的机体系点变换到世界系。

齐次坐标下：

$${\bar{\mathbf{p}}}_W={\mathbf{T}}_{WB}(t){\mathbf{T}}_{BS}{\bar{\mathbf{p}}}_S$$

这里 $\bar{\mathbf{p}}=[x,y,z,1{]}^T$。变换方向必须按“点现在在哪个坐标系、下一步要表达在哪个坐标系”读取：

p_S --T_BS--> p_B --T_WB--> p_W

所以这一阶段可以理解为：

上游定位先给出机器人在世界中的位姿，深度或点云再通过该位姿被放到世界坐标中。

但不能反过来说“Hydra 根据这一帧点云完成了定位”。在当前案例中，定位是输入；Hydra 使用定位来完成地图融合，并在后端用回环约束校正这条输入轨迹。

3.4 一个像素为什么知道自由空间方向

单独的标量深度 $z$ 确实没有方向，但像素 $(u,v)$ 和相机模型共同确定了射线方向：

$${\mathbf{r}}_S(u,v)=\frac{{\mathbf{K}}^{-1}{\left[\begin{array}{ccc}u& v& 1\end{array}\right]}^T}{\Vert {\mathbf{K}}^{-1}{\left[\begin{array}{ccc}u& v& 1\end{array}\right]}^T\Vert }$$

深度点约为 $z{\mathbf{r}}_S$。从相机中心到测得表面之间的射线段是“当前观测支持的自由侧”；表面之后并不会因为一个像素就被无限标成障碍。TSDF 只在有限截断带内融合负值，未被射线可靠观测的更远区域仍保持未知或低权重。

3.5 语义怎样附着到几何

若像素 $(u,v)$ 的语义输出是 wall，该标签与同一像素反投影出的三维点共享一条观测射线。系统可以把它作为该射线附近表面体素的语义证据。

这不等于“每个世界点永久保存一个确定类别”。更准确的链是：

像素类别/概率
  -> 与深度像素对齐
  -> 投影到被更新的语义体素
  -> 多帧累计类别证据
  -> Mesh 顶点继承或查询融合后的类别
  -> 同类活动 Mesh 顶点参与 Object / Mesh Place 候选

没有语义的深度仍能更新 TSDF。它只会导致对应语义体素保持 unknown，而不会让墙面从几何地图中消失。

动态类别则是另一条问题。案例假设 person 被加入动态积分掩码，因此该行人不进入长期静态 TSDF。这里的掩码只是在防止静态地图残影：

• 它不估计行人的速度；
• 不维持人的长期 Object ID；
• 不处理遮挡后的再识别；
• 不把 Object 层变成多目标跟踪器。

---

4. S0 的第二步：Active Window 怎样把输入变成 TSDF、语义体素和 Mesh

上一节已经把观测放进世界坐标，但直接累计世界点云会保留噪声、重复点和视角相关密度。Hydra 需要一个可以多帧融合、支持表面提取，又能局部更新的度量表示。这个角色由稀疏体素 block、TSDF（Truncated Signed Distance Field，截断有符号距离场）和 Active Window 共同完成。后文的 Mesh 指由顶点、边和三角面组成的三角网格表面。

图 3 的三个面板是一条严格的先后链：

面板	输入	做什么	输出
A	当前传感器视锥、有效深度和已分配 block	选择本帧可能受影响的局部 block/voxel	候选体素集合
B	候选体素中心和对应像素深度	计算投影式 SDF，融合 distance/weight/semantic	更新后的体素层
C	多帧累计体素和 full-update 时机	对 updated blocks 提取 TSDF 零交叉	Mesh 增量与地图快照

打开图 3 的 HTML 预览

4.1 体素和 block 是怎样产生的

先在世界坐标中规定一个固定分辨率网格。体素索引为：

$$\mathbf{i}={\left[\begin{array}{ccc}{i}_x& i_y& i_z\end{array}\right]}^T\in {\mathbb{Z}}^3$$

若体素边长为 $s_v=0.1\mathrm{m}$，其中心位置可写成：

$$\mathbf{x}(\mathbf{i})=s_v\left(\mathbf{i}+\frac{1}{2}1\right)$$

这里先忽略地图原点偏置。索引 $(20,30,10)$ 的体素中心约为：

$$\mathbf{x}={\left[\begin{array}{ccc}2.05& 3.05& 1.05\end{array}\right]}^T\mathrm{m}$$

实际地图不为整个无限世界预先申请体素。相邻体素被组织成固定大小的 block，只有传感器正在观测或地图需要保留的区域才分配 block。于是：

• 体素负责保存局部状态；
• block 负责稀疏内存分配、批量更新和并行调度；
• Active Window 决定哪些 block 仍参与高频重建。

4.2 本帧不会拿每条射线和全地图所有体素比较

对 $N_p$ 个像素和 $N_v$ 个全局体素做 $N_p{N}_v$ 次比较显然不可行。实际候选量被几层几何条件缩小：

1. 只考虑当前传感器视锥或量程覆盖的空间。
2. 只访问这些空间中已经分配或需要新分配的稀疏 block。
3. 体素中心必须能投影到图像范围内。
4. 对应像素必须有有效深度。
5. 只更新射线自由段和表面截断带；表面后方超过 $\mu$ 的体素跳过。
6. 后续 Mesh、GVD 和 Places 只消费本轮 updated blocks，而不是重复扫描完整地图。

因此，“沿射线比较体素与深度”描述的是 SDF 的几何定义，不表示每个像素都与全地图做笛卡尔积。

4.3 观测表面是什么

对当前像素，深度相机测得 $d_{\mathrm{surface}}=2.4\mathrm{m}$。所谓观测表面，就是这条射线在该距离处第一次返回的可见物体表面，例如墙、桌面或地面。

它有三个限定：

• 是本帧、当前视角的可见表面；
• 不是全局最近任意三维表面；
• 会受深度噪声、遮挡和动态物体影响。

多帧从不同角度重复观测同一墙面，才逐渐得到稳定的全局表面估计。

4.4 投影式 SDF 的正负号

设候选体素中心沿当前射线的距离为 $d_{\mathrm{voxel}}$。本次测量为：

$$d_{\mathrm{meas}}=d_{\mathrm{surface}}-d_{\mathrm{voxel}}$$

• $d_{\mathrm{voxel}}<d_{\mathrm{surface}}$：体素位于相机与表面之间，$d_{\mathrm{meas}}>0$。
• 二者相等：体素落在当前观测表面，$d_{\mathrm{meas}}=0$。
• $d_{\mathrm{voxel}}>d_{\mathrm{surface}}$：体素位于表面后方，$d_{\mathrm{meas}}<0$。

截断后：

$$D_{\mathrm{meas}}=\mathrm{clamp}\left(d_{\mathrm{meas}},-\mu ,\mu \right)$$

若 $\mu =0.3\mathrm{m}$，距离表面前方 $0.8\mathrm{m}$ 的体素最多记录 $+0.3\mathrm{m}$；表面后方 $0.8\mathrm{m}$ 的体素通常不会被继续深挖，或至多保留截断负值。

4.5 相机绕到墙后，符号会不会自相矛盾

关键不是“体素在相机前还是相机后”，而是体素相对同一物理表面的哪一侧。

以一堵有厚度的墙为例：

• 从 Office A 看见的是墙的 A 侧表面；
• 如果机器人真的能走到墙另一侧，看到的是墙的 B 侧表面；
• 墙内部对于两侧表面都应是负值或占据侧；
• 两侧房间的自由空间分别位于各自可见表面的正值侧。

只要位姿、深度和表面对应正确，多视角观测会形成两个零交叉边界和中间墙体，而不是把同一自由体素一会儿融合成正、一会儿融合成负。真正产生冲突的常见原因是：

• 位姿漂移使同一表面没有落入同一世界位置；
• 时间不同步；
• 动态物体被当作静态表面；
• 透明、反光或深度失效；
• 截断带过大，把不相关的背面观测混入同一体素。

TSDF 的加权平均会平滑小噪声，却不会自动解决大范围错误关联。Hydra 后端之所以还需要回环和形变，就是因为前端位姿漂移会把整片局部地图放错位置。

4.6 一个体素实际保存什么

在这条教学链里，应至少区分几类体素层，而不是说“一个体素同时就是所有地图”：

层	关键字段	字段来源	下游用途
TSDF voxel	distance、weight，按配置可有 color	深度射线多帧融合	Mesh 表面、观测状态
semantic voxel	类别证据、标签或计数/权重	对齐深度的语义观测	Mesh 语义、Object、Mesh Place
ESDF/GVD voxel	obstacle distance、parent 索引、GVD 状态等	TSDF/表面与距离传播	GVD 骨架和 Places

TSDF 距离的融合形式为：

$$D_{\mathrm{new}}=\frac{W_{\mathrm{old}}{D}_{\mathrm{old}}+W_{\mathrm{meas}}{D}_{\mathrm{meas}}}{W_{\mathrm{old}}+W_{\mathrm{meas}}}$$

$$W_{\mathrm{new}}=\min \left(W_{\mathrm{old}}+W_{\mathrm{meas}},W_{\max }\right)$$

这里 $D_{\mathrm{new}}$ 不是一帧深度，而是多帧对同一世界体素的当前估计。权重为零或接近零，表示该体素还没有足够观测，不能直接当作自由空间。

4.7 Partial update 和 Full update

两者不是两套不同地图算法，而是同一 Active Window 的不同输出节奏。

Partial update 到来时：

1. 接受当前输入帧；
2. 更新 TSDF、语义和可选 tracking 层；
3. 记录哪些 block 被改变；
4. 暂不生成发给前端的完整 ActiveWindowOutput。

它的作用是让高频传感器帧持续积累，而不强迫 Mesh、Object、GVD 和图连接器以同样高频运行。

Full update 到来时：

1. 继续完成本帧体素融合；
2. 汇总自上次发布以来的 updated blocks；
3. 对这些 block 生成或更新 Mesh；
4. 处理达到归档条件的 block；
5. 形成带时间戳、位姿、Mesh 增量和局部体素地图证据的 ActiveWindowOutput；
6. 把输出交给 GraphBuilder。

所以：

多个 Partial update
  -> 积累体素变化
  -> 一次 Full update
  -> 发布对齐的 Mesh/地图增量
  -> 前端集中更新图实体

4.8 Active Window 不是裁掉窗口外地图

Active Window 是高频可变工作集。机器人从 Office A 进入长走廊后：

• 机器人附近 block 继续接收新射线，属于 active；
• 较远 Office A block 不再需要每帧参加投影积分；
• 它们会被冻结、归档或转入长期地图状态；
• 已经生成的 Mesh、Place、Object 和历史 Agent 不会因为离开窗口就等价于被删除。

这一区分是后续回环的前提。若窗口外历史全部被丢弃，机器人返回 Office A 时就没有历史子图可检索，也没有历史几何可以校正。

4.9 Mesh 从哪里来

TSDF 表面位于 $D(\mathbf{x})=0$ 的零水平集。对一个体素立方单元的八个角点，若某条棱两端 TSDF 异号，说明零交叉穿过该棱。Marching Cubes 一类算法用这些交点构成三角面片。

这里的“八个角点”与“26 邻域”完全不同：

• 八个角点是在一个立方单元中定义局部表面拓扑；
• 26 邻域是在一个体素周围检查三维相邻体素连通；
• 前者服务于 Mesh 提取，后者后面服务于 GVD 细粒度图连接。

Full update 后，前端得到的不是整张 Mesh 的重复副本，而是与本轮 updated blocks 对应的增量和索引映射。Object 和 Mesh Place 之所以必须在 Mesh 更新后处理，就是因为它们保存的顶点索引必须与同一版累计 Mesh 对齐。

到这里，系统已经知道“表面在哪里、表面大致是什么类别”。仍然缺少的问题是：

机器人能在哪些自由空间中移动？这些连续空间怎样压缩成可搜索的拓扑图？

这需要从 TSDF/表面证据继续生成 ESDF、GVD 和 Places。

---

5. S1：机器人进入走廊，ESDF 和 GVD 怎样让 Place 一步步生长

TSDF 最擅长回答“表面在哪里”，但导航更关心“离障碍还有多远”。直接对每个查询点搜索最近三角形会很慢，因此系统需要把最近障碍距离预先传播到自由体素中。这就是 ESDF（Euclidean Signed Distance Field，欧氏有符号距离场）在本案例中的作用。

图 4 刻意把容易被口语混用的四种对象分开：

名称	是否是持久 DSG 节点	保存的核心信息	存在目的
GVD 体素	否	自由空间距离、障碍 parent、骨架证据	判断哪里靠近自由空间中轴
细粒度 GVD 图节点	否，属于提取中间图	一个保留 GVD 体素及其 26 邻域连接	保存密集骨架的真实局部连通
压缩簇	否，本轮临时集合	一组将被同一代表节点概括的细粒度节点	降低图密度
Place	是	ID、位置、清障距离、连接证据和生命周期状态	进入 DSG，供拓扑、Room、Frontier 和后端使用

打开图 4 的 HTML 预览

5.1 从 TSDF 得到 ESDF，第一步不是“复制 distance”

TSDF distance 是沿当前观测射线到表面的截断距离。它只在表面附近有高价值，且被限制在 $[-\mu ,\mu ]$。ESDF distance 则要回答：

对这个已知自由体素，三维欧氏空间中最近的障碍表面在哪里，相距多少？

所以不能直接令：

$$D_{\mathrm{ESDF}}(\mathbf{x})=D_{\mathrm{TSDF}}(\mathbf{x})$$

因为走廊中心可能离墙 $1\mathrm{m}$，而 TSDF 被截断在 $0.3\mathrm{m}$；直接复制会让所有远离墙面的自由空间看起来只有 $0.3\mathrm{m}$ 净空。

概念上的转换分为三步。

第一步：确定已观测和障碍种子。

只有 TSDF weight 足够的体素才有观测证据。表面零交叉附近、占据侧或由实现判定为固定/障碍的体素成为距离传播的种子。未观测体素不能因为“没有表面”就被当成无限自由。

第二步：为自由体素维护最近障碍 parent。

对自由体素 $v$，记其最近障碍种子索引为：

$$\pi (v)={\left[\begin{array}{ccc}{i}_x^p& i_y^p& i_z^p\end{array}\right]}^T$$

parent 不是语义父节点，也不是 DSG 父边；它只是“当前认为离 $v$ 最近的障碍体素在哪里”。

第三步：通过局部波前传播距离。

障碍种子先把距离设为接近零。随后从种子向邻居传播候选 parent。若某个邻居通过新 parent 得到更短距离，就更新其 parent 和 distance，并继续向外传播。障碍被移除或距离变大时，还需要 raise/invalidating 一类过程撤销旧 parent，再重新寻找更合适的障碍。

这使 ESDF 可以增量响应 TSDF/表面的变化，而不必在每次 Full update 后对全地图重新做一次全局最近邻搜索。

5.2 为什么可以用索引计算欧氏距离

规则体素网格的索引和物理坐标之间是线性映射。设：

$${\mathbf{i}}_v={\left[\begin{array}{ccc}20& 30& 10\end{array}\right]}^T,{\mathbf{i}}_p={\left[\begin{array}{ccc}20& 37& 10\end{array}\right]}^T$$

体素边长 $s_v=0.1\mathrm{m}$。两索引差为：

$$\Delta \mathbf{i}={\mathbf{i}}_v-{\mathbf{i}}_p={\left[\begin{array}{ccc}0& -7& 0\end{array}\right]}^T$$

对应物理位移为：

$$\Delta \mathbf{x}=s_v\Delta \mathbf{i}={\left[\begin{array}{ccc}0& -0.7& 0\end{array}\right]}^T\mathrm{m}$$

因此体素中心距离为：

$$d(v)=s_v{\Vert {\mathbf{i}}_v-{\mathbf{i}}_p\Vert }_2=0.1\times 7=0.7\mathrm{m}$$

使用索引不是用“格子编号大小”冒充物理距离，而是利用规则网格中已知的比例尺 $s_v$。若三个轴分辨率不同，就必须分别缩放；若地图不是规则欧氏网格，也不能直接使用这条公式。

5.3 走廊中心为什么成为 GVD

GVD 是 Generalized Voronoi Diagram，广义 Voronoi 图。对三维自由空间，可以把它理解为：

同时受到两个或更多不同障碍表面限制的自由空间中轴集合。

设走廊上、下两侧墙面分别产生障碍 parent $P_{\mathrm{top}}$ 和 $P_{\mathrm{bottom}}$。自由体素 $v$ 到两者的距离为：

$$d_{\mathrm{top}}(v)={\Vert {\mathbf{x}}_v-{\mathbf{x}}_{P_{\mathrm{top}}}\Vert }_2$$

$$d_{\mathrm{bottom}}(v)={\Vert {\mathbf{x}}_v-{\mathbf{x}}_{P_{\mathrm{bottom}}}\Vert }_2$$

当 $v$ 接近走廊中心时：

$$d_{\mathrm{top}}(v)\approx d_{\mathrm{bottom}}(v)$$

而且两个 parent 来自不同障碍方向。此时单独说“最近障碍是上墙”或“最近障碍是下墙”都不稳定，说明 $v$ 靠近两障碍影响区的分界，也就是自由空间中轴。

GVD 检测不是只比较两个浮点距离是否严格相等。离散体素、噪声和表面采样会使严格相等极少出现。实际提取会结合：

• 邻域中是否存在不同障碍 parent；
• parent 方向是否足够不同；
• 当前体素是否已观测且位于有效自由空间；
• 清障距离是否满足门限；
• 骨架基础点或其他实现字段是否达到要求。

因此 GVD 体素是“满足离散骨架判据的 ESDF 体素”，不是所有自由体素。

5.4 自由空间和中轴各自怎样定义

在本案例中：

• 自由空间：有观测证据，并且障碍距离大于占据/安全判定门限的体素集合；
• 未知空间：缺少足够 TSDF/ESDF 观测证据，不能默认可通行；
• 中轴：自由空间中最近障碍归属发生竞争的稀疏集合。

走廊宽约 $2\mathrm{m}$。若墙体位于 $y=2$ 和 $y=4$，理想中心线 $y=3$ 到两墙各约 $1\mathrm{m}$。因此中心附近 GVD 体素的 ESDF distance 约为 $1\mathrm{m}$。靠近门框处，最近障碍变成门框边缘，distance 会下降，形成 Place 图中的瓶颈。

这个 distance 后来成为 Place 的清障半径来源。它不是机器人已经走过的路径长度，也不是两个 Place 之间的边权本身。

5.5 一个 GVD 体素怎样成为细粒度图节点

保留下来的 GVD 体素仍只是规则网格上的状态。为了知道骨架在哪里分叉、延续或断开，提取器建立一张临时细粒度图：

每个通过筛选的 GVD voxel
  -> 一个 fine graph node
两个 fine nodes 的体素索引属于 26 邻域且连接有效
  -> 一条 fine graph edge

三维 26 邻域包含：

• 6 个共享面的邻居；
• 12 个共享棱的邻居；
• 8 个共享角点的邻居。

总数为：

$$6+12+8=26$$

这里的节点不是额外复制出的物理空间。它只是给 GVD 体素增加图邻接，使后续压缩时仍能记住“这些骨架点原本通过哪些局部体素连接”。

5.6 “簇”到底是什么

簇是压缩阶段临时放在一起的一组细粒度图节点。设第 $k$ 个簇为：

C k = { v 1 , v 2 , … , v n k } \mathcal{C}_k = \left\{ v_1,v_2,\ldots,v_{n_k} \right\} Ck​={v1​,v2​,…,vnk​​}

其中每个 $v_i$ 都是细粒度 GVD 图节点。把它们归入同一簇通常意味着：

• 空间位置足够接近；
• 属于同一局部骨架片段；
• 可以由一个更稀疏的代表节点概括；
• 压缩后仍能通过簇间边保留原始连通关系。

簇不是：

• 一个房间；
• 一组语义相同的物体；
• 一个连通分量的同义词；
• 已经写入 DSG 的 Place；
• 后端优化中的变量。

簇的寿命通常只覆盖本轮图提取/压缩。最终 Place 可以保存跨轮次 ID 和属性，临时簇本身不需要成为长期地图实体。

5.7 Place 是怎样从簇中选出来的

设簇 ${\mathcal{C}}_k$ 中每个细粒度节点位置为 ${\mathbf{x}}_i$，清障距离为 $d_i$。最简单的质心是：

$${\bar{\mathbf{x}}}_k=\frac{1}{n_k}\sum _{i=1}^{n_k}{\mathbf{x}}_i$$

但最终代表位置不一定机械地取几何质心。一个落在骨架外、清障较差或缺少稳定 GVD 证据的质心并不理想。提取器可以在候选中偏好：

• 接近簇中心；
• 清障距离较大；
• GVD basis/parent 证据更稳定；
• 与已有 Place 匹配或拓扑延续更合理的位置。

最终 Place $p_k$ 至少回答：

我代表哪一片自由空间骨架？
我的世界位置在哪里？
我到最近障碍大约多远？
我与哪些 Place 拓扑相连？
我仍在 Active Window 中，还是已经归档？
我与哪些 Mesh/形变控制证据相连？

5.8 Place 节点各字段从哪里来

把 Place 抽象为：

$$p_k=\left({\mathrm{id}}_k,{\mathbf{x}}_k,r_k,{\mathcal{B}}_k,{\mathcal{N}}_k,{\mathcal{M}}_k,s_k\right)$$

其中：

字段	含义	来源	怎样变化
${\mathrm{id}}_k$	图节点身份	新建或与旧 Place 关联时分配	合并/删除时可能被替代，不能用空间坐标冒充 ID
${\mathbf{x}}_k$	Place 世界位置	压缩簇代表位置	前端更新，后端可由 Place/形变关系校正
$r_k$	清障距离或自由空间球半径	代表 GVD/ESDF distance	新观测、重提取和后端写回时更新
${\mathcal{B}}_k$	GVD 支撑证据	簇内 basis/parent 信息	用于判断骨架稳定性和调试
${\mathcal{N}}_k$	同层 Place 邻居	细粒度图跨簇连接压缩而来	新边、失效边、合并时变化
${\mathcal{M}}_k$	Mesh/形变连接索引	Place-Mesh 连接器和压缩映射	Mesh 增量与后端形变时维护
$s_k$	active、archived、real_place 等状态	生命周期和节点类型	随窗口、Frontier/真实 Place 区分而变化

字段的精确 C++ 名称以 PlaceNodeAttributes 和当前配置为准。表中重点是来源链，而不是要求所有实现版本使用同一数学符号。

5.9 Place 边怎样从细粒度真实邻接产生

假设簇 ${\mathcal{C}}_i$ 中存在细粒度节点 $u$，簇 ${\mathcal{C}}_j$ 中存在节点 $v$，且原细粒度图中有边 $(u,v)$。压缩后可以建立：

$$(p_i,p_j)\in {\mathcal{E}}_P$$

因此“细粒度 GVD 的真实邻接关系”来源于：

GVD voxel 的 3D 邻域关系
  -> fine graph edges
  -> 记录哪些边跨越两个压缩簇
  -> 在对应 Place 之间建立压缩边

两个 Place 欧氏距离近，不足以产生边。墙两侧的 Place 可能只差 $0.3\mathrm{m}$，但原 GVD 骨架不穿过墙，就不应连通。

Place 边也不是回环边：

• Place 边表达当前地图中的自由空间拓扑邻接；
• 回环边表达两个相隔较远的时间状态实际上观测了同一地点；
• Place 图可以有环而没有发生 SLAM 回环；
• Place 图也可以是一条链，但机器人返回起点时仍会产生回环。

5.10 走廊 Place 链怎样随机器人前进生长

在 $t=8\mathrm{s}$，机器人只看到走廊入口：

Office A Places -- p_C0

到 $t=12\mathrm{s}$，更远走廊 block 获得 TSDF/ESDF：

Office A Places -- p_C0 -- p_C1

到 $t=18\mathrm{s}$，骨架延伸到 Meeting B 门口：

Office A -- p_C0 -- p_C1 -- p_C2 -- Meeting B candidates

这里“生长”不是旧 Place 自己伸长出一根线，而是每次 Full update 都发生：

1. 新 TSDF/ESDF block 进入更新集合；
2. 新 GVD 体素被检测；
3. 局部细粒度图加入新节点和边；
4. 压缩器建立新簇或把证据关联到已有 Place；
5. Places 图增加、更新、合并或删除节点/边；
6. 远离 Active Window 的稳定节点转为 archived。

5.11 Place 为什么能为导航服务，但不等于导航器

Place 图给出：

• 候选空间位置；
• 清障半径；
• 可通行拓扑边；
• 路径上的瓶颈净空线索；
• Object、Agent、Frontier 和 Room 的空间锚点。

外部导航栈仍需：

• 根据机器人 footprint 过滤净空；
• 在 Place 图上搜索全局拓扑路径；
• 把 Place 序列转成连续局部轨迹；
• 查询 ESDF/占据地图做碰撞检查；
• 输出速度和控制命令。

因此 p_A0 → p_C0 → p_C1 → p_B0 是高层空间通路，不是底盘可以直接执行的控制序列。

到这里，地图已经拥有自由空间骨架。下一步必须解决的是：

桌子、机器人轨迹、未知门口、两个房间和整栋建筑怎样挂到这条骨架上，而且每种边分别表示什么？

---

6. S1–S3：前端怎样把 Mesh、位姿和 Places 组织成 DSG

GraphBuilder 接收一次 Full update 后，不只运行一个“生成场景图”的黑盒。它并行或分阶段更新不同证据源，等待相关回调完成后，再统一建立跨层关系并形成后端输入。

图 5 的读法是：

线型/颜色	真实含义	不能误解成
绿色 Place 边	已知自由空间拓扑邻接	SLAM 回环或 Agent 时间边
灰色跨层边	Object/Agent/Place/Room/Building 的归属或锚定	可直接行走的路径
红色 Agent 边	离散轨迹时间相邻	当前仍可通行
蓝色 Mesh Place 边	语义表面区域的几何重叠/邻接	GVD Place 导航边
黄色 Frontier 边	未知候选挂到可接近的真实 Place	从真实 Place 直接穿进未知空间
紫色虚线分类路径	可选外部 Neural Tree/Hydra-GNN 推理	几何 Room 提取器本身已经完成房间类别识别

打开图 5 的 HTML 预览

6.1 Object：从“桌子类别表面”到“这一张桌子”

语义 Mesh 顶点只给出类别。例如，Office A 的桌面和 Meeting B 的会议桌可能都标成 table。Object 层还需要区分实例。

当前前端的核心链可以概括为：

活动 Mesh 顶点
  -> 只保留 object_labels 中允许的类别
  -> 按同类别和空间距离做欧氏聚类
  -> 删除点数过少的碎片
  -> 与同标签活动 Object 的包围盒/位置关联
  -> 延续旧 ID 或创建新 Object

一个 Object 节点保存的不是所有三角形副本，而是压缩引用和属性，例如：

• Object ID；
• 语义类别；
• 成员 Mesh 全局顶点索引；
• 质心；
• 包围盒；
• active/archived 状态；
• 后续可选的语义特征。

成员索引使后端 Mesh 形变后可以重新计算 Object 质心和包围盒，而不是只移动一个孤立中心点。

6.2 两把椅子共享一个 Place，会不会被压成一个 Object

不会因为共享 Place 自动合并。Place 表示“这片自由空间”，Object 表示“这一个物体实例”。

例如 Meeting B 的两把椅子 O_chair_B1 和 O_chair_B2 都可能最接近 p_B0：

          O_chair_B1
         /
p_B0 ---
         \
          O_chair_B2

它们是否为两个 Object 由各自 Mesh 聚类、标签、包围盒关联和后端合并规则决定。p_B0 只是共享空间父节点，不是只能存一个内容的槽位。

6.3 Mesh Place：为什么又有一种名字带 Place 的节点

Mesh Place 是带语义标签的 Mesh 表面区域。例如：

• Q_floor_A：Office A 的一片地面；
• Q_corridor：长走廊地面表面；
• Q_floor_B：Meeting B 地面。

它从 surface_places_labels 中允许的语义 Mesh 顶点出发，经聚类、递归切分、边界/椭圆近似后形成。其核心成员证据是 pcl_mesh_connections，即累计 Mesh 的全局顶点索引。

Mesh Place 与 GVD Place 的差异：

比较	Mesh Place $Q$	GVD Place $p$
输入	语义 Mesh 表面顶点	ESDF/GVD 自由空间骨架
代表	一块地面、墙面或其他表面区域	一个可通行自由空间位置
同层边	表面区域的重叠/邻接	自由空间骨架连通
默认跨层关系	当前默认不自动连接到 GVD Place	可作为 Object/Agent 父节点
服务	表面语义查询、边界、视图特征、随 Mesh 校正	导航拓扑、Room、Frontier、回环根和后端结构

因此图 5 中蓝色 Q_floor_A—Q_corridor 边不能替代绿色 p_A1—p_C0 边。前者没有验证机器人 footprint、障碍瓶颈或自由空间连续性。

6.4 Agent：从高频定位变成稀疏轨迹节点

外部定位可能 100 Hz 输出位姿，但 DSG 不需要每秒增加 100 个节点。Agent 生成器按时间或位移门限抽样已知 world_T_body。

若接受的位姿为：

$${\mathbf{A}}_i={\mathbf{T}}_{WB}(t_i)\in SE(3)$$

其中 $SE(3)$ 表示三维刚体旋转和平移构成的位姿空间。相邻 Agent 的里程计测量为：

$${\mathbf{Z}}_{i,i+1}^{\mathrm{odom}}={\mathbf{A}}_i^{-1}{\mathbf{A}}_{i+1}$$

这里要区分两份表示：

• DSG 中的 Agent 节点保存位置、朝向、时间戳、外部 key 和可选 BoW（Bag of Words，视觉词袋）等属性；
• 送往后端的 pose-graph packet 保存完整 ODOM 相对位姿测量。

只看 DSG 的 Agent-Agent 邻接边，不能恢复完整六自由度里程计因子。

6.5 GraphConnector 为什么在节点都更新后再运行

Object、Agent 和 Place 可能由不同回调产生。若 Object 刚创建时 Place 还没更新，立即连边会使用过期父节点。因此 GraphBuilder 在本轮实体更新完成后，再统一运行跨层连接器。

对 child $c$，默认空间连接可抽象为：

$$p^{\ast }(c)=\arg \underset{p\in {\mathcal{P}}_{\mathrm{active},\mathrm{real}}}{\min }{\Vert {\mathbf{x}}_c-{\mathbf{x}}_p\Vert }_2$$

随后建立父子关系 $p^{\ast }(c)\to c$，并维持单父约束。默认 child layer 配置会覆盖 layer 2 的主分区 Objects，以及启用 partitions 时的 Agent 分区。

该规则的作用是提供快速跨层索引：

Agent A_8 -> parent p_C0 -> corridor Place chain -> Room / Object
Object O_projector -> parent p_B1 -> Room R_B

它不证明两点之间无遮挡。Object 挂到墙另一侧的 Place 是一种可能失败模式，因为当前默认规则主要使用三维最近邻，而不是完整可见性和可通行性验证。

6.6 Frontier：储物间门口为什么出现一个临时节点

机器人第一次经过储物间门口时，走廊 p_C1 附近已经有真实 Place 球，但门后的 TSDF block 尚未获得权重。对体素 $v$，教学上可写：

$$\mathrm{unknown}(v)⟺w_{\mathrm{tsdf}}(v)<10^{-6}$$

候选 Frontier 还要求：

$$\mathrm{frontier}(v)⟺\mathrm{unknown}(v)\wedge \neg \mathrm{insidePlace}(v)\wedge \exists u\in {\mathcal{N}}_{26}(v):\mathrm{insidePlace}(u)$$

这句话逐项表示：

1. $v$ 自己尚未观测；
2. $v$ 不能已经位于真实 Place 的已知自由空间球内；
3. 它的某个 26 邻域体素 $u$ 位于已知 Place 球内；
4. 所以 $v$ 正贴着“已知可接近区域”的外缘。

大量候选体素会先聚类，再根据 PCA（Principal Component Analysis，主成分分析）得到的主轴方差切分，最终形成稀疏 Frontier f_0。它使用 real_place=false 与真实 GVD Place 区分，并连接到可接近的真实 Place。

6.7 Frontier 为机器人运动提供了什么

f_0 自己位于未知侧，不能当作已验证安全航点。它提供的是一个探索任务接口：

Frontier f_0
  -> 找到 parent real Place p_C1
  -> 外部探索器评估信息增益和任务代价
  -> 在 p_C1 附近生成安全观察位姿
  -> 局部规划器沿已知 Place/ESDF 到达观察位姿
  -> 相机朝门后观测

当门后体素获得 TSDF 权重，新自由空间生成 ESDF/GVD/Place 后：

• 旧 f_0 被真实 Place 球覆盖或因机器人接近而删除；
• 若更深处仍有未知空间，新 Frontier 在更远边界重新出现；
• 删除表示“这个旧未知边界已经失效”，不是丢失了永久世界实体。

6.8 Room：为什么不能仅按 Place 距离聚类

到 S2，Office A、长走廊和 Meeting B 都有 Places。Room 提取器要把细粒度拓扑分成更大的区域。

单纯用欧氏距离会遇到两个问题：

• 墙两侧 Place 很近，却不属于同一房间；
• 同一大房间两端 Place 很远，却应属于同一区域。

因此 Room 几何要使用 Place 图连通和清障结构。一个直观过程是：

1. 从 Places 图及其节点清障距离开始；
2. 按 clearance 阈值过滤或观察连通分量变化；
3. 宽阔区域内部的 Place 在较高阈值下仍保持成片；
4. 门口和狭窄走廊先成为瓶颈或分界；
5. 选择稳定的区域种子；
6. 将剩余 Places 分配给相应 Room；
7. 生成 Room-Place 和 Room-Room 关系。

可以把阈值 $\delta$ 下保留的 Place 子图写成：

$${\mathcal{G}}_P^{\delta }=\left({\mathcal{V}}_P^{\delta },{\mathcal{E}}_P^{\delta }\right)$$

V P δ = { p i ∣ r i > δ } \mathcal{V}_{P}^{\delta} = \left\{ p_i\mid r_i>\delta \right\} VPδ​={pi​∣ri​>δ}

$${\mathcal{E}}_P^{\delta }=\left\{(p_i,p_j)\in {\mathcal{E}}_P\mid c_{ij}>\delta ,p_i,p_j\in {\mathcal{V}}_P^{\delta }\right\}$$

其中 $c_{ij}$ 表示 Place 边沿途的瓶颈净空。当前最终核心提取使用严格大于号；候选阈值曲线的高效实现主要扫描边权事件，选出阈值后才同时检查节点 $r_i$ 和边 $c_{ij}$。

随着 $\delta$ 增大，门口和狭窄走廊的低-clearance Place 会较早退出，宽阔 Office 和 Meeting 区域保持为较稳定的连通部分。算法再利用这些变化确定房间种子和归属。

Room 不是墙面多边形本身，也不是语义标签 office。它首先是由 Place 拓扑和净空得到的几何区域节点。

6.9 房间类别 office / meeting 从哪里来

必须把两个问题分开：

1. 几何 Room 是否存在、包含哪些 Places？
2. 这个 Room 的语义类别是什么？

当前几何 Room 链根据 Places 图形成 R_A 和 R_B。论文中的 Neural Tree 或外部 Hydra-GNN 再读取 Room/Object 等图上下文，预测：

R_A + {desk, printer, office chair}
  -> office

R_B + {meeting table, chairs, projector}
  -> meeting room

神经树不是从 Room 中心坐标直接猜类别，而是利用房间内对象类别、数量、关系或学习到的图特征。分类输出可以写回 Room 的语义属性，但需要明确：

• 几何 Room 提取属于当前 core 的图构建/更新职责；
• Neural Tree/Hydra-GNN 属于论文或外部学习组件边界；
• 没有运行分类器时，Room 节点仍可存在，只是类别未知或使用默认值；
• Object 误检、漏检和跨房间错误归属会影响房间分类。

6.10 Building 为什么最后才出现

Building B0 是更高层聚合节点。只有 Room 已经存在，系统才有足够证据表达：

Building B0
  -> Room R_A
  -> Room R_B

它的中心和范围可以从 child Rooms 派生，而不是由某个传感器像素直接测得。后端校正 Places 并重建 Rooms 后，Building 也应随更新后的 Rooms 重新计算。

6.11 同一轮前端输出究竟交给谁

完成本轮 Mesh、Places、Objects、Agents、Frontiers 和连接更新后，GraphBuilder 会形成：

• 前端 DSG 增量/共享图状态；
• Mesh 增量和变形图相关增量；
• Agent pose-graph 增量；
• updated Places 和归档状态；
• 可选的 LCD 输入；
• 后端 BackendInput。

同一份前端图不是所有线程随意同时写。序列号、共享状态和队列用于让后端知道“这个 Mesh 增量、Agent 增量和 DSG 快照属于哪一轮前端状态”。

到 S3 结束时，机器人已经建立两个房间、走廊、对象和高层 Building。但随着它返回 Office A，前端定位累计误差会造成一个新的矛盾：

历史 Office A 与当前看到的 Office A 在语义和几何上很像，却被放在不同世界坐标。系统怎样确认它们是同一地点，并让整张地图连续对齐？

---

7. S4–S5：回访、场景图回环和联合后端校正

7.1 为什么返回 Office A 会产生两份偏移地图

机器人从 A_0 出发，经过约 $40\mathrm{m}$ 的累计运动后返回。假设上游 VIO/LIO 在这段时间产生：

• $x$ 方向累计漂移约 $0.8\mathrm{m}$；
• yaw 漂移约 $5^{\circ }$。

深度观测本身仍可能很准，但世界位姿 ${\mathbf{T}}_{WB}(t)$ 已偏移。于是当前 Office A 的新射线被融合到错误世界坐标附近：

历史墙面：正确坐标附近
回访墙面：整体平移并旋转
历史 O_desk：一个 Object
回访桌面候选：可能形成另一个偏移 Object
历史 Places：一组归档节点
当前 Places：在偏移坐标附近重新生长

TSDF 加权平均不应强行把相差 $0.8\mathrm{m}$ 的表面当成小噪声平均。系统首先要判断这不是两个相似办公室，而是同一地点的重访。

图 6 的三个面板分别对应：

面板	当前状态	产生的结果	后续仍需处理
A	历史和回访地图不一致	明确存在待解释的空间矛盾	不知道是否真回环，也不知道怎样分配误差
B	LCD 检索和 TEASER++ 验证成功	一条历史 Agent 到查询 Agent 的相对位姿测量	地图还没有移动
C	后端联合 Agent、控制点和临时 Place 因子	优化状态和连续形变	仍需写回 Mesh 和派生图实体

打开图 6 的 HTML 预览

7.2 历史区域离开 Active Window 后为什么仍能回环

机器人离开 Office A 时，Office block 不再参与高频重建，但相关长期状态仍被保留：

• 归档或冻结的 Mesh 几何；
• 归档 GVD Places；
• 已建立的 Objects；
• 历史 Agent 节点和时间；
• LCD 自己缓存的局部子图描述子。

Active Window 控制“本轮谁还频繁改变”，LCD 需要的是“哪些历史实体已经足够稳定，值得作为检索目标”。两者的时间角色恰好互补。

7.3 LCD 不是拿整张图与整张历史暴力比较

LCD 是 Loop Closure Detection，回环检测。当前场景图回环链可分为：

1. 等历史 Place 离当前机器人足够远，再缓存其局部描述子；
2. 等查询 Agent 已存在一小段时间，并且已经连接到父 Place；
3. 以查询 Agent 的父 Place 为根提取有限 Place/Object 子图；
4. 用 Place 描述子筛选历史根；
5. 用 Object 描述子和类别关系形成对应候选；
6. 可用时记录或比较 Agent 的视觉词袋证据；
7. 用 TEASER++ 对 Object 中心对应做鲁棒刚体配准；
8. 把地图配准变换换算为 Agent 间相对测量；
9. 将 RegistrationSolution 送往后端。

这里的“TEASER 点”就是送入 TEASER++ 的 DSG 节点位置，通常是 Object 中心；它不是一种额外传感器角点，也不是求解器自动从 Mesh 中重新提取的特征。

7.4 描述子负责找候选，TEASER++ 负责验证几何

Office A 中的 desk、printer 和椅子构成一个局部语义排列。查询子图和历史子图描述子相似，只能说明“值得尝试配准”，不能直接产生回环边。

假设候选 Object 对应为：

$$\left({\mathbf{s}}_i,{\mathbf{d}}_i\right),i=1,\dots ,N$$

其中：

• ${\mathbf{s}}_i$ 是查询/source 子图中的 Object 中心；
• ${\mathbf{d}}_i$ 是历史/destination 子图中的候选中心。

TEASER++ 估计刚体变换：

$${\mathbf{d}}_i\approx {\mathbf{R}}_{DS}{\mathbf{s}}_i+{\mathbf{t}}_{DS}$$

写成齐次形式：

$${\mathbf{T}}_S^D=\left[\begin{array}{cc}{\mathbf{R}}_{DS}& {\mathbf{t}}_{DS}\\ 0^T& 1\end{array}\right]$$

${\mathbf{T}}_S^D$ 的物理意义是：

把查询局部地图 $S$ 中的点移动到历史局部地图 $D$ 中。

它不是查询 Agent 的全局优化位姿。

7.5 地图配准变换怎样换成 Agent 测量

后端变量不是“source map”和“destination map”，而是历史 Agent $B_h$ 与查询 Agent $B_q$。因此需要把地图变换换算成：

$${\mathbf{Z}}_{hq}={\mathbf{T}}_{B_q}^{B_h}$$

其含义是：把查询机体系 $B_q$ 中的点表达在历史机体系 $B_h$ 中。

从一个点开始推导。设 ${\mathbf{p}}_{B_q}$ 是查询机体系中的点。

第一步，把它放入查询局部地图：

$${\mathbf{p}}_S={\mathbf{T}}_{B_q}^S{\mathbf{p}}_{B_q}$$

第二步，用 TEASER++ 结果把查询地图对齐到历史地图：

$${\mathbf{p}}_D={\mathbf{T}}_S^D{\mathbf{p}}_S$$

第三步，从历史地图转到历史 Agent：

$${\mathbf{p}}_{B_h}={\left({\mathbf{T}}_{B_h}^D\right)}^{-1}{\mathbf{p}}_D$$

代入前两步：

$${\mathbf{p}}_{B_h}={\left({\mathbf{T}}_{B_h}^D\right)}^{-1}{\mathbf{T}}_S^D{\mathbf{T}}_{B_q}^S{\mathbf{p}}_{B_q}$$

因此：

$$\boxed{{\mathbf{Z}}_{hq}={\mathbf{T}}_{B_q}^{B_h}={\left({\mathbf{T}}_{B_h}^D\right)}^{-1}{\mathbf{T}}_S^D{\mathbf{T}}_{B_q}^S}$$

这条式子解释了两者差异：

• ${\mathbf{T}}_S^D$ 移动整份查询子图；
• ${\mathbf{Z}}_{hq}$ 描述两个机器人时刻之间应满足的相对位姿；
• 二者只有在局部地图坐标与 Agent 坐标恰好重合时才会数值相同。

7.6 回环因子残差怎样定义

设后端当前估计的历史和查询 Agent 位姿为 ${\mathbf{A}}_h,{\mathbf{A}}_q\in SE(3)$。由状态预测的相对位姿是：

$${\hat{\mathbf{T}}}_{hq}={\mathbf{A}}_h^{-1}{\mathbf{A}}_q$$

测量为 ${\mathbf{Z}}_{hq}$。群上的误差变换为：

$${\mathbf{E}}_{hq}={\mathbf{Z}}_{hq}^{-1}{\mathbf{A}}_h^{-1}{\mathbf{A}}_q$$

再用李群对数映射得到六维残差：

$${\mathbf{r}}_{\mathrm{loop}}=\mathrm{Log}\left({\mathbf{Z}}_{hq}^{-1}{\mathbf{A}}_h^{-1}{\mathbf{A}}_q\right)\in {\mathbb{R}}^6$$

前 3 个分量可理解为局部旋转误差，后 3 个分量可理解为局部平移误差，具体排列取决于库约定。加权代价为：

$$E_{\mathrm{loop}}={\mathbf{r}}_{\mathrm{loop}}^T{\mathbf{\Omega }}_{\mathrm{loop}}{\mathbf{r}}_{\mathrm{loop}}$$

${\mathbf{\Omega }}_{\mathrm{loop}}$ 是信息矩阵，表示该测量各方向的相对可信度。鲁棒优化还可对潜在错误回环施加 GNC（Graduated Non-Convexity，渐进非凸优化）或其他鲁棒权重。

7.7 后端到底优化哪些量

本案例的联合状态可概括为：

X = { A i , M k , P l } \mathcal{X} = \left\{ \mathbf{A}_i, \mathbf{M}_k, \mathbf{P}_l \right\} X={Ai​,Mk​,Pl​}

其中：

• ${\mathbf{A}}_i$：Agent 位姿变量；
• ${\mathbf{M}}_k$：稀疏 Mesh 形变控制点的旋转和平移；
• ${\mathbf{P}}_l$：启用 Place 后端耦合时临时加入的 Place 位姿变量。

Objects、Mesh Places、Rooms、Building 和 Frontiers 通常不是各自新增一套永久优化变量。它们依赖校正后的 Mesh、Places 和规则更新器重算。

7.8 网格形变控制点是什么

稠密 Mesh 可能有数十万顶点。若每个顶点都是独立优化变量，计算量大，而且邻近表面容易被拉裂。形变图从 Mesh 顶点中按空间和时间规则选取稀疏控制点。

第 $k$ 个控制点包含：

$${\mathbf{M}}_k=\left({\mathbf{R}}_k^m,{\mathbf{t}}_k^m\right)$$

• 原始锚点位置 ${\mathbf{g}}_k$ 来自对应 Mesh 位置；
• ${\mathbf{t}}_k^m$ 表示优化后控制点位置；
• ${\mathbf{R}}_k^m$ 表示控制点附近局部方向的形变；
• 相邻控制点通过 Mesh-Mesh 形变边连接。

控制点不是语义关键点，也不是曲率最大点。它的职责是以较少变量控制一片局部 Mesh。

7.9 ARAP 残差为什么能防止 Mesh 撕裂

ARAP 是 As-Rigid-As-Possible，尽可能刚性。考虑控制点 $i$ 和 $j$，原始位置为 ${\mathbf{g}}_i,{\mathbf{g}}_j$，优化后位置为 ${\mathbf{t}}_i,{\mathbf{t}}_j$。

若 $i$ 附近保持局部刚性，原始相对向量：

$${\mathbf{g}}_j-{\mathbf{g}}_i$$

经过 $i$ 的局部旋转后，应接近优化后的相对向量：

$${\mathbf{t}}_j-{\mathbf{t}}_i$$

所以一个有方向残差可写成：

$${\mathbf{r}}_{ij}^{\mathrm{arap}}={\mathbf{R}}_i\left({\mathbf{g}}_j-{\mathbf{g}}_i\right)+{\mathbf{t}}_i-{\mathbf{t}}_j$$

若所有控制点做同一个整体刚体变换，该残差可以保持接近零；若相邻控制点被不连续地拉开、扭转或错位，残差增大。

这就是 ARAP 的物理作用：

• 允许长距离地图缓慢弯曲来吸收累计漂移；
• 抵抗局部三角面突然撕裂；
• 不要求整个地图只能做一个全局刚体变换。

7.10 Agent-Mesh 耦合边怎样把轨迹和控制点放进同一方程

回环残差只显式含 ${\mathbf{A}}_h$ 和 ${\mathbf{A}}_q$。若 Agent 与控制点之间没有任何共同因子，优化器即使修正 Agent，也不会知道 Mesh 应该跟着动。

Agent-Mesh 耦合边使用与形变边相似的位置一致性残差。概念上，它约束：

某个 Agent 附近的控制点，在优化后仍应保持与该 Agent 原始局部关系相符。

设 Agent 原始锚点为 ${\mathbf{g}}_a$，控制点原始位置为 ${\mathbf{g}}_m$，优化 Agent 为 $({\mathbf{R}}_a,{\mathbf{t}}_a)$，控制点优化位置为 ${\mathbf{t}}_m$。一个方向的残差可以写成：

$${\mathbf{r}}_{am}={\mathbf{R}}_a\left({\mathbf{g}}_m-{\mathbf{g}}_a\right)+{\mathbf{t}}_a-{\mathbf{t}}_m$$

它同时包含 Agent 变量和控制点变量。因此回环要求 Agent 移动时，保持 Agent-Mesh 局部关系会要求相邻控制点也移动；控制点之间的 ARAP 又把校正继续传播到更远 Mesh。

7.11 用一维手算看残差怎样推动控制点

先忽略旋转，只看 $x$ 方向校正：

• 查询 Agent 的修正量为 $a$；
• 与它相连的控制点修正量为 $m_1$；
• 邻近控制点修正量为 $m_2$；
• 回环希望查询 Agent 向左移动 $0.8\mathrm{m}$，即 $a\approx -0.8$；
• 历史端通过先验或大量旧约束近似固定。

构造一个单位权重教学代价：

$$E(a,m_1,m_2)=(a+0.8{)}^2+(m_1-a{)}^2+(m_2-m_1{)}^2+m_2^2$$

四项依次表示：

1. 回环残差：$a$ 应接近 $-0.8$；
2. Agent-Mesh 耦合：$m_1$ 不应脱离 $a$；
3. ARAP 简化项：相邻控制点修正不应突变；
4. 远端锚定：$m_2$ 不能无限自由漂移。

令偏导为零：

$$\frac{\partial E}{\partial a}=2(a+0.8)+2(a-m_1)=0$$

$$\frac{\partial E}{\partial m_1}=2(m_1-a)+2(m_1-m_2)=0$$

$$\frac{\partial E}{\partial m_2}=2(m_2-m_1)+2m_2=0$$

化简：

$$2a-m_1+0.8=0$$

$$2m_1-a-m_2=0$$

$$2m_2-m_1=0$$

解得：

$$a=-0.6\mathrm{m},m_1=-0.4\mathrm{m},m_2=-0.2\mathrm{m}$$

结果没有把全部 $0.8\mathrm{m}$ 瞬间施加到一个点：

• Agent 吸收最大修正；
• 近控制点跟随一部分；
• 更远控制点继续平滑衰减；
• 回环、耦合、ARAP 和锚定之间共同折中。

真实后端在 $SE(3)$ 上同时优化旋转和平移，并有大量 Agent、控制点和因子，但“误差通过共享变量传播”的机制与这个一维例子相同。

7.12 从 Jacobian/Hessian 看传播路径

在线性化点附近，把 Agent 增量记为 $\delta \mathbf{a}$，控制点增量记为 $\delta \mathbf{m}$。联合正规方程具有块结构：

$$\left[\begin{array}{cc}{\mathbf{H}}_{aa}& {\mathbf{H}}_{am}\\ {\mathbf{H}}_{ma}& {\mathbf{H}}_{mm}\end{array}\right]\left[\begin{array}{c}\delta \mathbf{a}\\ \delta \mathbf{m}\end{array}\right]=-\left[\begin{array}{c}{\mathbf{g}}_a\\ {\mathbf{g}}_m\end{array}\right]$$

回环因子增加 ${\mathbf{H}}_{aa}$ 和 ${\mathbf{g}}_a$；Agent-Mesh 因子因为同时含两类变量，产生非零交叉块：

$${\mathbf{H}}_{am}\ne 0$$

所以解整个线性系统时，$\delta \mathbf{a}$ 的变化会通过 ${\mathbf{H}}_{am}$ 影响 $\delta \mathbf{m}$。若删掉 Agent-Mesh 因子，则：

$${\mathbf{H}}_{am}=0$$

Agent 和 Mesh 子问题解耦，回环只能改轨迹，不能自动改控制点。

7.13 Places 为什么在后端使用 MST

完整 Places 图可能含多条替代路径和环。后端为了以较少临时因子保持 Place 子图连通，会从每个连通分量的已有 Place 边中选择 MST（Minimum Spanning Tree，最小生成树；多分量时是最小生成森林）。

MST 与完整 Place 图的区别不只是“一个没环、一个有环”：

• 完整 Place 图继续保留，供拓扑查询和导航；
• MST 只是本轮优化使用的稀疏结构；
• MST 只能从已有 Place 边中选，不会穿墙造边；
• 未选中的 Place 边不会从 DSG 删除；
• 树叶 Place 可通过 deformation_connections 接到 Mesh 控制点，内部 Place 再沿树获得校正传播。

Place 边本来是自由空间拓扑，不一定是彼此独立的高精度六自由度测量。全部作为强因子会高估信息和增加计算；MST 是保持连通与稀疏性的工程折中。

7.14 联合目标是什么

把主要项放在一起，概念目标可写为：

$${\mathcal{X}}^{\ast }=\arg \underset{\mathcal{X}}{\min }\left(\sum E_{\mathrm{prior}}+\sum E_{\mathrm{odom}}+\sum \rho (E_{\mathrm{loop}})+\sum E_{\mathrm{mesh}\text{-}\mathrm{mesh}}+\sum E_{\mathrm{agent}\text{-}\mathrm{mesh}}+\sum E_{\mathrm{place}\text{-}\mathrm{place}}+\sum E_{\mathrm{place}\text{-}\mathrm{mesh}}\right)$$

各项不是同一物理单位下的简单相加；协方差/信息矩阵决定相对刚度，鲁棒核 $\rho$ 降低错误回环的影响。优化器在这些约束之间寻找整体一致的 Agent、控制点和临时 Place 状态。

7.15 优化结果怎样真正校正地图

求解结束只得到变量的新值，还需要按表示依赖写回：

1. 用优化结果覆盖或更新 Agent 位姿。
2. 用多个邻近控制点的变换加权插值每个 Mesh 顶点。
3. 得到连续形变后的累计 Mesh。
4. 根据成员 Mesh 索引重算 Object 的质心和包围盒。
5. 根据 pcl_mesh_connections 重算 Mesh Place 位置、边界和可能的分裂/合并。
6. 更新、合并或重新定位 Places，并维护其 Mesh/形变连接。
7. 清理已被真实空间覆盖的 Frontiers。
8. 从校正后的 Places 重新构造 Rooms。
9. 从更新后的 Rooms 重算 Building。

回环不会直接把整张地图整体平移 $-0.8\mathrm{m}$。早期有强先验的区域移动较少，晚期漂移段移动较多，中间通过里程计、Agent-Mesh 和 ARAP 形成连续过渡。

---

8. 校正后的认知地图能回答什么

图 7 不是把所有能力都归给 Hydra。它明确区分“地图提供的证据”和“外部模块完成的动作”。

打开图 7 的 HTML 预览

8.1 查询投影仪在哪里

图查询可以沿：

O_projector
  -> parent p_B1
  -> parent Room R_B
  -> parent Building B0

得到：

• 语义实例是投影仪；
• 位于哪个自由空间锚点附近；
• 属于哪个 Room；
• 属于当前 Building。

若只保留点云和逐点标签，每次查询都要重新做实例聚类、空间归属和房间推断。

8.2 从 Office A 到 Meeting B

高层路径可以先在 Room 层选择目标区域，再下钻到 Place：

R_A
  -> p_A1
  -> p_C0
  -> p_C1
  -> p_B0
  -> R_B

Place 边给出已知自由空间拓扑。外部导航器还要：

• 检查每个 Place/边的净空是否适合当前机器人；
• 用 ESDF 或局部代价地图生成连续无碰轨迹；
• 处理临时动态障碍；
• 输出控制命令。

如果 A 与 B 之间没有任何 Place 边，Hydra 当前地图就没有证据证明二者通过已知自由空间连通。机器人不能凭欧氏直线穿过未知或墙体；它只能继续探索 Frontier、等待地图补全，或由外部系统提供另一条已验证连接。

8.3 下一处值得探索哪里

Frontier 提供：

• 未知边界中心或形状；
• 它依附的真实 Place；
• 当前活动/归档状态。

外部探索器再组合信息增益、到达成本、电量、风险和任务优先级选择目标。Frontier 被观测后删除，是因为问题“这里还是已知与未知的边界吗”已经有了新答案。

8.4 机器人何时经过走廊

Agent 节点保存时间戳和校正后位姿，并锚定到 Place。查询可以沿：

timestamp
  -> Agent A_i
  -> parent Place p_C*
  -> corridor / Room relation

Agent 与 Object 的空间关系还可支持“机器人在哪个时刻经过打印机附近”等时空查询。

8.5 Mesh Place 为机器人提供什么

Q_floor_A 等 Mesh Place 能提供：

• 一块语义表面的代表位置；
• 边界/尺度近似；
• 指向真实 Mesh 顶点的索引；
• 可选视图和语义特征；
• 回环后随变形 Mesh 重算的区域几何。

它适合回答“哪块表面是地面”“这个表面区域的边界和外观是什么”，但不直接输出底盘路径。到达该表面附近仍需通过 GVD Place、ESDF 和外部导航。

---

9. 把整个案例压成一张状态来源与生命周期表

下面这张表是检查自己是否真正理解 Hydra 的最短路径。若某一行只能说出名字，却说不出 source、update、lifetime 和 consumer，说明该概念仍停留在术语层。

状态/实体	直接来源	初始化或筛选	怎样更新	何时失效/归档	主要消费者
输入深度/点云	RGB-D、LiDAR	时间、量程、有效值、传感器模型	每帧替换	消费后不作为长期地图实体	TSDF 积分
机器人输入位姿	VIO/LIO/其他定位	时间关联和坐标约定	高频连续输入	后续可被后端校正的 Agent 表示替代	点变换、Agent pose graph
TSDF voxel	世界射线和深度	视锥、block、投影、截断带	distance/weight 加权融合	block 归档或地图清理	Mesh、观测状态、ESDF/GVD
semantic voxel	对齐深度的标签/概率	有效类别和积分掩码	多帧语义证据融合	与体素层一起归档/重建	Mesh 语义、Objects、Mesh Places
Mesh vertex/face	TSDF 零交叉	updated blocks 的局部表面提取	MeshDelta 合并、后端形变	索引压缩、顶点删除或替换	Objects、Mesh Places、控制点、可视化
ESDF/GVD voxel	TSDF/障碍种子和距离传播	已观测自由、parent 竞争、骨架门限	局部波前和 updated blocks	parent 失效、障碍变化、窗口归档	细粒度 GVD 图
fine GVD node	通过筛选的 GVD voxel	26 邻域连接有效	局部添加/删除/连边	压缩或本轮提取结束	cluster/compression
cluster	一组细粒度节点	空间与拓扑压缩规则	本轮合并、切分、代表选择	Place 生成后丢弃	Place 生成
GVD Place	压缩簇及旧节点关联	代表位置、净空、支撑证据	更新、合并、删除、active/archived	证据消失或被合并；历史可归档	导航、Room、Frontier、LCD、后端
Place edge	fine graph 跨簇连接	必须有自由空间拓扑证据	随局部图变化维护	连接证据失效	路径、Room、MST
Object	语义 Mesh 顶点聚类	object label、点数、空间连通	成员融合、ID 关联、几何重算	成员不足删除或离窗归档	查询、Room semantic、LCD
Mesh Place	surface-label Mesh 聚类	类别、聚类、递归切分	顶点引用、边界、同层边重算	活动节点可重建，跨窗部分冻结/归档	表面语义与区域查询
Agent	抽样输入位姿	时间/位移采样规则	新节点、ODOM 测量、后端位姿写回	历史永久保留或按系统策略清理	LCD、后端、时序查询
Frontier	unknown voxel 紧邻 Place 球	高度、聚类、PCA 切分、real_place=false	活动候选重建	被观测、Place 覆盖或机器人接近时删除	外部探索器
Room	校正前/后的 Places 图	净空 filtration、连通和种子分配	Places 变化后重建	归属变化、合并或重建	高层查询、Building、可选分类器
Room category	Room/Object 图特征	Neural Tree/Hydra-GNN 等外部模型	重新推理	图证据或模型变化	语义查询和任务规划
Building	Rooms	聚合现有 Rooms	Room 更新后重算	无 Room 或结构重建	顶层组织
LCD result	当前/历史 Place-Object 子图	描述子门控、TEASER++ 内点验证	每个成功回环产生测量	被后端拒绝、鲁棒降权或消费	联合后端
Agent/control variable	pose graph、形变图	key 与控制点选择	非线性优化	旧临时 Place 变量每轮重建	校正 Agent 和 Mesh

---

10. 这个案例刻意没有声称什么

一个可以发布的教程还必须明确系统边界。

10.1 Hydra core 不是完整导航栈

它提供 ESDF/GVD、Places、Room 和语义关系，但不自动完成所有：

• 全局路径规划；
• 局部动态避障；
• 轨迹优化；
• 速度控制；
• 底盘驱动。

10.2 语义标签通常来自外部模型

当前 core 可以消费、规范化和融合语义证据，但不应把所有像素分类都说成 Hydra 内部固定网络的输出。数据集适配器、ROS 系统或外部分割网络负责提供标签。

10.3 动态掩码不是完整动态世界模型

把 person 从静态 TSDF 中排除能减少残影，却没有自动解决长期身份、轨迹预测和动态 Object 生命周期。

10.4 几何 Room 与语义 Room category 不同

Place 图可以生成没有类别的几何 Room。office、meeting 等类别需要单独的分类器或外部推理链。

10.5 Place 路径不等于保证可执行

Place 边是自由空间拓扑证据。机器人 footprint、实时障碍、运动学约束和控制误差仍需外部规划器验证。

10.6 回环成功不等于地图已经修改

必须依次确认：

描述子有候选
  -> TEASER++ 有足够一致内点
  -> RegistrationSolution 进入 backend_lcd_queue
  -> 后端触发优化
  -> Agent-Mesh / deformation graph 保持耦合
  -> optimized Mesh 写回
  -> DsgUpdater 重算派生实体

任何一段断开，都可能出现“日志显示回环成功，但地图没有变化”。

---

11. 用八句话复述全过程

1. 外部定位给出机器人位姿，深度/点云和语义观测通过外参进入同一世界坐标。
2. Active Window 只为当前局部视锥和截断带附近的稀疏 block 更新 TSDF 与语义体素。
3. 多次 Partial update 积累体素证据，Full update 从 updated blocks 提取 Mesh 并发布一致增量。
4. 障碍种子和 parent 距离传播形成 ESDF，多个障碍 parent 竞争的位置形成 GVD。
5. GVD 体素先成为 26 邻域细粒度图节点，再组成临时压缩簇，最后生成持久 Places 和拓扑边。
6. 语义 Mesh 产生 Objects/Mesh Places，抽样位姿产生 Agents，未知边界产生 Frontiers，Places 再生成 Rooms 和 Building。
7. 返回起点时，历史与当前 Place/Object 子图经描述子和 TEASER++ 产生一条 Agent 相对位姿回环测量。
8. 后端联合 Agent、控制点、ARAP、Agent-Mesh 和 Place 结构优化，形变 Mesh 并重算 Objects、Places、Rooms 与 Building。

完整表示链可以写成：

sensor + pose + semantics
  -> normalized world observations
  -> sparse TSDF / semantic volume
  -> Mesh + ESDF
  -> GVD voxels
  -> fine GVD graph
  -> temporary clusters
  -> persistent Places
  -> Objects / Mesh Places / Agents / Frontiers
  -> Rooms / Building / optional room semantics
  -> LCD Agent measurement
  -> joint Agent-control-Place optimization
  -> deformed Mesh and corrected DSG

---

12. 与前九篇教程的对应关系

本文只把已有原理放进同一案例，不替代各主定义文章：

本文阶段	继续深读
为什么需要 DSG	第 1 篇
输入、队列和模块边界	架构导读
深度、TSDF、语义、Mesh、Active Window	第 2 篇
ESDF、GVD、簇、Places	第 3 篇
Objects、Mesh Places、Agents、跨层边	第 4 篇
Frontier	第 5 篇
Rooms、Building、Neural Tree	第 6 篇
场景图回环和 TEASER++	第 7 篇
联合后端、MST、形变和写回	第 8 篇
Agent 残差到控制点的 Jacobian/Hessian	第 9 篇

---

13. 当前仓库的关键证据入口

正文按概念组织，下面只列最短核对路径：

机制	关键源码入口
输入接收、位姿查询和 packet 规范化	include/hydra/input/input_module.h、src/input/input_module.cpp、src/input/input_conversion.cpp
Active Window、Partial/Full update、TSDF/Mesh 输出	src/active_window/active_window_module.cpp、src/active_window/reconstruction_module.cpp
GraphBuilder 回调、Places、Objects、Agents 和跨层连接	src/frontend/graph_builder.cpp、src/frontend/graph_connector.cpp
Object 与 Mesh Place 分割	src/frontend/mesh_segmenter.cpp、src/frontend/place_2d_segmenter.cpp
Frontier	src/frontend/frontier_extractor.cpp
Rooms 和 Building 更新	src/backend/update_rooms_functor.cpp、src/backend/update_buildings_functor.cpp
LCD 缓存、检索与注册	src/loop_closure/loop_closure_module.cpp、src/loop_closure/detector.cpp、src/loop_closure/registration.cpp
后端输入、优化、Mesh 形变和 DSG 写回	src/backend/backend_module.cpp、src/backend/dsg_updater.cpp
Agent pose graph	src/odometry/pose_graph_from_odom.cpp、src/odometry/pose_graph_tracker.cpp

外部依赖和论文边界继续以附录 A为准；调参和故障排查见附录 B；按源码顺序阅读见附录 C。