DVMSLAM中的sim3变换
DVM-SLAM 中的 Sim(3) 变换：从地图合并到持续对齐
核心结论：Sim(3) 是 DVM-SLAM 解决“单目多机器人坐标系不一致”的几何工具，它负责把不同 agent 的地图从各自 world frame 对齐到同一个 group frame。
1. 一句话解释 Sim(3)
在 DVM-SLAM 中，每个机器人本地运行单目 ORB-SLAM，并独立建立自己的地图。由于是单目相机，每个机器人得到的地图尺度并不天然一致：robot A 认为走廊长 10 个单位，robot B 可能认为同一段走廊长 7 个单位。因此，两个地图不能只靠旋转和平移拼接，还需要估计一个尺度因子。
Sim(3) 变换可以写成：
x′=sRx+t \mathbf{x}' = s\mathbf{R}\mathbf{x} + \mathbf{t}x′=sRx+t
其中：
| 符号 | 含义 | 在 DVM-SLAM 中解决的问题 |
|---|---|---|
| sss | scale，尺度 | 单目地图尺度不一致 |
| R\mathbf{R}R | rotation，旋转 | 两个 agent 坐标轴方向不同 |
| t\mathbf{t}t | translation，平移 | 两个 agent 地图原点不同 |
齐次矩阵形式：
TSim(3)=[sRt0T1] T_{Sim(3)} = \begin{bmatrix} s\mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix}TSim(3)=[sR0Tt1]
总共 7 个自由度。
在 g2o 中用 g2o::Sim3 表示，通常记作 gScw（Sim3 from world to camera）。
单目 SLAM 每台机器人独立建图时，两张地图在 overlap 区域的真实几何关系就是 Sim3
世界坐标系原点和朝向 是任意的（arbitrary）。
绝对尺度 也是任意的（同一物理场景，A 机器人地图里 1m 可能在 B 机器人地图里是 0.7m 或 1.3m）。
符号,含义 gScw,当前相机在世界系下的 Sim3 位姿 gSiw,第 i 个 KeyFrame 在世界系下的 Sim3 gScm,Current KF 与 Matched KF（对端）之间的 Sim3 mg2oMergeScw,最重要：Merge 确认后，当前锚点 KF 在目标 Merge 世界系下的 Sim32. 为什么 DVM-SLAM 不能只用 SE(3)？
SE(3) 只包含旋转和平移：
x′=Rx+t \mathbf{x}' = \mathbf{R}\mathbf{x} + \mathbf{t}x′=Rx+t
它默认两个地图的尺度已经相同。但 DVM-SLAM 是纯单目 decentralized C-SLAM，每个 agent 都可能在任意未知位置独立初始化，没有全局尺度先验。论文也强调，许多去中心化系统不用单目视觉的重要原因之一，就是单目会引入 arbitrary map scale 和更频繁的定位丢失问题。
SE3（刚体变换）只有 6 DoF，固定 s=1。
Sim3 允许整张地图整体缩放，但保持形状（角度和相对比例）不变。
因此，在 DVM-SLAM 里：
- 机内局部跟踪 / 局部 BA：主要在本机地图尺度下工作；
- 跨机地图合并：必须估计 Sim(3)，否则不同尺度地图会被错误焊接；
- 合并后的地图校准：仍需要周期性重新估计 Sim(3)，防止早期合并误差逐渐放大。
3. Sim(3) 在 DVM-SLAM 中出现的两个关键位置
3.1 第一次地图合并：Map Merging
DVM-SLAM 的地图合并不是一开始就传整张地图，而是先通过 BoW 判断“是否可能看到了同一个地方”。流程可以理解为：
关键点：
- BoW 阶段只负责找候选：它判断“两个关键帧看起来像不像同一地点”。
- 真正合并时需要完整地图信息：候选成立后才交换地图，执行几何验证和地图焊接。
- 合并成功后得到Tj→iT_{j\rightarrow i}Tj→i：如果i<ji<ji<j，论文描述为 agent j 将自己的坐标空间变换到 agent i 的地图中，并把这个变换传播给自己组内成员。
- 组内 agent 共享同一坐标系：只有这样，后续 keyframe / mappoint 才能无需再次变换地插入本地共享地图副本。
3.2 合并后的持续校准：Map Alignment Refiner
DVM-SLAM 使用“早期合并”策略：只要发现足够强的重叠证据，就会尽早合并地图。这样有利于快速建立 group，但也会带来一个问题：
刚合并时重叠区域很小，Sim(3) 估计可能有轻微尺度、旋转或平移误差。地图越长，这个误差越容易被放大，最终导致共享地图逐渐发散。
为了解决这个问题，DVM-SLAM 在合并后持续执行Map Alignment Refiner。其核心流程是：
这里的优化目标可以写成：
Tlocal→leader∗=argmins,R,t∑k∈M∥pkleader−(sRpklocal+t)∥2 T^*_{local\rightarrow leader} = \arg\min_{s,R,t} \sum_{k \in \mathcal{M}} \left\| \mathbf{p}^{leader}_k - (sR\mathbf{p}^{local}_k+t) \right\|^2Tlocal→leader∗=args,R,tmink∈M∑pkleader−(sRpklocal+t)2
其中M\mathcal{M}M是通过 UUID 找到的共同 MapPoint 集合。RANSAC 用来剔除误匹配，Kabsch-Umeyama 用来求最小 RMSE 的 Sim(3)。
4. Sim(3) 与 UUID 的关系：为什么“同一个点”必须能被精确指认？
Sim(3) 的估计需要共同关键帧或共同地图点。如果 robot A 和 robot B 都有一个本地mnId = 5，这并不能说明它们是同一个物理对象，因为mnId只在本机内部有意义。
因此 DVM-SLAM 的通信与序列化地图中必须携带 UUID：
| 对象 | 本机编号 | 跨机身份 |
|---|---|---|
| KeyFrame | mnId | uuid |
| MapPoint | 本地对象指针 / id | uuid |
UUID 在 Sim(3) 相关流程中的作用：
- BoW 去重：对方发来的关键帧 BoW 是否已经处理过；
- merge 对齐：
merge_candidate_key_frame_uuids指明候选公共关键帧； - 地图重连：serialized map 中的 KF / MP 按 UUID 在本地数据库中查找并重新连接；
- Map Alignment Refiner：按 UUID 找到双方共同观测到的 MapPoint，再求Tlocal→leaderT_{local\rightarrow leader}Tlocal→leader。
可以把它理解为：
Sim(3) 是“怎么对齐”，UUID 是“对齐谁和谁”。
5. Sim(3) 与去中心化 PGO 的关系
DVM-SLAM 的去中心化 PGO 不是传统意义上的全网分布式矩阵求解，而是通过增量、异步的关键帧和地图点共享完成的。
合并后，同一 group 内 agent 已经处在共同坐标系中，因此外部 keyframe 和 mappoint 可以直接移动到本地 map，无需再次做坐标变换。随后本地执行：
- 插入外部关键帧kextk_{ext}kext；
- 重连共视关键帧和观测地图点；
- 合并重复 MapPoint；
- 围绕kextk_{ext}kext执行局部 PGO。
所以二者关系是：
| 模块 | 作用 | 是否直接做 PGO |
|---|---|---|
| BoW / Visual Word Set | 找 merge 候选 | 否 |
| Sim(3) Map Merge | 建立共同坐标系 | 否，主要是坐标系对齐与地图焊接 |
| Map Alignment Refiner | 周期性修正尺度/旋转/平移误差 | 否，主要是几何校准 |
| External Keyframe Inserter + Local PGO | 插入外部 KF/MP 后优化本地共享图 | 是 |
一句话：
Sim(3) 先把地图放到同一个坐标系，PGO 再在这个共同坐标系内优化关键帧与地图点关系。
6. 和通信流程的对应关系
可以按三阶段解释 Sim(3)：
| 阶段 | 主要通信 | Sim(3) 角色 |
|---|---|---|
| Merge 前 | new_key_frame_bows | 暂无 Sim(3)，只找候选 |
| Merge 中 | get_current_map/map_to_attempt_merge | 通过完整地图尝试求Tj→iT_{j\rightarrow i}Tj→i |
| Merge 后 | change_coordinate_frame/successfully_merged | 传播 Sim(3)，让组内 agent 进入同一 reference frame |
| 协作期 | new_key_frames/get_map_points | 同坐标系增量同步；必要时 refinement 重新估计 Sim(3) |
7.总结
DVM-SLAM 是纯单目多机器人 SLAM，因此每个机器人独立建图时不仅坐标原点和朝向不同，地图尺度也可能不同。普通 SE(3) 只能处理旋转和平移，无法解决尺度不一致，所以 DVM-SLAM 在跨机器人地图合并时必须使用 Sim(3)，也就是同时估计尺度、旋转和平移。
在系统流程中，Sim(3) 第一次出现在 map merging 阶段。机器人先通过 BoW 和 UUID 发现潜在重叠区域，候选成立后才交换完整地图并尝试几何合并。如果合并成功，系统会估计从高 ID 或待合并 agent 坐标系到 leader 坐标系的 Sim(3) 变换，并通过 ChangeCoordinateFrame 等消息传播给组内其他机器人，使整个 group 进入同一坐标系。
但是 DVM-SLAM 采用早期合并策略，初始重叠区域可能较小，因此 Sim(3) 会存在轻微误差。随着地图扩展，这种尺度、旋转和平移误差可能被放大。为此系统又设计了 Map Alignment Refiner：通过 UUID 找到双方共同 MapPoint，用 RANSAC 剔除错误匹配，再用 Kabsch-Umeyama 算法重新估计 Sim(3)，周期性把本地地图重新对齐到 leader 的 reference frame。
因此，Sim(3) 可以理解为 DVM-SLAM 去中心化协同建图中的“坐标系胶水”：它不等同于 PGO，但它为后续外部关键帧插入、地图点重连和局部 PGO 提供了统一的几何基础。
8. 记忆版总结
单目多机为什么要 Sim(3)？ 因为不同 agent 的地图尺度不一致。 Sim(3) 在哪里用？ 1) 初次 map merge：估计 T_j→i，把 peer map 拉到 leader map。 2) map alignment refiner：用共同 MapPoint 周期性重估 T_local→leader。 UUID 为什么重要？ UUID 决定“哪个 KF/MP 是同一个物理对象”；Sim(3) 决定“如何把它们对齐”。 和 PGO 什么关系？ Sim(3) 负责统一坐标系，PGO 负责在统一坐标系内优化图。