098｜ORB-SLAM3：跟踪丢了以后，地图还能继续生长吗

总览 · 目录 · 上一篇 · 下一篇

论文：Carlos Campos 等，ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM，2020 年预印本，后发表于 IEEE T-RO。原文 · 官方代码。

先接住前作的问题

ORB-SLAM2已经能回环和复用地图，但长时间丢失仍会断开运行；ORB-SLAM3 直接扩展这一系统，加入视觉惯性初始化与 Atlas 地图管理。它与VINS-Mono是完整视觉惯性系统的平行选择。

五个前置概念

五个前置概念：重投影误差检查三维点是否投到观测位置；BA联合调整位姿和地图点；预积分压缩区间惯性约束；Sim(3)在刚体变换外允许尺度变化；场所识别提出历史地点候选，仍需要几何验证。

一、问题设定：跟踪断裂后怎样保留地图片段并在以后重接

相机被手挡住，或者机器人突然转向，视觉 SLAM 就可能跟不上原来的特征。一个脆弱的系统会因此丢掉整段运行；另一个危险做法是继续输出貌似平滑但其实已经失真的轨迹。

背景：地图不只是最近两帧之间的位移

只比较相邻图像，误差会随时间积累。保留三维地图点，可以让当前帧与更早的帧共同约束同一个位置；识别重访还能把相隔很久的片段连接起来。数据关联因而有不同时间跨度，而不只是“特征检测是否够快”。

输入可以是相机图像，或相机加 IMU；不同传感器配置对应不同可观测量。单目几何一般没有绝对尺度：把所有点和相机平移一起放大，图像投影仍可不变。双目的已知基线或具有充分运动激励的惯性观测，可提供尺度信息。

本任务没有策略、奖励或试错训练。变量是相机/机体位姿、地图点，以及惯性配置中的速度、偏置和重力相关量。本文讨论的是稀疏定位建图；它不自动生成可抓取物体语义或高质量稠密网格。

二、联合惯性初始化与Atlas数据关联

ORB-SLAM3 的重要思想是把失踪处理纳入系统：视觉与 IMU 联合估计，暂时无法连回旧地图时可以建立新地图，以后找到可靠联系再合并。它还改进了惯性初始化和场所识别，使历史观测更早、更有效地参与估计。

视觉误差是什么

设世界点为 $P_j$，$T_i$ 是机体到世界的变换，$T_{CB}$ 是机体到相机的已知外参。观察像素为 $u_{ij}$，则简化重投影残差为：

$$r_{ij}=u_{ij}-\pi(T_{CB}T_i^{-1}P_j).$$

$\pi$ 是相机投影模型。它表达的不是“两个描述子相差多少”，而是同一个三维点在当前位姿下应该落在哪个像素。描述子用于提出对应，几何误差用于检查和优化，两者不能混为一个步骤。

如果相机内参、畸变或外参错误，即使点匹配正确，预测像素也可能系统偏移。增加优化迭代不能保证消除这种模型错误。

把惯性约束接进来

可以用通用教学目标表示联合估计：

$$\min_{X,P}\sum_{i,j}\rho\!\left(\|r_{ij}\|^2_{\Sigma_{ij}^{-1}}\right) +\sum_i\|r^{imu}_{i,i+1}\|^2_{\Sigma_{imu}^{-1}}.$$

$X$ 包含运动状态，$P$ 是地图点，$\rho$ 降低错误视觉匹配的影响。惯性残差要求相邻位姿、速度与测得的运动相容。两类约束互相补充：图像能校正漂移，IMU 能提供短时运动预测。

这不意味着每帧求解全部历史变量。跟踪、局部建图和全局校正有不同优化范围，后台耗时也不同。一次局部 BA 运行超过相机帧间隔，并不自动意味着跟踪线程也以相同频率阻塞；判断实时性要看线程依赖、积压和输出延迟。

初始化不是随便填几个零

视觉惯性初始化需要建立尺度、速度、重力方向和偏置之间的合理关系。论文 §V-B 将初始化组织成视觉估计、惯性变量估计、联合细化。直觉上，先利用几何得到形状和运动，再让惯性观测解释其物理尺度，比从完全不可信的初值同时调整所有变量更有结构。

但它仍受可观测性约束。想象相机几乎不动：视觉缺乏视差，惯性也缺少区分尺度和偏置的变化。算法知道重力存在，并不意味着从任意短暂静止数据都能恢复完整运动状态。原文也讨论了弱激励下继续进行尺度细化的需要。

Atlas：先保留片段，再证明它们属于同一世界

用自构例子理解：机器人在 A 房间建图，经过一段完全黑暗的走廊后进入 B 房间。此时没有足够观测证明 B 相对 A 的位置，可以先建立独立片段。稍后从另一条明亮路径回到 A，若得到可靠的共同几何，就能估计片段变换并融合。

合并地图至少涉及三个不同动作：提出候选地点、验证几何变换、消除重复点并传播校正。只因两张图像看起来像，就强行合并，会把整张地图拉坏。另一方面，没有重叠或可辨认的历史观测时，保留两个片段是诚实的状态，不是算法忘了按某个按钮。

地图对齐还必须作用于同一坐标系中的所有状态。设 $S_{AB}:p_B\mapsto sRp_B+t$ 把地图 B 的坐标变到地图 A；$s>0$ 是尺度、$R$ 为旋转、$t$ 为平移。对于尚未定尺度、位置与速度使用同一图单位且时间单位不变的状态，正确变换为

$$p_A=sRp_B+t,\qquad v_A=sRv_B,\qquad R_{AI}=RR_{BI}.$$

$I$ 表示机体/IMU 系，$R_{BI}$ 把该系向量表达在地图 B 中。平移不会加到速度上，姿态也不能乘上尺度；IMU 偏置若表达在传感器自身坐标系，则不会因世界图坐标改变而旋转。这些是坐标变换规律，并不意味着任意成熟惯性地图都允许自由缩放。

原创手算。 取 $s=2$、$R$ 为绕竖直轴转 $90^\circ$、$t=(10,0,0)$；地图 B 的点 $(1,0,0)$ 变为 $(10,2,0)$，速度 $(1,0,0)$ 变为 $(0,2,0)$。若只更新点而保留速度 $(1,0,0)$，下一次惯性位置预测便与已经转过的地图不相容。

原文V-B与VI-C区分未成熟地图的 Sim(3) 对齐与成熟惯性地图的 SE(3) 对齐。后者已有公制尺度，理想重力对齐后剩余世界旋转自由度主要是偏航；实际还要检查横滚、俯仰的一致性，并优化惯性连接。多次地图实验因此验证的不只是“认出旧图像”，还包括融合后状态和几何能否共同解释观测。

此伪代码概括机制。实际线程、删除条件与优化窗口应以固定版本代码为准。输出状态必须与位姿一起看，不能把初始化中、暂时丢失和正常跟踪都当成同等可靠的数据。

地图断开时保留新图，重新合并时必须再次建立跨图关联。下面检查坐标与尺度一致性，再读恢复实验的对齐口径。

三、管理新图与合并，检查尺度和参考系

输入：图像；可选 IMU；相机与外参标定
在当前活动地图中提取并匹配视觉特征
根据已有运动与 IMU 预测当前状态
优化当前跟踪状态，判断有效对应是否足够
若短时视觉丢失：利用惯性预测尝试恢复匹配
若长期无法恢复：按条件初始化一个新的活动地图
后台维护关键帧、地图点并做局部优化
查询历史地图中的场所候选，并进行几何一致性验证
若候选在同图：执行回环校正
若候选在另一图：对齐、融合和联合细化地图
输出当前估计、地图关联关系及跟踪状态

四、按传感器与对齐方式读恢复和精度实验

读图：不同颜色来自多次运行，融合后在同一参考系中叠加。历史地图增加了先验信息，因而这幅多会话结果不能与从零开始的单次运行当成相同输入比较。

实验设计：先看对齐方法，再看小数点

多地图合并必须有共同观测建立相对变换；尺度对齐后的低 ATE 不能证明单目已经自行恢复了公制尺度。

三组关键证据各回答不同问题：

1. 表 II 与图 4：EuRoC 的跨运行误差与失败。双目惯性平均 ATE 为 0.035 米；表中部分其他配置未完成所有序列，带星号的成功子集均值不能当作同一测试集合的排名。

读图：每格是一轮运行结果，颜色对应共同误差色标；同时观察重复波动与序列差异。

2. 表 IV：TUM-VI room 序列中，纯单目平均 0.039 米、双目 0.068 米、双目惯性 0.009 米。单目看似优于双目，部分来自其允许额外尺度对齐，不能据此得出少一个相机通常更准。
3. 表 V 与图 5–6：历史地图复用。它支持多次观察带来的收益，但历史地图就是额外信息，不宜与从零开始的单次运行混作公平输入对比。

读图：红、蓝轨迹对应不同会话处理方式；背景图用来定位，而定量精度仍须结合正文表格。

TUM-VI 很多长序列只有起终区域真值，room 序列才有完整覆盖。作者展示的室外轨迹改善和地图一致性有参考价值，但缺少中段真值的位置不能被写成已经逐米验证厘米精度。

自构实验与前置知识

未运行实验：选同一段有重访的图像序列，在中途人工遮挡连续若干帧，比较禁用多地图、启用多地图和加入 IMU 三种情况。固定特征预算与标定，记录丢失时间、新建地图数、成功合并数、最终可追踪比例和 ATE。然后删除重访部分，检查系统是否仍保留独立地图，而不是凭空恢复联系。

五、保留标定、激励与可靠跨图关联要求

作者把可靠估计所需的多个环节整合为完整系统：初始化、不同时间跨度的数据关联、丢失处理、合并和局部优化。它并不是换一个损失函数就获得全部收益。阅读消融或配置对比时，应问具体改变了传感器、输入历史、数据关联还是优化范围。

下面是方法层面的分析。遮挡期间若 IMU 偏置未估好，短时预测也会变差；长时间黑暗没有可用几何，创建新图不能恢复那段缺失轨迹；重复走廊可能提出错误场所候选；地图无限累积还会影响存储和优化成本。

因此，平均误差低和工程恢复能力强是不同维度。一个方法可能只在容易片段上误差很低，却经常丢失；另一个方法误差略大但覆盖更完整。部署前至少同时看成功覆盖率、重定位时间、误差和尾部延迟。

六、追问：两张地图各自残差很小，为什么不能仅据此允许合并？

七、比较多地图系统与雷达实时估计

• 平行路线：VINS-Mono：让单目视觉与 IMU 组成可以长期运行的定位系统。比较不同视觉惯性系统的初始化、恢复和长期管理。
• 教学联系：FAST-LIO2：直接用点云定位，为什么先要解决地图更新。比较关键帧优化与雷达迭代滤波的实时取舍。

八、自测与参考答案

1．为什么加入 IMU 后，单目尺度仍可能初始化不好？

答：IMU 提供的是受偏置和噪声影响的运动观测。若运动缺乏足够激励，尺度、速度、重力和偏置之间仍难区分。传感器的存在不等于数据已经包含所需信息。

2．新建地图与重定位有什么区别？

答：重定位是在已有地图中恢复当前位姿；新建地图承认暂时无法建立这种联系，先维护独立片段。只有后来获得可靠共同观测，才能确定地图间变换并合并，不能直接把两个坐标原点当成同一个位置。

3．一个允许尺度对齐的单目结果比双目 ATE 更低，应该怎样解释？

答：它说明在额外校正全局尺度以后，轨迹形状误差可能更小；不说明系统自己恢复了更准的米制位置。公平解释要保留对齐自由度，并另报尺度误差、失败序列和完整轨迹覆盖。