论文阅读《Large-Scale Direct SLAM with Stereo Cameras》

摘要

我们提出了一种适用于双目相机的大尺度直接SLAM算法(stereo LSD-SLAM)，该算法在标准CPU上以高帧率实时运行。与基于稀疏兴趣点的方法相比，我们的方法直接基于所有高对比度像素的光度一致性来对图像进行对齐，这些像素包括角、边和高纹理区域。它同时通过两种类型的立体线索(cue)来估计这些像素的深度：通过固定基线双目相机得到的静态立体，以及利用相机运动的时间多视角立体。通过合并两种视差来源，我们的算法甚至在当固定基线立体视觉受到限制时，也可以估计像素的深度。另一方面，使用固定的基线可以避免纯单目SLAM中常见的尺度漂移。我们进一步提出了一种强大的方法来加强照明不变性，能够处理帧间的侵略性亮度变化，从而大大提高了在现实环境中的性能。在实验中，我们展示了双目SLAM基准如KITTI或EuRoC数据集的最先进结果。

1 介绍

解决实时约束下的视觉同时定位与建图(SLAM)的传统方法是利用稀疏兴趣点，因为它将图像中的大量像素减少到少量的特征。最近，一些实时的直接方法被提出，这些方法避免了对兴趣点的依赖，而是利用光度一致性约束直接对像素进行图像对齐和三维重建。与基于兴趣点的方法相比，直接方法的前提是图像信息可以稠密地被使用。不需要手动设计兴趣点检测器、描述子和匹配算法，这也会将SLAM算法限制在特定类型的特征上——通常只使用图像角点。相反，在直接的SLAM方法中，丰富的像素集合有助于深度估计和建图。

在本文中，我们首次提出了一个大尺度的直接视觉SLAM方法，用于在CPU上实时运行的双目相机。我们的方法估计高灰度梯度的像素点的深度及其不确定度，在线重建半稠密深度地图。通过基于深度图的图像光度对齐，它同时跟踪刚体运动。

在我们之前的大规模直接单目SLAM(LSD-SLAM)研究中，我们通过像素级立体获得关键帧中的深度。通过光度图像对齐来跟踪关键帧的相机运动。对于全局尺度的SLAM，关键帧之间相互对齐，并通过图优化来优化关键帧的位姿。由于在单目SLAM中无法观察到重构尺度，我们在图像直接对齐和位姿图优化中对尺度进行了优化。

图1 Stereo LSD-SLAM是一种完全直接的双目相机SLAM方法。它在单CPU上以30Hz运行，可以计算出精确的相机运动以及半稠密的概率深度图。我们利用静止和时间立体以及仿射照明变化修正，使得该方法在现实场景中准确和鲁棒。附件中的视频中展示了一些例子。

 

在这项工作中，我们将单目LSD-SLAM的时间立体与固定基线双目相机的静态立体组合起来。在每个像素处，我们的双目LSD-SLAM根据可用性将静态立体和时间立体集成到估计中。这个SLAM方法结合了单目运动结构恢复和固定基线双目深度估计。静态双目高效地去除尺度作为一个自由参数，而时间立体允许从双目相机小基线以外的基线估计深度。时间立体不像静态立体那样局限于一个特定的方向，如水平方向。相反，它的基线对应于帧间的平移运动。我们进一步提出了一种在直接图像对齐中处理光照变化的方法，这大大提高了我们的算法在现实世界中的鲁棒性。

我们在流行的Kitti基准和EuRoC微型飞行器挑战赛(EuRoC Challenge 3)的数据集上评估了Stereo LSD-SLAM，展示了我们方法的最先进性能。

2 相关工作

近年来，基于兴趣点的视觉里程计和SLAM方法得到了广泛的研究。Nister等人的开创性工作[1]创造了视觉里程计这个术语，他们提出了通过持续帧对帧匹配来估计单目和双目相机运动的稀疏方法。Chiuso等人[2]提出了第一个基于非线性滤波的实时单目SLAM方法。Davison[3]提出了MonSlam，一种实时的基于EKF的方法，在小型工作空间中演示了SLAM。在EKF-SLAM公式中跟踪稀疏的兴趣点，以恢复相机运动和兴趣点的(全局)3D位置。另一个稀疏单目SLAM的例子是并行跟踪和建图(PTAM[4])，它在一个光束调整框架中对跟踪和建图的优化进行分离和并行化。最近，Strasdat等人[5]在一种基于关键帧的稀疏单目SLAM优化方法中将尺度作为参数。

使用固定基线双目相机，尺度变得直接可观。一项早期的工作将EKF-SLAM应用于兴趣点的稀疏集合。Paz等人[7]在一个稀疏分层EKF-SLAM框架中结合了单目立体和固定基线立体。

避免稀疏兴趣点检测的直接方法最近在视觉SLAM中引起了关注。与稀疏方法相比，直接方法的一个主要优点是，它们不依赖于手工设计的图像特征，这些特征限制了可用于后续处理的信息类型。在RGB-D领域，直接法因其精度高和效率高而成为最先进的方法。LSD-SLAM[11]是第一个大尺度的直接单目SLAM方法。在LSD-SLAM中，相机运动跟踪到关键帧，然后利用概率滤波估计半稠密深度图。位姿图优化将关键帧以全局一致的方式对齐。LSD-SLAM在位姿图优化中考虑了尺度漂移，并找到了一个一致的尺度。对于双目相机，Comport等人提出了一种直接视觉里程计方法。他们的方法不显式地恢复深度，但对像素使用四焦(quadrifocal)约束来估计相机运动。在[13]中的直接立体方法中，视差图随时间集成(integrated)，而双目相机的运动通过使用估计深度的直接图像对齐进行跟踪。我们的方法中的关键帧也集成(integrate)了深度，而我们使用的是概率滤波。我们的方法结合了双目相机的固定立体和单目相机的时间立体。我们将其与基于位姿图的SLAM系统相结合，该系统可以全局优化关键帧的位姿。我们工作的进一步重要贡献是修正仿射照明变化，使得直接图像对齐在现实条件下可用。与之前的方法不同，我们以交替的方式优化仿射照明校正参数，这允许在图像对齐和照明校正中使用不同的外点剔除方案。

3 双目相机LSD-SLAM

LSD-SLAM[11]是一种基于关键帧的定位和建图方法，主要步骤如下：

• 相机的运动被跟踪到地图中的一个参考关键帧。如果相机离地图中现有的关键帧太远，就会生成新的关键帧。
• 当前参考关键帧的深度从时间立体中获得。
• 通过相互直接图像对齐和位姿图优化，使关键帧的位姿全局一致。

在双目LSD-SLAM中，通过静止立体直接估计关键帧的深度(见图2)。这种方法有许多优点，可以完全依赖于时间或静止立体。静止立体允许估计绝对尺度的世界，它独立于相机的运动。然而，静止立体被限制在一个恒定的基线(在许多情况下，是一个固定的方向)，这极大地将性能限制在一个特定的范围内。时间立体并不会将性能限制在一个特定的范围内，如[11]中所述。同样的传感器可以用于非常小和非常大的环境，并在两者之间无缝切换。另一方面，它不提供尺度和要求非退化的相机运动。将时间和静止立体结合的另一个好处是，可以使用多个基线方向：静止立体通常有一个水平基线——这不允许沿水平边缘估计深度，而时间立体允许通过提供其它运动方向来完成深度图。

图2 双目LSD-SLAM系统概述

具体而言，我们的主要贡献如下：

• 我们将LSD-SLAM推广到双目相机，将时间立体和静止立体结合在一起，形成了一种直接的实时的SLAM方法。
• 我们在直接图像对齐过程中显式地建模照明变化，从而使得该方法即使在具有挑战性的现实环境下也高度鲁棒。
• 我们对来自现实机器人应用的两个基准数据集进行了系统的评估，展示了我们方法的最先进性能。

A 符号

我们用粗体大写字母表示矩阵(如$R\text{R}R$)，用粗体小写字母表示向量(如$\xi \text{ξ}\xi$)。操作符$[\cdot {]}_n$表示选择矩阵的第$n$行。在本文中，我们使用$d$来表示路标点深度$z$的倒数，即$d=z^{-1}$。

在双目LSD-SLAM中，一个地图被维护为一组关键帧 K i = { I i l , I i r , D i , V i } \mathcal{K}_i=\{ I_i^l, I_i^r, D_i, V_i \} Ki​={Iil​,Iir​,Di​,Vi​}。每个关键帧包括双目相机的左图像$I^l:\Omega \to \mathbb{R}$和右图像$I^r:\Omega \to \mathbb{R}$，一张逆深度图$D_i:{\Omega }_{D_i}\to {\mathbb{R}}^{+}$和其方差图$V_i:{\Omega }_{D_i}\to {\mathbb{R}}^{+}$。只维护双目图像中的一张图像的深度和方差，我们总是选择左侧图像作为参考系。我们假定图像域$\Omega \subset {\mathbb{R}}^2$在双目校正图像坐标中给出，即相机的内外参数是先验已知的。域${\Omega }_{D_i}\subset \Omega$是像素的半稠密约束，利用它来估计深度。

我们将像素坐标记为$u\text{u}u=(u_x{u}_{y}1{)}^T$。三维位置$p\text{p}p=(p_x{p}_y{p}_{z}1{)}^T$通过映射投影至图像平面，即$u\text{u}u=\pi (p\text{p}p):=K\text{K}K((p_x/p_z)(p_y/p_z)1{)}^T$，其中$K\text{K}K$是相机矩阵。利用逆深度$d$进行反投影，即$p\text{p}p={\pi }^{-1}(u\text{u}u,d):=((d^{-1}{K\text{K}K}^{-1}u\text{u}u{)}^{T}1{)}^T$。

B 深度估计

我们在关键帧中估计场景的几何形状。每个关键帧的像素子集的逆深度服从高斯分布。该子集由图像梯度大的像素组成，因为与无纹理区域的像素相比，这些像素提供了丰富的结构信息和更鲁棒的视差估计。图3显示了这样一个半稠密深度图和相关方差图的示例。我们通过之前关键帧的深度传播来初始化当前图像上的深度图。随后，深度图在像素级深度滤波框架中使用新的观测数据进行更新。我们在空间上归一化深度图，并剔除外点。

图3 在每个关键帧中，对于所有灰度梯度大的像素，其逆深度服从高斯分布，从而可以估计它们的深度。从左到右：灰度图、半稠密逆深度图和逆深度方差图。

 

与单目SLAM相比，本文中的深度是通过静态立体和时间立体获得的。

静态立体：我们在另一张双目图像中沿着极线方向搜索来确定该图像上像素点的视差。在双目校正图像的情况下，这种搜索可以沿着水平方向非常有效地执行。

作为对应测量，我们使用沿着扫描线5个像素的SSD光度误差。在对视差进行亚像素精确细化(refinement)后，通过文献[16]中确定的几何误差和光度误差估计其方差。如果逆深度上有一个均值$d$和标准差${\sigma }_d$的高斯先验，我们将搜索约束在$[d-2{\sigma }_d,d+2{\sigma }_d]$。在实际应用中，除了重新初始化的假设之外，所有假设的搜索区间只包含很少的像素，这大大加快了搜索速度，降低了找到不正确或模糊匹配的概率。根据这两个误差源，我们预计图像梯度接近垂直方向的像素或图像梯度沿水平方向较低的像素不能提供准确的视差估计。因此，我们在静态立体中忽略这些像素。

当一个新的关键帧被初始化时，我们立即执行静态立体来更新和修剪(prune)传播的深度图。特别的，修剪(prune)删除了被遮挡的像素，我们填充了前向warping深度图所产生的空洞(holes)。随后，我们对跟踪到的非关键帧进行静态立体处理，并将得到的视差信息整合到跟踪到的关键帧中。在第一步中，依据位姿估计$\xi \text{ξ}\xi$将关键帧像素$u$处的逆深度假设转换到新帧上，
$${u\text{u}u}^{\prime }=\pi ({T\text{T}T}_{\xi }{\pi }^{-1}(u\text{u}u,d))\text{\hspace{1em}(1)}$$

$$d^{\prime }=[{T\text{T}T}_{\xi }{\pi }^{-1}(u\text{u}u,d){]}_3^{-1}\text{\hspace{1em}(2)}$$

$${\sigma }_{d^{\prime }}^2=(\frac{d}{d^{\prime }}{)}^4{\sigma }_d^2\text{\hspace{1em}(3)}$$
传播假设被用作立体搜索的先验，确定了各自的观测深度$d_{obs}^{\prime }$和观测方差${\sigma }_{d^{\prime },obs}^2$。最后，利用下式将观测变换回关键帧，
$$d_{obs}=[{T\text{T}T}_{\xi }^{-1}({\pi }^{-1}({u\text{u}u}^{\prime },d_{obs}^{\prime })){]}_3^{-1}\text{\hspace{1em}(4)}$$

$${\sigma }_{obs}^2=(\frac{d_{obs}^{\prime }}{d_{obs}}{)}^4{\sigma }_{d^{\prime },obs}^2\text{\hspace{1em}(5)}$$
并融合到深度图中。注意，从非关键帧的观测只能从现有的先验假设像素中产生，新的假设只从关键帧静止立体产生，或从时间立体。这个过程如图2所示。

图4 时间立体 vs 静止立体。在一个场景中，单独的时间立体(极线平行于道路上的车道标记)和静止立体(极线平行于水平桥梁)都不能捕捉到所有的信息。我们的组合方法融合了两者的信息，因此可以重建场景中的一切。

 

时间立体：跟踪后，我们估计当前帧与参考关键帧之间的视差，并将其融入到关键帧中。同样，我们只使用预期逆深度误差足够小的像素。我们从几个标准来确定这个不确定度：图像的梯度应该足够大，图像梯度不应该平行于极线，像素不应该靠近极点。我们参考文献[16]来了解这个方法的更多细节。虽然我们使用一个简单的5像素SSD误差，我们利用跟踪过程获得的仿射映射来修正仿射照明变化，正如第3-C节中所描述的。注意，对于时间立体，其几何误差通常比静止立体的高，因为相对相机位姿来自于直接图像对齐。这种位姿估计通常比双目相机离线外参校正更不准确。

C 带有仿射照明校正的直接图像对齐

我们利用直接图像对齐来确定两张图像之间的相机运动。我们使用这种方法来跟踪相机相对于参考关键帧的运动。它还用于估计关键帧之间的相对位姿约束，以实现位姿图优化。最后，我们提出了一种鲁棒的方法来补偿仿射光照变化。

直接图像对齐：通过最小化光度残差来估计图像$I_1^l$和$I_2^l$之间的相对位姿，
$$r_u^I(\xi \text{ξ}\xi ):=I_1^l(u\text{u}u)-I_2^l(\pi ({p\text{p}p}^{\prime }))\text{\hspace{1em}(6)}$$
其中$p^{\prime }:={T\text{T}T}_{\xi }{\pi }^{-1}(\pi ,D_1(u\text{u}u))$和$\xi \text{ξ}\xi$将图像$I_1^l$变换到图像$I_2^l$。我们同样确定了残差的不确定度${\sigma }_{r,u}^I$。从关键帧跟踪到当前帧的优化目标是，
$$E^{track}(\xi \text{ξ}\xi ):=\sum _{u\in {\Omega }_{D_1}}\rho \left(\frac{r_u^I(\xi \text{ξ}\xi )}{{\sigma }_{r,u}^I}\right)\text{\hspace{1em}(7)}$$
其中$\rho$是鲁棒权重函数，我们将其设为Huber范数。注意，与文献[12]相比，我们只将$I_1^l$对齐到$I_2^l$。虽然可以选择对新的右图像$I_2^r$添加光度约束，但我们观察到，这在实践中会降低精度：$I_1^l$和$I_2^r$之间的基线比$I_1^l$和$I_2^l$之间的大得多，导致更多来自遮挡和反射的外点。由于融合的深度在关键帧中是可用的，我们添加了关键帧之间对齐的几何残差，
$$r_u^D(\xi \text{ξ}\xi ):=[{p\text{p}p}^{\prime }{]}_3-D_2(\pi ({p\text{p}p}^{\prime }))\text{\hspace{1em}(8)}$$
这提供了起初跟踪新帧时无法获得的额外信息，因为这些还没有相关的深度估计。组合的目标为，
$$E^{keyframes}(\xi \text{ξ}\xi ):=\sum _{u\in {\Omega }_{D_1}}\left[\rho \right(\frac{r_u^I(\xi \text{ξ}\xi )}{{\sigma }_{r,u}^I})+\rho (\frac{r_u^D(\xi \text{ξ}\xi )}{{\sigma }_{r,u}^D}\left)\right]\text{\hspace{1em}(9)}$$
注意，这个公式利用了两帧的全部深度信息，包括来自静止立体的传播和融合观测（见第3-B小节）。这与文献[12]中的隐式四焦(quadrifocal)方法相反。

我们使用迭代重加权列文伯格-马夸尔特算法在一个左组合公式最小化这些目标：从初始估计${\xi \text{ξ}\xi }^{(0)}$开始，在每次迭代中通过求解具有固定权重的$E$的二阶近似的最小值来获得左乘增量$\delta {\xi \text{ξ}\xi }^{(n)}$：
$$\delta {\xi \text{ξ}\xi }^{(n)}=-({J\text{J}J}^{T}WJ\text{WJ}WJ+\lambda \mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}({J\text{J}J}^{T}WJ\text{WJ}WJ){)}^{-1}{J\text{J}J}^{T}Wr\text{Wr}Wr\text{\hspace{1em}(10)}$$
其中，
$$J\text{J}J=\frac{\partial r\text{r}r(ϵ\text{ϵ}ϵ\circ {\xi \text{ξ}\xi }^{(n)})}{\partial ϵ\text{ϵ}ϵ}{|}_{ϵ=0}\text{\hspace{1em}(11)}$$
是堆叠残差$r\text{r}r(\xi \text{ξ}\xi )$相对于左乘增量$ϵ\text{ϵ}ϵ$的导数。${J\text{J}J}^{T}WJ\text{WJ}WJ$是$E$的高斯牛顿近似的海塞矩阵。$W\text{W}W$是包含权值的对角矩阵。然后通过与计算出来的更新值相乘得到新的估计值，
$${\xi \text{ξ}\xi }^{(n+1)}=\delta {\xi \text{ξ}\xi }^{(n)}\circ {\xi \text{ξ}\xi }^{(n)}\text{\hspace{1em}(12)}$$
为了提高优化的效率和收敛范围，我们采用了由粗到精的方案。

假设残差在统计上是独立的，最后一次迭代的海塞矩阵的逆$({J\text{J}J}^{T}WJ\text{WJ}WJ{)}^{-1}$是左乘增量$ϵ\text{ϵ}ϵ$到最终最小值的协方差${\Sigma }_{\xi }$的估计，即
$${\xi \text{ξ}\xi }^{(n)}=ϵ\text{ϵ}ϵ\circ {\xi \text{ξ}\xi }_{true}\mathrm{w}\mathrm{i}\mathrm{t}\mathrm{h}ϵ\text{ϵ}ϵ\sim \mathcal{N}(0,{\Sigma }_{\xi })\text{\hspace{1em}(13)}$$
实际上，残差是高度相关的，这样的${\Sigma }_{\xi }$仅是一个下界。然而，它包含了关于不同自由度的噪声之间的相关性的有价值的信息。

仿射照明校正：直接图像对齐基本上是基于亮度恒定假设的，这是严重违反，例如，当相机曝光时间调整，以更好地拟合场景的平均亮度。一个著名的对策是使用一个代价函数，它对仿射光照变化是不敏感的，例如使用归一化互相关(NCC)而不是简单的差的平方和(SSD)进行匹配。在这里，我们提出了一个类似的方法，并修改了公式(6)中的光度残差，使其不受仿射照明变化的影响：
$$r_u^I(\xi \text{ξ}\xi ):=a{I}_1^l(u\text{u}u)+b-I_2^l({p\text{p}p}^{\prime })\text{\hspace{1em}(14)}$$
而不是在一个常见的误差公式中对$a$、$b$和$\xi \text{ξ}\xi$进行联合优化，我们使用不同的加权方案交替进行优化，(1)固定$a$和$b$，优化$\xi \text{ξ}\xi$；(2)固定$\xi \text{ξ}\xi$，优化$a$和$b$。这是由于$\xi \text{ξ}\xi$、$a$和$b$对外点的反应非常不同：

• $a$和$b$中的最小值受到遮挡和过曝像素的严重影响，因为这些往往会“拉”在同一个错误的方向上。另一方面，它通常只受少量内点残差的约束。因此，我们采用了一个简单的侵略性的切断SSD误差，即 ρ a , b ( r ) : = m i n { δ m a x , r 2 } \rho_{a,b}(r) := \mathrm{min}\{ \delta_{max}, r^2 \} ρa,b​(r):=min{δmax​,r2}。图5显示了两个示例场景，以及所得到的带有和不带有外点剔除的仿射映射。
• $\xi \text{ξ}\xi$的最小值受外点的影响要小得多，因为它们倾向于不同方向的“拉”，相互抵消。反过来，$\xi \text{ξ}\xi$的某些维度可能只受少量像素的限制，这些像素最初具有很高的残差。剔除这些外点会导致其收敛到错误的局部最小值。因此，我们采用[11]中提出的加权方案，它只降低权值，而不去除残差。

图5 仿射照明修正：强光照变化的两个场景。在右侧，我们显示了直接图像对齐后所有残差的散点图。绿线表示我们方法的最佳拟合，而红线表示所有像素的最佳拟合。请注意它是如何严重地受到由遮挡和过曝造成的外点的影响，这些外点在散点图中很容易识别。

 

$a$和$b$的最小化是通过迭代来实现的，
$$E_{a,b}(a,b):=\sum _{u\in {\Omega }_{D_1}}{\rho }_{a,b}((a{I}_1^l(u\text{u}u)+b)-I_2^l({u\text{u}u}^{\prime }))\text{\hspace{1em}(15)}$$
其中${u\text{u}u}^{\prime }:=\pi ({p\text{p}p}^{\prime })$，上式有如下闭式解：
$$a^{\ast }=\frac{{\sum }_{u\in {\Omega }_L}{I}_1^l(u\text{u}u)I_2^l({u\text{u}u}^{\prime })}{{\sum }_{u\in {\Omega }_L}{I}_2^l({u\text{u}u}^{\prime })I_2^l({u\text{u}u}^{\prime })}\text{\hspace{1em}(16)}$$

$$b^{\ast }=\frac{1}{|{\Omega }_L|}\sum _i(I_1^l({u\text{u}u}^{\prime })-a^{\ast }{I}_2^l(u\text{u}u))\text{\hspace{1em}(17)}$$

其中${\Omega }_L$表示内点集，
Ω L : = { u ∈ Ω D 1 ∣ ρ a , b ( ( a I 1 l ( u ) + b ) − I 2 l ( u ′ ) ) < δ m a x } \Omega_L:=\{ \pmb{u} \in \Omega_{D_1} | \rho_{a,b}((aI_1^l(\pmb{u})+b)-I_2^l(\pmb{u}')) < \delta_{max} \} ΩL​:={uuu∈ΩD1​​∣ρa,b​((aI1l​(uuu)+b)−I2l​(uuu′))<δmax​}
然后将得到的仿射参数$a$和$b$用于时间立体和深度传播一致性检验。

D 基于关键帧的SLAM

一旦确定了关键帧${\mathcal{K}}_i$，也就是说，在它被替换为跟踪参考并且不会接收到任何进一步的深度更新之后，它被添加至位姿图，在后台不断优化。通过对深度残差和仿射照明校正执行$\mathrm{S}\mathrm{E}(3)$对齐，可以获得一组可能的回环候选帧${\mathcal{K}}_{j_1},\cdots ,{\mathcal{K}}_{j_n}$，它们满足

• 物理距离小于$(60+p\cdot 0.05)$米。
• 在观看方向上的差值小于$(35+p\cdot 0.01)$度。

其中$p$为关键帧图中两个关键帧之间的最短连接路径的长度，单位为米。它是累积相对位姿误差的保守近似。对于非常大的地图，可以通过利用基于外观的图像检索技术(如FAB-MAP[17])找到额外的回环。然而，在我们的实验中，我们发现这是没有必要的。对于$p\le 100m$的关键帧，我们使用沿着该路径合成边缘得到的相对位姿作为初始化来执行直接图像对齐，否则使用单位阵。

对于每个候选关键帧${\mathcal{K}}_{j_k}$，我们通过最小化公式(9)来直接计算${\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 }^{-1}({\xi \text{ξ}\xi }_{j_{k}i}\circ {\xi \text{ξ}\xi }_{i{j}_k})\text{\hspace{1em}(18)}$$
其中
$$\Sigma \text{Σ}\Sigma :={\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\text{\hspace{1em}(19)}$$
足够小，它们作为约束被添加到位姿图中。此处，${\mathrm{A}\mathrm{d}\mathrm{j}}_{j_{k}i}$是${\xi \text{ξ}\xi }_{j_{k}i}$在$\mathrm{S}\mathrm{E}(3)$上的伴随矩阵。为了加速删除不正确的回环候选帧，我们在金字塔的每一层应用该一致性检验。只有当通过该检验时，直接图像对齐才继续到下一个更高的分辨率。这允许丢弃大多数不正确的候选帧，只浪费很少的计算资源。图6显示了Kitti数据集最长序列之一中金字塔的每一层跟踪了多少约束。

图6 来自Kitti基准的00序列的位姿图，包含1227个关键帧和3719个约束。该表显示了金字塔的每一层有多少个约束，以及在每一层中进行反向图像对齐所需的平均时间。请注意，大多数不正确的回环候选帧已经在非常粗糙的分辨率下被丢弃了，这是非常快的。在整个序列中，只需要43次大的回环尝试，就可以找到该序列中所有的回环。

 

4 结果

我们展示了Stereo LSD-SLAM获得的结果(1)在著名的Kitti数据集上，(2)在室内飞行的微型飞行器(MAV)记录的三个序列上，取自EuRoC挑战赛3。我们评估不同参数设置下的运行时间和准确性。虽然我们的实现使用多核CPU，但本章给出的所有计时都是指在2.8 GHz的Intel i7-4900MQ CPU上的单线程执行。

A EuRoC数据集

我们在EuRoC数据集上运行Stereo LSD-SLAM，该数据集来自一架在房间内飞行的微型飞行器，该飞行器配备了运动捕捉系统，用于获取位姿真值。数据集包含3条轨迹，且运动越来越剧烈。图7展示了重建结果。这三条轨迹的均方根误差分别为6.6厘米、7.4厘米和8.9厘米。在这个数据集中，我们删除了每个轨迹的前150张和后150张图像，因为其中一些只能看见地表。

图7 来自微型飞行器的EuRoC数据集。上图：第1条(左)和第3条(右)轨迹的重建结果。下图：从第3条轨迹中选择的图像，光照的剧烈变化(第1张和第2张)，运动模糊(第3张)和低纹理(第4张)。

 

B Kitti数据集

我们在著名的Kitti数据集上评估了我们的方法。表1总结了带有和不带有回环的Stereo LSD-SLAM和VO的结果。给出的是半分辨率图像的结果，因为我们认为这是精度和计算速度之间更好的权衡——参见第4-D小节。在评估序列11-21上，我们实现了平均平移RMSE为1.21%，目前在双目方法中排名第二。然而，双目LSD-SLAM比实现类似精度的方法要快得多。与测试序列00-10相比，误差的增加是由于20和21中存在许多移动物体，这导致直接图像对齐偶尔失败(第4-F小节)。此外，Kitti基准只提供10Hz的图像，而行驶速度高达80公里/小时，这对直接法而言是一个较大的挑战，因为直接法善于利用帧间的小运动。

 

表1 KITTI基准上的结果

其中$t_{rel}$是平移的RMSE漂移(%)，每隔100米至800米。$r_{rel}$是旋转的RMSE漂移(°/100米)，每隔100米至800米。$t_{abs}$是6自由度对齐后的绝对RMSE，单位为米。时间为每帧的单线程的计算时间，以毫秒为单位。

C 里程计 vs SLAM

在本小节，我们评估了在运行完整的SLAM系统时执行大尺度回环的能力，以及仅在最后$l$帧的一个小窗口中执行回环的效果——有效地将Stereo LSD-SLAM转化为视觉里程计。当$l=0$时，不进行带有几何误差的图像对齐，只使用初始帧对齐时的位姿。在此比较中，我们只考虑包含显著回环的Kitti序列，即00、02、05、06和07序列。图8总结了结果：可以清楚地看到，执行全SLAM大大降低了长期漂移，这一点并不令人惊讶。然而，这会增加计算成本：当执行完全SLAM时，总体计算预算需要翻倍以上(参见表1)，因为需要优化整个位姿图，同时需要跟踪许多回环。本小节中的所有数字都是指以半分辨率图像运行Stereo LSD-SLAM的结果。

图8 视觉里程计 vs SLAM。左：在不同的评估段长度下的平移漂移，每条曲线表示不同的位姿图优化窗口l。对于l等于无穷，我们的方法执行全SLAM；因此，当对较长的分段进行评估时，平移漂移减小(下降到0.5%)。右：Kitti基准00序列的6自由度对齐轨迹。虽然进行局部位姿图优化会略微提高局部精度，但它不能消除长段上的漂移。

 

D 图像分辨率的影响

Stereo LSD-SLAM的一个漂亮特性是，随着图像分辨率的降低，所获得的精度会优雅地下降，而所需的计算预算会迅速减少。事实上，我们能够在Kitti数据集上运行完整的SLAM和VO，其分辨率为原来的八分之一，即$154\times 46$像素。并且仍然实现了合理的平均平移漂移，为2.5%(SLAM)和3.5%(VO)，大大减少了计算成本，运行在$15\times$实时(SLAM)和$40\times$实时(VO)。结果如图9所示。

图9 图像分辨率：该图显示了不同图像分辨率的平均平移RMSE t_{rel}，以及所需的计算时间。Stereo LSD-SLAM允许一个平滑地交换另一个。对于原始图像分辨率的八分之一，它在单个线程中运行在400Hz(VO)/145Hz(SLAM)，仍然实现平均漂移仅为3.5%(VO)和2.5%(SLAM)。

 

E 性能分析

在表2中，我们总结了算法各部分所需的计算时间。所有的计时都是以毫秒为单位的，表示每张图像上各部分耗时。对于低分辨率的图像，在预处理步骤中进行下采样，因为这通常可以在不增加硬件成本的情况下完成(像素分块)。可以清楚地观察到，除位姿图优化外，算法的所有部分都直接随图像中像素的数量缩放。只有在非常低的分辨率下，分辨率独立的操作，如海塞矩阵在LM优化时的求逆操作，开始有视觉冲击(visual impact)。

表2 计算所需的时间

 

F 物体移动和遮挡

直接图像对齐方法的一个显著特性是“锁定特性”。在多个运动或外点的存在下，由粗到精的策略导致直接方法锁定在线性化有效半径内的最主要的运动上。一个鲁棒的加权函数允许最小化不属于这个运动的像素的影响。图10显示了图像的大部分移动或被遮挡的三个示例。在前两个例子中，主导运动被正确地识别，而在第三个例子中，图像对齐锁定在前景中移动的汽车上。我们只在Kitti基准的20序列中观察到这个问题，因为有许多汽车以相同的速度移动，这使得场景中的主要运动是汽车。对于在线评估，我们通过移除汽车前面一定体积内的所有点来解决这个问题。然而，未来的工作可以利用我们的方法，例如通过将场景运动分割成一些刚体运动。

图10 移动物体和强遮挡场景的例子。在右侧，我们展示了直接图像对齐后的灰度残差(小残差显示为灰色；较大的负/正残差以黑色/白色显示)。在前两个例子中，直接图像对齐锁定了正确的运动，而在最后一个例子中，它锁定了场景中的错误运动，即移动的汽车，并未能正确对齐两个图像。这可以从车道标记周围的残差中看出。

 

G 定性结果

我们在图11中展示了估计的半稠密深度图的一些定性结果，以及由此产生的点云。注意深度在几乎所有有梯度信息的区域是如何被估计的，并且许多细微的细节(标志和灯柱)是如何被恢复的。此外，包含时间立体允许估计严格水平结构的深度，如在一些图像中可见的输电线路。

图11 Kitti数据集的点云和深度图(08、14、15、18序列)，以全分辨率运行。也可参阅所附的视频。

 

5 结论

我们提出了Stereo LSD-SLAM，一种用双目相机实现SLAM的新方法。我们的方法利用静止的固定基线的立体以及时间的可变基线的立体。静止立体在双目相机的有效操作范围内提供精确的深度。它还消除了由沿视线方向退化的运动造成的尺度模糊和困难，这是仅使用时间立体的单目SLAM固有的问题。另一方面，在时间立体中，深度可以在帧间平移运动的可变基线方向上被估计。

我们的方法使用半稠密像素集合的光度和几何残差来直接对齐图像。我们选择有足够信息的用于静止或时间立体估计的像素。与基于稀疏兴趣点的方法相比，我们的方法不局限于特定类型的图像特征，它们在解耦处理阶段被提取以用来进行图像对齐。

在我们的实验中，Stereo LSD-SLAM展示了在流行的Kitti基准数据集上最先进的结果，用于自动驾驶汽车上的双目里程计和的SLAM。Stereo LSD-SLAM在EuRoC挑战赛3用微型飞行器(MAV)记录的具有挑战性的序列中也表现得非常准确。这两个数据集对于纯粹的单目SLAM方法都是非常具有挑战性的，因为运动主要是沿着视线(汽车)，或者主要由旋转(MAVs)组成。

在未来的工作中，我们考虑将我们的方法扩展到双目立体相机以外的多相机设备。与惯性或GPS信息进行传感器融合可以进一步提高局部和全局的精度和鲁棒性。最后，我们计划解决多目标运动分割和估计。这样，我们的方法不仅恢复了图像中的主导运动，而且还恢复了独立运动物体的运动。

参考文献

略