论文阅读《LSD-SLAM: Large-Scale Direct Monocular SLAM》

摘要

我们提出了一种直接(无特征)单目SLAM算法，与目前最先进的直接方法相比，该算法允许构建大尺度且一致的环境地图。采用基于直接图像对齐的高精度位姿估计方法，同时利用关键帧的位姿图和相应的半稠密深度图，实时重建三维环境。这是通过小基线双目相机滤波得到的。对尺度漂移的解析表示允许该方法用于挑战性的序列，包括那些场景中尺度发生较大变化的序列。本文有两个创新点：(1)在$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$上运行的一种新的直接跟踪方法，从而明确地检测尺度漂移；(2)一个优雅的概率解决方案，将带噪声的深度值纳入跟踪。由此产生的直接单目SLAM系统在CPU上实时运行。

1 介绍

实时单目同时定位和建图(SLAM)和三维重建已经成为越来越受欢迎的研究课题。两个主要原因是：(1)它们在机器人领域中的应用，特别是在无人机(UAV)导航方面的应用；(2)增强现实和虚拟现实应用正在慢慢进入大众市场。

单目SLAM的主要好处之一，也是最大的挑战之一，是其固有的尺度模糊。真实世界的尺度无法被观测到，并且会随着时间的推移而漂移，这是主要的误差来源之一。它的优点是可以在不同规模的环境之间无缝切换，例如室内的办公桌环境和大规模的室外环境。另一方面，具备尺度的传感器，如深度相机或双目相机，能够提供可靠测量的范围有限，因此不能提供这种灵活性。

1.1 相关工作

A 基于特征的方法

基于特征的方法(包括基于滤波的和基于关键帧的)的基本思想是将整个问题，即从图像中估计几何信息，分解为两个连续的步骤。首先，从图像中提取一组特征观测。其次，相机的位置和场景的几何作为这些特征观测的函数被计算。

虽然这种解耦简化了整个问题，但它也有一个重要的限制。只有符合特征类型的信息才能被使用。特别是，当使用关键点时，包含直线边缘或曲线边缘的信息，尤其是在人造环境中构成图像的很大一部分，将会被丢弃。过去已经有几种方法通过包括基于边缘甚至基于区域的特征来弥补这一缺陷。然而，由于高维特征空间的估计繁琐，在实际应用中很少使用。为了获得稠密重建，使用多视图几何利用估计的相机位姿连续地重建出稠密地图。

B 直接方法

直接视觉里程计(VO)方法绕过这一限制，直接优化图像的灰度来获得几何，该方法可以使用图像中的所有信息。除了更高的准确性和鲁棒性，特别是在关键点很少的环境中，该方法可以提供更多关于环境几何的信息，这对机器人或增强现实应用非常有价值。

虽然RGB-D相机或双目传感器的直接图像对齐算法已经被很好地确定，但直到最近才提出单目直接VO算法。在文献[20,21,24]中，精确的和完全稠密的深度图使用变分公式计算，但这该方法的计算量很大，需要最先进的GPU实时运行。在文献[9]中，提出了一种半稠密深度滤波公式，大大降低了计算复杂度，这种方法允许在CPU甚至是现代智能手机上实时运行。通过将直接跟踪与关键点相结合，文献[10]在嵌入式平台上实现了高帧率实时运行。然而，所有这些方法都是纯的视觉里程计，它们只在局部跟踪相机的运动，并不能建立一个一致的、全局的和带有回环的环境地图。

C 位姿图优化

这是一种著名的SLAM技术，用于构建一致的全局地图。世界被表示为由位姿约束连接的若干关键帧，可以使用通用的图优化框架(如g2o)进行优化。

在文献[14]中，提出了一种基于位姿图的RGB-D SLAM方法，该方法引入了几何误差，允许在纹理较少的场景中进行跟踪。为了解决单目SLAM中产生的尺度漂移问题，文献[23]提出了一种基于关键点的单目SLAM系统，该系统将相机位姿表示为3D相似变换，而不是刚体运动。

1.2 贡献和大纲

我们提出了一种大规模直接单目SLAM(LSD-SLAM)方法，该方法不仅可以局部跟踪相机的运动，还可以建立一致的大规模的环境地图(见图1和图2)。该方法使用直接图像对齐，并结合文献[9]中首次提出的基于滤波的半稠密深度图估计。全局地图用位姿图的形式表示，关键帧作为顶点，3D相似变换作为边，优雅地融入环境的尺度比例，并允许检测和修正累积漂移。该方法在CPU上实时运行，甚至在现代智能手机上作为里程计运行。本文的主要贡献如下。(1)一种用于大规模直接的单目SLAM的框架，特别是一种新的尺度感知图像对齐算法，可以直接估计两个关键帧之间的相似性变换$\xi \in \mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$。(2)概率一致地将估计深度的不确定性纳入跟踪。

图1 LSD-SLAM：LSD-SLAM生成一致的全局地图，使用直接图像对齐和概率半稠密深度地图来代替关键点。顶部：实时生成的中等轨迹的所有关键帧的累积点云(来自手持单目相机)。底部：带有彩色编码的半稠密逆深度图的关键帧。参见补充视频。

 

图2 除了精确和半稠密的三维重建，LSD-SLAM还可以估计关联的不确定性。从左到右：以不同的最大方差限制累积点云。请注意重建明显变得更密集，但同时也包含更多的噪声。

 

2 初步准备(preliminaries)

在本章中，我们对相关的数学概念和符号作了简要的总结。特别地，我们将位姿用李代数表示(第2.1节)，推导出李流上直接图像对齐的加权最小二乘(第2.2节)，并简要介绍不确定性的传播(第2.3节)。

符号。我们用粗体大写字母($R\text{R}R$)表示矩阵，用粗体小写字母表示向量($\xi \text{ξ}\xi$)。矩阵的第$n$行记作$[\cdot {]}_n$。图像记作$I:\Omega \to \mathbb{R}$，其中$\Omega \subset {\mathbb{R}}^2$为归一化的像素坐标，$\mathbb{R}$表示一维实数。像素级逆深度图记作$D:\Omega \to {\mathbb{R}}^{+}$。像素级逆深度方差图记作$V:\Omega \to {\mathbb{R}}^{+}$。在整篇文章中，我们使用$d$来表示路标点深度$z$的倒数，即$d=z^{-1}$。

2.1 3D刚体变换和相似变换

3D刚体变换。三维刚体变换$G\text{G}G\in \mathrm{S}\mathrm{E}(3)$表示三维的旋转和平移，记作
$$G\text{G}G=\left(\begin{array}{cc}R\text{R}R& t\text{t}t\\ 0\text{0}0& 1\end{array}\right)R\text{R}R\in \mathrm{S}\mathrm{O}(3),t\text{t}t\in {\mathbb{R}}^3\text{\hspace{1em}(1)}$$
在优化过程中，需要一个相机位姿的最小表示，它由相关李代数的对应元素$\xi \text{ξ}\xi \in \mathfrak{s}\mathfrak{e}(3)$给出。李代数通过指数映射转换为李群，即$G\text{G}G={\mathrm{e}\mathrm{x}\mathrm{p}}_{se(3)}(\xi \text{ξ}\xi )$。该映射的逆变换为$\xi \text{ξ}\xi ={\mathrm{l}\mathrm{o}\mathrm{g}}_{SE(3)}(G\text{G}G)$。此外，我们使用$\mathfrak{s}\mathfrak{e}(3)$中的元素来表示位姿，直接写作向量$\xi \text{ξ}\xi \in {\mathbb{R}}^6$。从坐标系$i$移动一个点到坐标系$j$的变换记作${\xi \text{ξ}\xi }_{ji}$。为方便起见，我们将位姿连接操作符$\circ :\mathfrak{s}\mathfrak{e}(3)\times \mathfrak{s}\mathfrak{e}(3)\to \mathfrak{s}\mathfrak{e}(3)$定义为，
$${\xi \text{ξ}\xi }_{ki}:={\xi \text{ξ}\xi }_{kj}\circ {\xi \text{ξ}\xi }_{ji}:={\mathrm{l}\mathrm{o}\mathrm{g}}_{SE(3)}({\mathrm{e}\mathrm{x}\mathrm{p}}_{se(3)}({\xi \text{ξ}\xi }_{kj})\cdot {\mathrm{e}\mathrm{x}\mathrm{p}}_{se(3)}({\xi \text{ξ}\xi }_{ji}))\text{\hspace{1em}(2)}$$
进一步，我们定义三维投影warp函数$\omega$，它将图像中的一点$p\text{p}p$和其逆深度$d$通过$\xi \text{ξ}\xi$变换到相机坐标系，
$$\omega (p\text{p}p,d,\xi ):=\left(\begin{array}{c}{x}^{\prime }/z^{\prime }\\ y^{\prime }/z^{\prime }\\ 1/z^{\prime }\end{array}\right)with\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right)={\mathrm{e}\mathrm{x}\mathrm{p}}_{se(3)}(\xi \text{ξ}\xi )\left(\begin{array}{c}{p\text{p}p}_x/d\\ {p\text{p}p}_y/d\\ 1/d\\ 1\end{array}\right)\text{\hspace{1em}(3)}$$

3D相似变换。一个三维相似变换$S\text{S}S\in Sim(3)$包括旋转、缩放和平移。
$$S\text{S}S=\left(\begin{array}{cc}sR\text{R}R& t\text{t}t\\ 0\text{0}0& 1\end{array}\right)withR\text{R}R\in SO(3),t\text{t}t\in {\mathbb{R}}^{3}ands\in {\mathbb{R}}^{+}\text{\hspace{1em}(4)}$$
对于刚体变换，其最小表示是由相关李代数$\xi \text{ξ}\xi \in \mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$给出的，现在它有一个额外的自由度，即$\xi \text{ξ}\xi \in {\mathbb{R}}^7$。指数映射和对数映射，位姿连接(concatenation)和投影warp函数可以类似地定义为$\mathfrak{s}\mathfrak{e}(3)$的情况，进一步的细节见文献[23]。

2.2 李代数流形上的加权高斯牛顿优化方法

利用高斯牛顿法使两幅图像的光度误差最小化，
$$E(\xi \text{ξ}\xi )=\sum _i\underset{=:r_i^2(\xi )}{\underbrace{(I_{ref}({p\text{p}p}_i)-I(\omega ({p\text{p}p}_i,D_{ref}({p\text{p}p}_i),\xi \text{ξ}\xi )){)}^2}}\text{\hspace{1em}(5)}$$
假设存在独立同分布的高斯残差，上式给出了对$\xi \text{ξ}\xi$的最大似然估计。我们使用左乘公式：从初始估计${\xi \text{ξ}\xi }^{(0)}$开始，在每次迭代中，通过求解$E$的高斯牛顿二阶近似的最小值来计算左乘增量$\delta {\xi \text{ξ}\xi }^{(n)}$。
$$\delta {\xi \text{ξ}\xi }^{(n)}=-({J\text{J}J}^{T}J\text{J}J{)}^{-1}{J\text{J}J}^{T}r\text{r}r({\xi \text{ξ}\xi }^{(n)})withJ\text{J}J=\frac{\partial r\text{r}r(ϵ\text{ϵ}ϵ\circ {\xi \text{ξ}\xi }^{(n)})}{\partial ϵ\text{ϵ}ϵ}{|}_{ϵ=0}\text{\hspace{1em}(6)}$$
其中$J\text{J}J$是堆叠残差向量$r\text{r}r=(r_1,\cdots ,r_n{)}^T$对左乘增量$ϵ\text{ϵ}ϵ$的导数，${J\text{J}J}^{T}J\text{J}J$是高斯牛顿法中$E$的海塞矩阵近似。然后通过与计算出来的更新相乘得到新的估计，
$${\xi \text{ξ}\xi }^{(n+1)}=\delta {\xi \text{ξ}\xi }^{(n)}\circ {\xi \text{ξ}\xi }^{(n)}\text{\hspace{1em}(7)}$$
为了对来自遮挡或反射的异常值具有鲁棒性，研究者提出了不同的加权方案，从而得到一个迭代重加权的最小二乘问题。在每次迭代中，计算一个权值矩阵$W\text{W}W=W\text{W}W({\xi \text{ξ}\xi }^{(n)})$，减小较大残差的权重。迭代求解的误差函数为，
$$E(\xi \text{ξ}\xi )=\sum _i{w}_i(\xi \text{ξ}\xi )r_i^2(\xi \text{ξ}\xi )\text{\hspace{1em}(8)}$$
更新计算为，
$$\delta {\xi \text{ξ}\xi }^{(n)}=-({J\text{J}J}^{T}W\text{W}WJ\text{J}J{)}^{-1}{J\text{J}J}^{T}W\text{W}Wr({\xi \text{ξ}\xi }^{(n)})\text{\hspace{1em}(9)}$$
假设残差是独立的，最后一次迭代的海塞矩阵的逆$({J\text{J}J}^{T}WJ\text{WJ}WJ{)}^{-1}$是左乘误差的协方差${\sum \text{∑}\sum }_{\xi }$估计，
$${\xi \text{ξ}\xi }^{(n)}=ϵ\text{ϵ}ϵ\circ {\xi \text{ξ}\xi }_{true}withϵ\text{ϵ}ϵ\sim \mathcal{N}(0,{\Sigma \text{Σ}\Sigma }_{\xi })\text{\hspace{1em}(10)}$$
实际上，残差是高度相关的，因此${\Sigma }_{\xi }$只是一个下界——但它包含了关于不同自由度上噪声之间相关性的有价值的信息。注意，我们遵循左乘法约定，使用右乘法约定可以得到等价的结果。然而，估计的协方差${\Sigma }_{\xi }$依赖于乘法顺序，当在位姿图优化框架中使用时，必须考虑到这一点。这里使用的左乘法约定与文献[23]一致，而例如，g2o中的默认类型实现却为右乘法约定。

2.3 不确定性的传播

不确定性传播是一种统计工具，用于推导函数$f(X\text{X}X)$输出的不确定性，由其输入$X\text{X}X$的不确定性引起。假设$X\text{X}X$为高斯分布，协方差为${\Sigma }_X\text{ΣX}{\Sigma }_X$，则$f(X\text{X}X)$的协方差可近似(使用f的雅可比矩阵${J\text{J}J}_f$)为，
$${\Sigma \text{Σ}\Sigma }_f\approx {J\text{J}J}_f{\Sigma }_X\text{ΣX}{\Sigma }_X{J\text{J}J}_f^T\text{\hspace{1em}(11)}$$

3 大尺度直接单目SLAM

我们首先在3.1节中概述了完整的算法，并在3.2节中简要介绍了全局地图的表示。然后在3.3节(跟踪新帧)、3.4节(深度图估计)、3.5节(关键帧到关键帧跟踪)和最后3.6节(地图优化)中描述了算法的三个主要组成部分。

3.1 完整的算法

该算法由跟踪、深度图估计和地图优化三个主要部分组成，如图3所示。

图3 完整的LSD-SLAM算法概述

 

跟踪组件持续跟踪新的相机图像。也就是说，它利用前一帧的位姿作为初始化，估计它们相对于当前关键帧的刚体位姿$\xi \text{ξ}\xi \in \mathfrak{s}\mathfrak{e}(3)$。

深度图估计组件使用被跟踪的帧来细化或替换当前关键帧。深度是通过像素级滤波来实现的，再加上文献[9]中提出的交错空间正则化来改进。如果相机移动得太远，一个新的关键帧将从现有的近的关键帧中的点通过投影来初始化。

一旦一个关键帧被替换为跟踪参考，因此它的深度图将不会被进一步细化(refine)，它将被地图优化组件合并到全局地图中。为了检测回环和尺度漂移，利用尺度感知估计当前帧到最近关键帧的相似变换$\xi \text{ξ}\xi \in \mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$。

初始化。为了引导LSD-SLAM系统，用随机深度图和大方差初始化第一个关键帧。在最初的几秒钟内，如果相机有足够的平移运动，算法就会“锁定”到特定的配置，并在几个关键帧传播后收敛到正确的深度配置。附件中视频展示了一些例子。对这种不需要专门的初始引导就能收敛的能力进行更加全面的评估超出了本文的范围，并留给以后的工作。

3.2 地图表示

该地图被表示为关键帧的位姿图。每个关键帧${\mathcal{K}}_i$包含相机图片$I_i:{\Omega }_i\to \mathbb{R}$和逆深度图$D_i:{\Omega }_{D_i}\to {\mathbb{R}}^{+}$以及逆深度方差$V_i:{\Omega }_{D_i}\to {\mathbb{R}}^{+}$。注意，深度图和方差仅为像素的子集定义${\Omega }_{D_i}\subset {\Omega }_i$，包含足够大的灰度梯度附近的所有图像区域，因此是半稠密的。关键帧之间的边包含了相似变换${\xi \text{ξ}\xi }_{ji}\in \mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$的相对对齐，以及相应的协方差矩阵${\Sigma \text{Σ}\Sigma }_{ji}$。

3.3 跟踪新帧：直接$\mathfrak{s}\mathfrak{e}(3)$图像对齐

从现有的关键帧${\mathcal{K}}_i=(I_i,D_i,V_i)$开始，通过最小化方差归一化的光度误差来计算新图像$I_j$的相对三维位姿${\xi \text{ξ}\xi }_{ji}\in \mathfrak{s}\mathfrak{e}(3)$，
$$E_p({\xi \text{ξ}\xi }_{ji})=\sum _{p\in {\Omega }_{D_i}}\Vert \frac{r_p^2(p,{\xi }_{ji})}{{\sigma }_{r_p(p,{\xi }_{ji})}^2}{\Vert }_{\delta }\text{\hspace{1em}(12)}$$
$$with{r}_p(p,{\xi }_{ji}):=I_i(p)-I_j(\omega (p,D_i(p),{\xi }_{ji}))\text{\hspace{1em}(13)}$$
$${\sigma }_{r_p(p,{\xi }_{ji})}^2:=2{\sigma }_I^2+(\frac{\partial r_p(p,{\xi }_{ji})}{\partial D_i(p)}{)}^2{V}_i(p)\text{\hspace{1em}(14)}$$
其中$\Vert \cdot \Vert$是Huber范数，
$$||r^2|{|}_{\delta }:=\{\begin{array}{l}\frac{r^2}{2\delta }\mathrm{i}\mathrm{f}|r|\le \delta \\ \\ |r|-\frac{\delta }{2}\mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}\end{array}\text{\hspace{1em}(15)}$$
应用于归一化残差。残差方差的计算使用协方差传播，如第2.3节所述，并利用逆深度方差$V_i$。进一步，我们假设图像灰度为高斯噪声${\sigma }_I^2$。如第2.2节所述，使用迭代重加权高斯牛顿优化来实现最小化。

与以前的直接方法相比，本文提出的公式显式地考虑了深度估计上变化的噪声。这与直接的单目SLAM特别相关，不同像素的噪声差异很大，这取决于它们可见时间的长短。这与处理RGB-D数据的方法相反，后者逆深度的不确定性近似恒定。图4显示了这种加权在不同类型的运动中的表现。注意，新图像的深度信息不可知，因此新图像的尺度没有确定，并在$\mathfrak{s}\mathfrak{e}$(3)上执行最小化。

图4 数据归一化。(a)参考图像。(b-d)跟踪图像和残差的逆方差\sigma_{r_p}^{-2}。对于纯旋转，深度噪声对残差噪声没有影响，因此所有归一化因子相同。对于z方向上的平移，深度噪声对图像中心的像素没有影响，而对于x方向上的平移，它只影响x方向上灰度梯度的残差。

 

3.4 深度图估计

关键帧的选取。如果相机离现有的地图太远，一个新的关键帧将由最近的跟踪图像创建。我们对当前关键帧的相对距离和相对角度进行加权组合，
$$\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}({\xi \text{ξ}\xi }_{ji}):={\xi \text{ξ}\xi }_{ji}^{T}W\text{W}W{\xi \text{ξ}\xi }_{ji}\text{\hspace{1em}(16)}$$
其中$W\text{W}W$为包含权值的对角矩阵。请注意，正如在下一节中描述的，对每个关键帧都进行了缩放，使其平均逆深度为1。因此，这个阈值是相对于当前场景尺度而言的，并确保有足够的可能性进行小基线的立体比较。

图5 sim(3)上直接关键帧对齐。(a)-(c)两个关键帧、深度和深度方差。(d)-(f)光度残差、深度残差和Huber权重，优化前(左)和优化后(右)。

 

深度图创建。一旦新一帧被选择为关键帧，它的深度图将由前一关键帧中的投影点来初始化，然后按照文献[9]中提出的方法进行一次空间正则化和外点剔除。然后，缩放深度地图，使其平均逆深度为1 。这个缩放系数将直接融入到$\mathfrak{s}\mathfrak{i}\mathfrak{m}$(3)相机位姿中。最后，它替换了之前的关键帧，并用于跟踪后续的新帧。

深度图精修(refinement)。利用没有成为关键帧的被跟踪帧细化当前的关键帧。对于期望立体精度足够大的图像区域，执行大量非常有效的小基线立体比较，如文献[9]中所述。结果被合并到现有的深度图中，从而改进它并可能添加新的像素，这是使用文献[9]中提出的滤波方法完成的。

3.5 约束获取：直接$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$图像对齐

$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$上直接图像对齐。与RGB-D SLAM或双目SLAM相比，单目SLAM本质上是尺度模糊的，即真实世界的绝对尺度是不可观测的。在长轨迹上，这会导致尺度漂移，这是误差的主要来源之一。此外，所有距离都只按尺度定义，这导致基于阈值的外点剔除或参数化鲁棒核(如Huber)定义不明确。我们利用场景深度和跟踪精度之间固有的相关性来解决这个问题。每个创建的关键帧的深度图均被缩放，使其平均逆深度为1。作为回报，关键帧之间的边被估计为$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$中的元素，优雅地融合了关键帧之间的尺度差异，并且，特别是对于大型回环，允许显式地检测累积的尺度漂移。

为此，我们提出了一种新的方法在$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$上进行直接的、尺度漂移感知的图像对齐，该方法用于对齐两个不同尺度的关键帧。除了光度残差$r_p$之外，我们还加入了深度残差$r_d$，它惩罚了关键帧之间的逆深度的标准差，允许直接估计它们之间的尺度变换。被最小化的总误差函数为，
$$E({\xi \text{ξ}\xi }_{ji}):=\sum _{p\in {\Omega }_{D_i}}\Vert \frac{r_p^2(p\text{p}p,{\xi \text{ξ}\xi }_{ji})}{{\sigma }_{r_p(p,{\xi }_{ji})}^2}+\frac{r_d^2(p\text{p}p,{\xi \text{ξ}\xi }_{ji})}{{\sigma }_{r_d(p,{\xi }_{ji})}^2}{\Vert }_{\delta }\text{\hspace{1em}(17)}$$
其中光度残差$r_p^2$及其方差${\sigma }_{r_p}^2$由公式(13)和公式(14)分别给出。而深度残差及其方差的计算为，
$$r_d(p\text{p}p,{\xi \text{ξ}\xi }_{ji}):=[{p\text{p}p}^{\prime }{]}_3-D_j([{p\text{p}p}^{\prime }{]}_{1,2})\text{\hspace{1em}(18)}$$
$${\sigma }_{r_d(p,{\xi }_{ji})}^2:=V_j([p\text{p}p{]}_{1,2}^{\prime })(\frac{\partial r_d(p\text{p}p,{\xi \text{ξ}\xi }_{ji})}{\partial D_j([{p\text{p}p}^{\prime }{]}_{1,2})}{)}^2+V_i(p\text{p}p)(\frac{\partial r_d(p\text{p}p,{\xi \text{ξ}\xi }_{ji})}{\partial D_i(p\text{p}p)}{)}^2\text{\hspace{1em}(19)}$$
其中${p\text{p}p}^{\prime }:={\omega }_s(p\text{p}p,D_i(p\text{p}p),{\xi \text{ξ}\xi }_{ji})$表示被变换之后的点。请注意，Huber范数被应用于归一化的光度残差和深度残差之和——这解释了一个事实，如果一个是异常值，另一个通常也是异常值。注意，对于$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$上的跟踪，需要包含深度误差，因为仅依靠光度误差并不能约束尺度。利用迭代重加权高斯-牛顿算法（第2.2节）对$\mathfrak{s}\mathfrak{e}(3)$上的直接图像对齐进行最小化。在实践中，$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$跟踪在计算上只比$\mathfrak{s}\mathfrak{e}(3)$跟踪稍微贵一点，因为只需要很少的额外计算。

图6 两个大尺度变化的场景。每个关键帧显示相机frustum，其大小与关键帧的尺度相对应。

 

约束搜索。在地图中插入一个新的关键帧${\mathcal{K}}_i$之后，一些可能的回环关键帧${\mathcal{K}}_{j1},\cdots ,{\mathcal{K}}_{jn}$被收集。我们使用最接近的十个关键帧，以及一个通过基于外观的建图算法检测出来的大尺度回环关键帧候选项。为了避免插入错误的回环或插入错误跟踪的回环，我们执行一个反向跟踪检验。对于每个候选项${\mathcal{K}}_{jk}$，我们独立跟踪${\xi \text{ξ}\xi }_{j_{k}i}$和${\xi \text{ξ}\xi }_{i{j}_k}$。只有当两个估计在统计上是相似的，即如果
$$e({\xi \text{ξ}\xi }_{j_{k}i},{\xi \text{ξ}\xi }_{i{j}_k}):=({\xi \text{ξ}\xi }_{j_{k}i}\circ {\xi \text{ξ}\xi }_{i{j}_k}{)}^T({\Sigma \text{Σ}\Sigma }_{j_{k}i}+{\mathrm{A}\mathrm{d}\mathrm{j}}_{j_{k}i}{\Sigma \text{Σ}\Sigma }_{i{j}_k}{\mathrm{A}\mathrm{d}\mathrm{j}}_{j_{k}i}^T{)}^{-1}({\xi \text{ξ}\xi }_{j_{k}i}\circ {\xi \text{ξ}\xi }_{i{j}_k})\text{\hspace{1em}(20)}$$
足够小，它们被添加到全局地图中。因此，利用伴随矩阵${\mathrm{A}\mathrm{d}\mathrm{j}}_{j_{k}i}$将${\Sigma \text{Σ}\Sigma }_{i{j}_k}$变换到正确的正切空间中。

$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$跟踪的收敛半径。直接图像对齐的一个重要限制在于该问题固有的非凸性，因此需要一个足够准确的初始化。虽然对于新相机帧的跟踪来说，一个足够好的初始化是可用的（由前一帧的位姿给出），但在寻找回环约束时，并非如此，特别是对于大型回环。

对此的一个解决方案是使用非常少量的关键点来计算更好的初始化。使用来自现有逆深度图的深度值，这需要对齐两组三维点，这可以通过Horn的方法有效地给出闭式解。然而，我们在实践中发现，即使对于大型回环，收敛半径也足够大。特别是，我们发现可以通过以下措施大大地增加收敛半径。

高效二阶最小化(ESM)。虽然我们的结果证实了之前的工作，即ESM不会显著增加稠密图像对齐的精度，但我们观察到它确实略微增加了收敛半径。

由粗到精方法。虽然金字塔方法通常用于直接图像对齐，但我们发现，从$20\times 15$像素的非常低的分辨率开始，比通常的要小得多，有助于增加收敛半径。

对这些措施性能的评估见第4.3节。

3.6 地图优化

地图由一组关键帧和跟踪的$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$约束组成，在后台使用位姿图优化框架不断进行优化。最小化误差函数，根据第2.2节的左乘约定，由下式定义，
$$E({\xi \text{ξ}\xi }_{W1}\cdots {\xi \text{ξ}\xi }_{Wn}):=\underset{({\xi }_{ji},{\Sigma }_{ji})\in \epsilon }{\Sigma }({\xi \text{ξ}\xi }_{ji}\circ {\xi \text{ξ}\xi }_{Wi}^{-1}\circ {\xi \text{ξ}\xi }_{Wj}{)}^T{\Sigma \text{Σ}\Sigma }_{ji}^{-1}({\xi \text{ξ}\xi }_{ji}\circ {\xi \text{ξ}\xi }_{Wi}^{-1}\circ {\xi \text{ξ}\xi }_{Wj})\text{\hspace{1em}(21)}$$
其中$W$表示世界系。

4 结果

我们对LSD-SLAM进行了定量评估，包括使用公开的数据集，以及使用手持单目相机记录的具有挑战性的户外轨迹。补充视频中显示了一些被评估的轨迹。

4.1 大轨迹的定性结果

我们在几个长且具有挑战性的轨迹上测试了该算法，其中包括许多相机旋转、大规模变化和大回环。图7显示了一个大约500m长的轨迹，在发现大回环前后耗时6分钟。图8显示了一个具有挑战性的轨迹，在场景深度上有很大的变化，其中还包括一个回环。

图7 长且具有挑战性的户外轨迹的回环(左为回环后，右为回环前)。还显示了生成的点云的三个特写，以及特定关键帧的半稠密深度图。

 

图8 大尺度变化情况下的轨迹的累积点云，包括平均逆深度小于20厘米到大于10米的视图。在回环之后(右上)，几何图形是一致对齐的，而在此之前(左上)部分场景以不同的比例存在了两次。 最下面的一排显示了场景的不同特写。提出的尺度感知公式允许精确地估计精细细节和大规模几何——这种灵活性是单目方法的主要好处之一。

 

4.2 定量评估

我们在公开可用的RGB-D数据集上评估LSD-SLAM。请注意，对于单目SLAM来说，这是一个非常具有挑战性的基准，因为它包含快速旋转运动、强烈的运动模糊和滚动快门伪影。我们使用第一个深度图来启动系统，并得到正确的初始尺度。图9给出了得到的绝对轨迹误差，并与其它方法进行了比较。

图9 对于TUM RGB-D基准和两个模拟序列的结果，以绝对轨迹误差(RMSE)的形式展示，单位厘米。对于LSD-SLAM，我们还显示了创建的关键帧的数量。x表示跟踪失败，-表示没有可用数据。为了进行比较，我们分别给出了半稠密单目VO[9]、基于关键点的单目SLAM[15]、直接RGB-D SLAM[14]和基于关键点的RGB-D SLAM的结果[7]。注意，[14]和[7]使用来自传感器的深度信息，而其它的则不使用。

 

4.3 $\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$跟踪的收敛半径

我们计算了两个示例序列的收敛半径，结果如图10所示。即使直接图像对齐是非凸的优化问题，我们发现利用第3.5节中的措施，非常大的相机运动也可以被跟踪。可以看出，这些方法只增加了收敛半径，对跟踪精度没有显著影响。

图10 不同金字塔层数下sim(3)上的收敛半径和精度，直接图像对齐，带有或不带有ESM最小化(用明亮和灰暗表示)。各自序列的所有帧在第300帧(左)和第500帧(右)上被跟踪，使用标识作为初始化。下面的图显示了跟踪成功的帧；上面的图显示了最终的平移误差。ESM和金字塔层数的增加明显增大了收敛半径，但对跟踪精度没有显著影响。如果跟踪收敛，它几乎总是收敛到相同的最小值。

 

5 结论

我们提出了一种新的直接(无特征)单目SLAM算法，我们称之为LSD-SLAM，它能够在CPU上实时运行。与现有的直接方法(都是纯里程计方法)相比，它在环境的全局地图上维护和跟踪，其中包含关键帧的位姿图，以及相关的概率半稠密深度图。该方法主要由两个关键的创新组成。(1)在$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$上直接对齐两个关键帧，显式地合并和检测尺度漂移。(2)一种新的概率上的方法，将噪声的估计加入到深度图跟踪上。该地图以点云的形式表示，给出了一个半稠密的和高度精确的环境三维重建。我们的实验表明，该方法可以可靠地跟踪和绘制长度超过500米的手持式轨迹，特别是在相同序列内的大尺度变化(平均逆深度小于20厘米到大于10米)和大旋转，证明了它的通用性、鲁棒性和灵活性。

参考文献

略