076｜LSD-SLAM：让单目直接法拥有能闭环的全局地图

论文：Jakob Engel、Thomas Schöps、Daniel Cremers，LSD-SLAM: Large-Scale Direct Monocular SLAM，ECCV 2014，pp. 834–849。

原始论文 · 作者项目页 · 官方代码

总览 · 上一篇：接触隐式轨迹优化 · 下一篇：LOAM

补充来源：论文镜像。

先接住前作的问题

LK说明亮度梯度能引导局部配准，但长期单目地图还会发生尺度漂移。LSD-SLAM把半稠密逆深度与Sim(3)关键帧图组合。

五个前置概念

理解本篇最需要的五个概念：

• 逆深度：用距离的倒数表达深度，有利于处理远处点与深度不确定性。
• 光度残差：通过几何投影后，对应像素亮度之间的差。
• SE(3)：三维旋转和平移组成的刚体变换空间。
• Sim(3)：在刚体变换上加入统一缩放，用于协调局部尺度。
• 位姿图：节点表示相机关键帧，边表示相对几何约束，通过联合优化减少不一致。

一、问题设定：直接单目跟踪怎样修正长期尺度漂移

假设拿着普通摄像头从桌面走到走廊，再回到桌旁。摄像头能比较前后图像，推断自己移动了多少，却不知道世界里的“一单位”究竟是一厘米还是一米。连续估计还可能让这个单位慢慢变化，于是重返起点时，同一张桌子在地图里出现两个大小不同的副本。

背景：直接法缺少的不是更多像素，而是全局约束

特征法先找角点、描述与匹配，再优化几何；直接法直接比较经过几何变换后对应位置的亮度。后一种思路能够利用边缘附近的信息，但使用更多像素不等于拥有闭环能力。只根据相邻图像不断积累运动，仍然会发生漂移。

需要明确三个不同问题：

• 绝对尺度模糊：把场景与平移一起放大，相机图像可以保持不变。这是传感器信息不足。
• 尺度漂移：不同时刻的局部重建使用了不一致的尺度。这是长期估计误差。
• 场景尺度变化：镜头从很近的物体转向很远的建筑。这是观察条件变化。

其中第二个问题可以通过重新观察同一场景增加约束来缓解；第一个不能仅靠算法名称中的“scale-aware”消除。

论文的状态是相机位姿、关键帧深度及其不确定性；观测是标定后的图像。场景通常被当作静态，亮度对应近似成立，跟踪需要合理初值。系统输出相机轨迹与由关键帧深度组成的半稠密地图，而不是一个能直接决定机器人下一步动作的控制器。

“半稠密”也不是随意抽掉一半像素。它保留足够大亮度梯度附近的区域，因为均匀白墙上的一个像素移到旁边后几乎一样亮，难以提供位置约束。梯度丰富的轮廓通常比平坦区域更有用，但一条直边仍可能只约束垂直于边的运动。

二、连接半稠密深度、直接对齐与Sim(3)图

1. 每个关键帧保存一幅带置信度的局部几何

按照§3.2，关键帧可记为

$$K_i=(I_i,D_i,V_i).$$

$I_i$为图像，$D_i(p)=1/z_i(p)$为像素$p$的逆深度，$V_i(p)$为逆深度方差。这里的方差属于几何估计的一部分，不只是画图时决定颜色的附件。

新图像到来时，先用当前关键帧估计位姿。没有成为新关键帧的图像仍有价值：它们提供小基线立体比较，逐步细化深度。相机离当前关键帧足够远后，才把已有深度投影到新关键帧，继续局部估计。

理解这个流程的一个关键是：普通帧不必永久保存全部数据才能发挥作用。它可以把信息融合进关键帧的深度估计，随后不再承担全局图节点的职责。这降低了全局问题规模，同时也意味着后端不会重新联合优化每一张原始图像。

2. 普通帧追踪在SE(3)上进行

设$\omega(p,D_i(p),T)$表示把参考像素反投影到三维，经$T$变换，再投影到新图像。光度残差是

$$r_p=I_i(p)-I_j\!\left(\omega(p,D_i(p),T)\right).$$

这个式子中的未知量是相机运动，而不是两张图像之间逐像素独立的光流。所有像素共享同一个刚体运动，因此几何约束把它们联系起来。

论文§3.3、式(12)–(14)进一步用残差方差归一化：

$$\sigma_{r_p}^{2} =2\sigma_I^2+ \left(\frac{\partial r_p}{\partial D_i(p)}\right)^2V_i(p).$$

$\sigma_I^2$是图像强度噪声方差。第一项来自两张图像；第二项表示深度误差经过投影和亮度变化传播后，会怎样影响残差。优化还结合Huber鲁棒处理，降低异常观测的影响。

这比“深度不准就统一少信一点”更细。若相机只是纯旋转，投影方向不依赖场景深度，深度不确定性对这次残差的影响可以为零；若发生平移，影响取决于运动方向和局部亮度梯度。相同深度方差并不对应相同光度残差方差。

这条方差传播式可以直接手算。原创教学相机取焦距 $f=100$ 像素，参考点到目标相机坐标的相对变换只含水平平移 $t_x=0.1$ 个地图长度单位；在简化针孔几何中，像素位移为 $ft_xD$，故对逆深度 $D$ 的导数为10。若局部亮度梯度为2，残差对逆深度导数幅值就是20。取逆深度方差 $V=0.01$、每张图像强度噪声方差1，残差总方差为 $2+20^2\times0.01=6$；同样残差在纯旋转、其余条件相同的假设下只含强度项2。归一化后，前一种观测的信息权重只有后一种的三分之一。

这里没有新做一次真实实验，而是把式（14）的每个因素展开。它还说明“选高梯度像素”与“梯度越大越可信”不是同一个结论：高梯度增加位姿敏感性，也会放大深度投影误差。比较观测必须同时检查雅可比和残差方差，不能只把高梯度全部赋予相同权重。

原稿§3.1还说明随机深度、高方差初始化需要起始阶段足够平移，并将更完整的收敛评价留待后续。纯旋转可以对齐图像，却没有凭空提供三角化基线；因此演示视频中的成功初始化不能解读为任意运动都能恢复深度。

3. 两个有深度的关键帧才能讨论相对尺度

普通新图像还没有自身深度坐标系，因此其跟踪是SE(3)问题。两个关键帧则各自拥有深度，可以估计

$$S= \begin{bmatrix}sR&t\\0&1\end{bmatrix} \in\operatorname{Sim}(3),$$

其中$R$为旋转、$t$为平移、$s>0$为尺度。相较SE(3)，多了一个尺度自由度。

但是增加未知量并不自动增加信息。§3.5、式(17)–(19)同时使用光度残差和逆深度残差：前者要求图像对齐，后者要求变换后的深度与另一关键帧已有深度相容。两类残差分别按不确定性归一化，再共同接受鲁棒处理。只有光度项不足以约束这一相对尺度参数。

论文将新关键帧的平均逆深度归一化为一，并把相应缩放保存在Sim(3)位姿中。这是在管理坐标单位，不能解释成把实际世界中的物体都变成同样大小。

4. 约束进入图之前，还要检查是否可信

候选关键帧来自附近节点及外观检索。对候选配对进行双向跟踪，检查两个估计组合后是否接近单位变换；检查使用协方差和相应坐标变换，而非只比较两次平移的欧氏距离。接受的约束进入后台位姿图优化。

这使“direct”一词需要准确理解：核心几何估计直接优化图像信息，不代表完整系统绝不使用外观检索工具。外观相似只是提名，几何一致性才决定约束能否使用。

下面是根据§3.1–3.6整理的教学伪代码，省略线程同步和数值实现：

输入：标定图像序列；初始关键帧深度及其方差
建立当前关键帧 K 与关键帧图 G
对每张新图像 I：
    用上一帧位姿初始化，以SE(3)直接对齐 I 与 K
    用深度方差传播得到光度残差权重，迭代求解位姿
    如果尚不需要新关键帧：
        在可靠区域做小基线立体比较
        融合逆深度与方差，执行空间正则化
    否则：
        把已有深度传播到新关键帧，归一化局部尺度
        将完成的关键帧加入全局图
        检索可能重叠的关键帧
        用光度和深度残差估计双向Sim(3)变换
        通过一致性检查的约束加入图
后台：优化关键帧Sim(3)位姿，更新全局地图
输出：连续相机位姿与可闭环的半稠密重建

数学目标是降低观测残差与图约束不一致；算法改变是深度不确定性进入跟踪、尺度进入关键帧对齐；实验指标则是轨迹误差与收敛范围。三者相关，却不是同一件事。

Sim(3)约束能修正关键帧之间的相对尺度，不能产生外部米制信息。下面沿深度更新与回环流程检查，再分开读取轨迹对齐与地图一致性。

三、更新关键帧和尺度约束，用回访例子检查

设两个关键帧观察同一张桌子：第一帧的局部地图把桌宽记为1，第二帧因局部单位不同记为2。这个数字差异本身不意味着桌子变形。估计适当的相似变换后，可以把两份局部几何放到一致坐标系里。但若不知道真实桌宽，最终仍不能断言它是1米还是0.8米。

再设两个像素的光度残差都是10，其中一个预测标准差为2，另一个为10。归一化残差分别为5和1。前者的观测偏差相对于预期噪声更异常，应更容易受到鲁棒核抑制。这是自构算例，尚未运行；它说明的是统计标准化，不是“噪声越大就越重要”。

四、分开读长轨迹、误差与收敛范围实验

读图：表内是按序列比较的厘米级轨迹 RMSE。fr2/xyz 上某 RGB-D 对照更低，fr2/desk 上本方法又较强；不同传感器与初始化信息不应被压成“所有序列冠军”。

实验设计：不同实验回答不同问题

Sim(3) 关键帧对齐需要双方深度信息，普通帧跟踪仍是 SE(3)。轨迹误差表与半稠密地图覆盖不是同一个评价对象。

读图：跟踪向深度估计提供相机位姿，关键帧再进入地图优化；三列共同说明实时系统并非只有一个直接配准器。

这些实验没有强化学习中的环境交互预算。对应的公平性问题是：输入传感器是否一样、初始化是否一样、图像分辨率和硬件是否一样、轨迹对齐是否一样。本文未逐项确认处理器型号、每个模块的耗时分布、重复运行次数与置信区间。

特别要保留第一帧深度这一条件。将表中结果概括为“LSD-SLAM从头到尾只见RGB，战胜所有RGB-D方法”是不成立的。相反，拥有持续深度观测的方法与只在开头使用深度的方法也不应被当作信息量完全相同的竞争者。

轨迹误差小不直接证明每个三维点都准确；点云看起来完整也不直接证明定位误差小。对机器人而言，还需要关心掉跟踪时长、重定位成功率、延迟峰值以及地图是否足以支撑下游任务。

证据：五处最值得对照原文的地方

图 2。 放宽深度方差阈值，点云更密也更噪；地图密度与可信度存在取舍；不能据此推出：点越多越适合避障。

读图：从左到右点云更密，却出现更多离散噪声点；增大保留阈值不能等同于几何更准确。

图 4、式(14)。 不同运动下的残差方差分布不同；权重应结合深度误差如何投影到图像；不能据此推出：方差估计已在所有环境中校准。

读图：同一深度误差在旋转与平移下投影不同，所以下方权重图随运动改变；不能用统一常数替代这种几何关系。

图 7、§4.1。 长轨迹闭环前后重建对照；系统可在展示轨迹上纠正累计不一致；不能据此推出：所有500米路线都能成功。

读图：左右两幅全局地图显示闭环前后的一致性变化，红框对应的局部放大和下方深度图保留了重建细节。

图 9、§4.2。 fr2/desk轨迹RMSE为4.52厘米；比较的半稠密单目VO为13.50厘米；在该实验中全局SLAM结果优于所列VO结果；不能据此推出：所有差异都来自某一个新组件。

图 10、§4.3。 ESM与更深金字塔扩大成功初始化范围；成功收敛时精度变化不明显；收敛范围和最终精度是不同维度；不能据此推出：非凸问题已具有全局收敛保证。

读图：下方彩条记录哪些初始化能成功，上方曲线记录成功后的误差；扩大收敛范围与提高最终精度是两个不同指标。

图 9虽然在正文附近被称作表，图注编号实际为Fig. 9。其fr2/xyz行给出LSD-SLAM为1.47厘米，所列直接RGB-D方法为1.18厘米；因此论文不能概括为每个序列都优于深度传感器方法。不同传感器与初始化条件也使“总冠军”叙事失去意义。

从4.52和13.50自行计算，误差绝对减少8.98厘米，相对减少约66.5%，即$(13.50-4.52)/13.50$。这只是该行的比较，不能当成平均提升。模拟结果与真实序列还受成像模型、模糊和滚动快门等因素影响，不宜混合平均后宣称统一收益。

图 10很适合训练阅读实验的习惯：如果更多初始化能收敛，但成功后的误差几乎相同，正确结论是“更容易找到原有的好解”，而不是“好解本身更精确”。

五、保留单目尺度、光度假设与初始化边界

论文最有价值的组合是，把概率半稠密地图、直接几何估计和尺度感知图优化放进同一个在线系统。它沿用了已有的小基线深度过滤与图优化思想；不能把所有组件都算成新发明。

作者在§3.1明确将随机深度初始化的系统性评估留待后续工作；§3.5承认直接对齐的非凸性和初值要求。§2.2还提醒残差存在相关性，因此由近似Hessian得到的协方差不能被无条件视作精确统计保证。这些限定会影响我们如何理解双向一致性阈值。

以下是机制分析，而非论文额外实测结果：亮度突变、强反射、动态物体会破坏光度对应；长期纯旋转可能支持姿态估计，却不能提供新的深度三角化信息；错误但自洽的闭环也可能污染全局图。鲁棒核能减弱异常残差，不能创造缺失的观测。

半稠密地图还不等于封闭表面或占据地图。看不到的区域应视为未知，不能因为没有点就解释为空闲空间。因此把LSD-SLAM接到机械臂避障或移动机器人规划时，仍需明确地图表示转换和保守性规则。

六、追问：回环把地图尺度拉一致后，为什么仍不能宣称恢复了米制尺度？

回环把地图尺度拉一致后，为什么仍不能宣称恢复了米制尺度？

参考答案：相对尺度一致性约束不同于来自外部的绝对长度信息。单目系统可以修正各段地图之间的尺度漂移，同时仍保留整体尺度自由度。应在评测中说明对齐允许哪些变换，不能把对齐后的误差直接读成独立恢复了真实米制长度。

七、转向稀疏联合光度优化

• 平行路线：DSO：用少量像素联合优化，而不是用尽所有像素。以稀疏联合光度优化处理视觉里程计，目标与后端不相同。

八、自测与参考答案

问题1：既然Sim(3)更灵活，为什么所有新图像都不直接估计七自由度？

答案：新图像只有亮度，没有独立的深度坐标系。仅靠投影亮度无法给新增尺度自由度提供足够约束，盲目加入参数只会引入不可辨识性。普通帧相对于已有深度图估计SE(3)；两个拥有深度图的关键帧才能用深度一致性估计相对尺度。判断模型自由度时，要先问观测究竟约束了什么。

问题2：深度方差很大，是否必然意味着该像素不能用于旋转估计？

答案：不必然。纯旋转时三维点的投影方向与深度无关，因此深度不确定性不一定传播成光度残差不确定性。像素是否有用还取决于亮度梯度、遮挡与噪声。这正是通过雅可比传播方差优于只看深度方差阈值的原因；但纯旋转仍无法单独补足新的深度信息。

问题3：闭环后两份桌子重合，而且ATE降低，是否证明地图已具备可靠避障能力？

答案：不能。重合说明某些全局约束更一致，ATE评价的是相机轨迹。半稠密地图可能仍缺少无纹理障碍、细杆或未观察区域，也可能有局部几何误差。要评价避障，需要另测障碍覆盖、错误空闲判定、碰撞率及延迟，并保留未知区域。定位成功、地图视觉完整和下游任务安全是三个需要分别检验的命题。