LSD-SLAM: Large-Scale Direct Monocular SLAM

Jakob Engel and Thomas Schöps and Daniel Cremers
Technical University Munich

Abstract

摘要

We propose a direct (feature-less) monocular SLAM algorithm which, in contrast to current state-of-the-art regarding direct methods, allows to build large-scale, consistent maps of the environment. Along with highly accurate pose estimation based on direct image alignment, the 3D environment is reconstructed in real-time as pose-graph of keyframes with associated semi-dense depth maps. These are obtained by filtering over a large number of pixelwise small-baseline stereo comparisons. The explicitly scale-drift aware formulation allows the approach to operate on challenging sequences including large variations in scene scale. Major enablers are two key novelties: (1) a novel direct tracking method which operates on $\mathrm{sim}(3)$, thereby explicitly detecting scale-drift, and (2) an elegant probabilistic solution to include the effect of noisy depth values into tracking. The resulting direct monocular SLAM system runs in real-time on a CPU.

我们提出一种直接法（无特征点，feature-less）单目 SLAM 算法。与当时直接法的主流工作不同，它能够构建大规模、一致的环境地图。在基于直接图像对齐的高精度位姿估计之外，三维环境被实时重建为带有半稠密深度图的关键帧位姿图。这些深度图通过对大量像素级小基线立体比较做滤波得到。显式感知尺度漂移的表述，使该方法能够处理包含场景尺度大幅变化的困难序列。两个关键新意构成了主要支撑：（1）一种在 $\mathrm{sim}(3)$ 上运行的新型直接跟踪方法，从而显式检测尺度漂移；（2）一种把有噪声深度值的影响纳入跟踪的简洁概率方案。由此得到的直接法单目 SLAM 系统可在 CPU 上实时运行。

1 Introduction

1 引言

Real-time monocular Simultaneous Localization and Mapping (SLAM) and 3D reconstruction have become increasingly popular research topics. Two major reasons are (1) their use in robotics, in particular to navigate unmanned aerial vehicles (UAVs) [10, 8, 1], and (2) augmented and virtual reality applications slowly making their way into the mass-market.

实时单目同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）以及三维重建，已成为越来越受关注的研究课题。两个主要原因是：（1）它们在机器人中的应用，尤其是用于无人飞行器（UAV）导航 [10, 8, 1]；（2）增强现实与虚拟现实应用正逐渐进入大众市场。

One of the major benefits of monocular SLAM – and simultaneously one of the biggest challenges – comes with the inherent scale-ambiguity: The scale of the world cannot be observed and drifts over time, being one of the major error sources. The advantage is that this allows to seamlessly switch between differently scaled environments, such as a desk environment indoors and large-scale outdoor environments. Scaled sensors on the other hand, such as depth or stereo cameras, have a limited range at which they can provide reliable measurements and hence do not provide this flexibility.

单目 SLAM 的一个主要优点——同时也是最大的挑战之一——来自固有的尺度歧义：世界的尺度无法被观测，并会随时间漂移，这是主要误差来源之一。好处是，这使得系统可以在不同尺度的环境之间无缝切换，例如室内桌面环境与大规模户外环境。相比之下，带尺度的传感器，如深度相机或双目相机，只能在有限距离内提供可靠测量，因而不具备这种灵活性。

1.1 Related Work

1.1 相关工作

Feature-Based Methods. The fundamental idea behind feature-based approaches (both filtering-based [15, 19] and keyframe-based [15]) is to split the overall problem – estimating geometric information from images – into two sequential steps: First, a set of feature observations is extracted from the image. Second, the camera position and scene geometry is computed as a function of these feature observations only.

基于特征的方法。基于特征的方法（包括基于滤波的 [15, 19] 和基于关键帧的 [15]）的基本想法，是把从图像估计几何信息这一整体问题拆成两个相继步骤：首先，从图像中提取一组特征观测；其次，仅把这些特征观测作为函数输入，计算相机位置和场景几何。

Fig. 1: Large-Scale Direct Monocular SLAM: LSD-SLAM generates a consistent global map, using direct image alignment and probabilistic, semi-dense depth maps instead of keypoints. Top: Accumulated pointclouds of all keyframes of a medium-sized trajectory (from a hand-held monocular camera), generated in real-time. Bottom: A selection of keyframes with color-coded semi-dense inverse depth map. See also the supplementary video.

图 1：大规模直接法单目 SLAM：LSD-SLAM 用直接图像对齐和概率半稠密深度图，而不是关键点，生成一致的全局地图。上：中等长度轨迹（来自手持单目相机）全部关键帧累积得到的点云，实时生成。下：若干关键帧及其伪彩色半稠密逆深度图。另见补充视频。

While this decoupling simplifies the overall problem, it comes with an important limitation: Only information that conforms to the feature type can be used. In particular, when using keypoints, information contained in straight or curved edges – which especially in man-made environments make up a large part of the image – is discarded. Several approaches have been made in the past to remedy this by including edge-based [16, 6] or even region-based [5] features. Yet, since the estimation of the high-dimensional feature space is tedious, they are rarely used in practice. To obtain dense reconstructions, the estimated camera poses can be used to subsequently reconstruct dense maps, using multiview stereo [2].

这种解耦简化了整体问题，但也带来一个重要限制：只有符合所采用特征类型的信息才能被使用。尤其在使用关键点时，直线或曲线边缘所包含的信息会被丢弃，而在人造环境中，这些边缘往往占图像的很大一部分。过去已有若干工作试图通过引入基于边缘 [16, 6] 甚至基于区域 [5] 的特征来弥补这一点。然而，由于高维特征空间的估计十分繁琐，它们在实践中很少被使用。若要得到稠密重建，可以利用已估计的相机位姿，再用多视图立体方法随后重建稠密地图 [2]。

Direct Methods. Direct visual odometry (VO) methods circumvent this limitation by optimizing the geometry directly on the image intensities, which enables using all information in the image. In addition to higher accuracy and robustness in particular in environments with little keypoints, this provides substantially more information about the geometry of the environment, which can be very valuable for robotics or augmented reality applications.

直接法。直接视觉里程计（visual odometry，VO）方法通过直接在图像强度上优化几何，绕开了这一限制，从而能够使用图像中的全部信息。除了在关键点稀少的环境中往往具有更高的精度和鲁棒性之外，它还提供关于环境几何的远为丰富的信息，这对机器人或增强现实应用非常有价值。

Fig. 2: In addition to accurate, semi-dense 3D reconstructions, LSD-SLAM also estimates the associated uncertainty. From left to right: Accumulated pointcloud thesholded with different maximum variance. Note how the reconstruction becomes significantly more dense, but at the same time includes more noise.

图 2：除了精确的半稠密三维重建之外，LSD-SLAM 还估计相应的不确定性。从左到右：按不同最大方差阈值截断后的累积点云。可以看到，重建会显著变密，但同时也会包含更多噪声。

While direct image alignment is well-established for RGB-D or stereo sensors [14, 4], only recently monocular direct VO algorithms have been proposed: In [24, 20, 21], accurate and fully dense depth maps are computed using a variational formulation, which however is computationally demanding and requires a state-of-the-art GPU to run in real-time. In [9], a semi-dense depth filtering formulation was proposed which significantly reduces computational complexity, allowing real-time operation on a CPU and even on a modern smartphone [22]. By combining direct tracking with keypoints, [10] achieves high frame-rates even on embedded platforms. All these approaches however are pure visual odometries, they only locally track the motion of the camera and do not build a consistent, global map of the environment including loop-closures.

虽然直接图像对齐在 RGB-D 或双目传感器上已经相当成熟 [14, 4]，单目直接视觉里程计算法却是直到近期才被提出：在 [24, 20, 21] 中，用变分表述计算精确且完全稠密的深度图，但计算量很大，实时运行需要当时最好的 GPU。在 [9] 中，提出了一种半稠密深度滤波表述，显著降低了计算复杂度，从而可以在 CPU 上实时运行，甚至可在当时的智能手机上运行 [22]。通过把直接跟踪与关键点结合起来，[10] 即使在嵌入式平台上也能达到高帧率。然而，这些方法都只是纯视觉里程计：它们只在局部跟踪相机运动，并不构建包含回环的一致全局环境地图。

Pose Graph Optimization. This is a well-known SLAM technique to build a consistent, global map: The world is represented as a number of keyframes connected by pose-pose constraints, which can be optimized using a generic graph optimization framework like g2o [18].

位姿图优化。这是一种众所周知的、用于构建一致全局地图的 SLAM 技术：世界被表示成由位姿–位姿约束连接的若干关键帧，可以用 g2o [18] 这类通用图优化框架来优化。

In [14], a pose graph based RGB-D SLAM method is proposed, which also incorporates geometric error to allow tracking through scenes with little texture. To account for scale-drift arising in monocular SLAM, [23] proposed a keypoint-based monocular SLAM system which represents camera poses as 3D similarity transforms instead of rigid body movements.

文献 [14] 提出一种基于位姿图的 RGB-D SLAM 方法，并把几何误差也纳入进来，以便在纹理很少的场景中继续跟踪。为处理单目 SLAM 中出现的尺度漂移，[23] 提出一种基于关键点的单目 SLAM 系统，把相机位姿表示为三维相似变换，而不是刚体运动。

1.2 Contributions and Outline

1.2 贡献与文章结构

We propose a Large-Scale Direct monocular SLAM (LSD-SLAM) method, which not only locally tracks the motion of the camera, but allows to build consistent, large-scale maps of the environment (see Fig. 1 and 2). The method uses direct image alignment coupled with filtering-based estimation of semi-dense depth maps as originally proposed in [9]. The global map is represented as a pose graph consisting of keyframes as vertices with 3D similarity transforms as edges, elegantly incorporating changing scale of the environment and allowing to detect and correct accumulated drift. The method runs in real-time on a CPU, and as odometry even on a modern smartphone [22]. The main contributions of this paper are (1) a framework for large-scale, direct monocular SLAM, in particular a novel scale-aware image alignment algorithm to directly estimate the similarity transform $\boldsymbol{\xi} \in \mathrm{sim}(3)$ between two keyframes, and (2) probabilistically consistent incorporation of uncertainty of the estimated depth into tracking.

我们提出一种大规模直接法单目 SLAM（Large-Scale Direct monocular SLAM，LSD-SLAM）方法，它不仅在局部跟踪相机运动，还能够构建一致的大规模环境地图（见图 1 和图 2）。该方法采用直接图像对齐，并与 [9] 中最初提出的基于滤波的半稠密深度图估计相结合。全局地图被表示成一张位姿图：顶点是关键帧，边是三维相似变换，从而自然地纳入环境尺度的变化，并能够检测和纠正累积漂移。该方法可在 CPU 上实时运行，作为里程计时甚至可在当时的智能手机上运行 [22]。本文的主要贡献是：（1）一个面向大规模直接法单目 SLAM 的框架，特别是一种新的尺度感知图像对齐算法，用于直接估计两关键帧之间的相似变换 $\boldsymbol{\xi} \in \mathrm{sim}(3)$；（2）把估计深度的不确定性以概率一致的方式纳入跟踪。

2 Preliminaries

2 预备知识

In this chapter we give a condensed summary of the relevant mathematical concepts and notation. In particular, we summarize the representation of 3D poses as elements of Lie-Algebras (Sec. 2.1), derive direct image alignment as weighted least-squares minimization on Lie-manifolds (Sec. 2.2), and briefly introduce propagation of uncertainty (Sec. 2.3).

本章对相关数学概念和记号作一简要汇总。具体而言，我们汇总把三维位姿表示为李代数元素的做法（第 2.1 节），把直接图像对齐推导为李流形上的加权最小二乘最小化（第 2.2 节），并简要介绍不确定性传播（第 2.3 节）。

Notation. We denote matrices by bold, capital letters ($\mathbf{R}$) and vectors as bold, lower case letters ($\boldsymbol{\xi}$). The $n$'th row of a matrix is denoted by $[\cdot]_n$. Images $I: \Omega \to \mathbb{R}$, the per-pixel inverse depth map $D: \Omega \to \mathbb{R}^+$ and the inverse depth variance map $V: \Omega \to \mathbb{R}^+$ are written as functions, where $\Omega \subset \mathbb{R}^2$ is the set of normalized pixel coordinates, i.e., they include the intrinsic camera calibration. Throughout the paper we use $d$ to denote the inverse of the depth $z$ of a point, i.e., $d = z^{-1}$.

记号。我们用粗体大写字母表示矩阵（$\mathbf{R}$），用粗体小写字母表示向量（$\boldsymbol{\xi}$）。矩阵的第 $n$ 行记作 $[\cdot]_n$。图像 $I: \Omega \to \mathbb{R}$、逐像素逆深度图 $D: \Omega \to \mathbb{R}^+$ 以及逆深度方差图 $V: \Omega \to \mathbb{R}^+$ 都写成函数，其中 $\Omega \subset \mathbb{R}^2$ 是归一化像素坐标的集合，也即它们已包含相机内参标定。全文用 $d$ 表示一点深度 $z$ 的倒数，即 $d = z^{-1}$。

2.1 3D Rigid Body and Similarity Transformations

2.1 三维刚体变换与相似变换

3D Rigid Body Transformations. A 3D rigid body transform $\mathbf{G} \in \mathrm{SE}(3)$ denotes rotation and translation in 3D, i.e. is defined by

三维刚体变换。三维刚体变换 $\mathbf{G} \in \mathrm{SE}(3)$ 表示三维中的旋转和平移，即定义为

$$\mathbf{G} = \begin{pmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{pmatrix} \quad\text{with}\quad \mathbf{R} \in \mathrm{SO}(3)\ \text{and}\ \mathbf{t} \in \mathbb{R}^3. \tag{1}$$

During optimization, a minimal representation for the camera pose is required, which is given by the corresponding element $\boldsymbol{\xi} \in \mathfrak{se}(3)$ of the associated Lie-algebra. Elements are mapped to $\mathrm{SE}(3)$ by the exponential map $\mathbf{G} = \exp_{\mathfrak{se}(3)}(\boldsymbol{\xi})$, its inverse being denoted by $\boldsymbol{\xi} = \log_{\mathrm{SE}(3)}(\mathbf{G})$. With a slight abuse of notation, we consistently use elements of $\mathfrak{se}(3)$ to represent poses, which we directly write as vector $\boldsymbol{\xi} \in \mathbb{R}^6$. The transformation moving a point from frame $i$ to frame $j$ is written as $\boldsymbol{\xi}_{ji}$. For convenience, we define the pose concatenation operator $\circ: \mathfrak{se}(3) \times \mathfrak{se}(3) \to \mathfrak{se}(3)$ as

在优化过程中，需要相机位姿的一种极小表示，它由相应李代数中的对应元素 $\boldsymbol{\xi} \in \mathfrak{se}(3)$ 给出。元素通过指数映射 $\mathbf{G} = \exp_{\mathfrak{se}(3)}(\boldsymbol{\xi})$ 映到 $\mathrm{SE}(3)$，其逆记为 $\boldsymbol{\xi} = \log_{\mathrm{SE}(3)}(\mathbf{G})$。我们略微滥用记号，始终用 $\mathfrak{se}(3)$ 的元素来表示位姿，并直接把它写成向量 $\boldsymbol{\xi} \in \mathbb{R}^6$。把一点从坐标系 $i$ 变到坐标系 $j$ 的变换记作 $\boldsymbol{\xi}_{ji}$。为方便起见，我们定义位姿连接算子 $\circ: \mathfrak{se}(3) \times \mathfrak{se}(3) \to \mathfrak{se}(3)$ 为

$$\boldsymbol{\xi}_{ki} := \boldsymbol{\xi}_{kj} \circ \boldsymbol{\xi}_{ji} := \log_{\mathrm{SE}(3)}\bigl(\exp_{\mathfrak{se}(3)}(\boldsymbol{\xi}_{kj}) \cdot \exp_{\mathfrak{se}(3)}(\boldsymbol{\xi}_{ji})\bigr). \tag{2}$$

Further, we define the 3D projective warp function $\omega$, which projects an image point $\mathbf{p}$ and its inverse depth $d$ into a by $\boldsymbol{\xi}$ transformed camera frame

此外，我们定义三维投影扭曲函数 $\omega$，它把图像点 $\mathbf{p}$ 及其逆深度 $d$ 投影到经 $\boldsymbol{\xi}$ 变换后的相机坐标系中

$$\omega(\mathbf{p}, d, \boldsymbol{\xi}) := \begin{pmatrix} x'/z' \\ y'/z' \\ 1/z' \end{pmatrix} \quad\text{with}\quad \begin{pmatrix} x' \\ y' \\ z' \\ 1 \end{pmatrix} := \exp_{\mathfrak{se}(3)}(\boldsymbol{\xi})\begin{pmatrix} p_x/d \\ p_y/d \\ 1/d \\ 1 \end{pmatrix}. \tag{3}$$

3D Similarity Transformations. A 3D similarity transform $\mathbf{S} \in \mathrm{Sim}(3)$ denotes rotation, scaling and translation, i.e. is defined by

三维相似变换。三维相似变换 $\mathbf{S} \in \mathrm{Sim}(3)$ 表示旋转、缩放和平移，即定义为

$$\mathbf{S} = \begin{pmatrix} s\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{pmatrix} \quad\text{with}\quad \mathbf{R} \in \mathrm{SO}(3),\ \mathbf{t} \in \mathbb{R}^3\ \text{and}\ s \in \mathbb{R}^+. \tag{4}$$

As for rigid body transformations, a minimal representation is given by elements of the associated Lie-algebra $\boldsymbol{\xi} \in \mathrm{sim}(3)$, which now have an additional degree of freedom, that is $\boldsymbol{\xi} \in \mathbb{R}^7$. The exponential and logarithmic map, pose concatenation and a projective warp function $\omega_s$ can be defined analogously to the $\mathfrak{se}(3)$ case, for further details see [23].

与刚体变换类似，极小表示由相应李代数的元素 $\boldsymbol{\xi} \in \mathrm{sim}(3)$ 给出，现在多出一个自由度，即 $\boldsymbol{\xi} \in \mathbb{R}^7$。指数映射与对数映射、位姿连接以及投影扭曲函数 $\omega_s$，都可以按与 $\mathfrak{se}(3)$ 情形类似的方式定义，细节见 [23]。

2.2 Weighted Gauss-Newton Optimization on Lie-Manifolds

2.2 李流形上的加权高斯–牛顿优化

Two images are aligned by Gauss-Newton minimization of the photometric error

通过对光度误差做高斯–牛顿最小化，将两幅图像对齐

$$E(\boldsymbol{\xi}) = \sum_i \underbrace{\bigl(I_{\mathrm{ref}}(\mathbf{p}_i) - I(\omega(\mathbf{p}_i, D_{\mathrm{ref}}(\mathbf{p}_i), \boldsymbol{\xi}))\bigr)^2}_{=:\,r_i^2(\boldsymbol{\xi})}, \tag{5}$$

which gives the maximum-likelihood estimator for $\boldsymbol{\xi}$ assuming i.i.d. Gaussian residuals. We use a left-compositional formulation: Starting with an initial estimate $\boldsymbol{\xi}^{(0)}$, in each iteration a left-multiplied increment $\delta\boldsymbol{\xi}^{(n)}$ is computed by solving for the minimum of a Gauss-Newton second-order approximation of $E$:

在残差独立同分布且为高斯的假定下，这给出 $\boldsymbol{\xi}$ 的极大似然估计。我们采用左复合表述：从初值估计 $\boldsymbol{\xi}^{(0)}$ 出发，在每次迭代中，通过求解 $E$ 的高斯–牛顿二阶近似的极小值，计算一个左乘增量 $\delta\boldsymbol{\xi}^{(n)}$：

$$\delta\boldsymbol{\xi}^{(n)} = -(\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\mathbf{r}(\boldsymbol{\xi}^{(n)}) \quad\text{with}\quad \mathbf{J} = \left.\frac{\partial \mathbf{r}(\boldsymbol{\epsilon} \circ \boldsymbol{\xi}^{(n)})}{\partial \boldsymbol{\epsilon}}\right|_{\boldsymbol{\epsilon}=0}, \tag{6}$$

where $\mathbf{J}$ is the derivative of the stacked residual vector $\mathbf{r} = (r_1, \ldots, r_n)^T$ with respect to a left-multiplied increment, and $\mathbf{J}^T\mathbf{J}$ the Gauss-Newton approximation of the Hessian of $E$. The new estimate is then obtained by multiplication with the computed update

其中 $\mathbf{J}$ 是堆叠残差向量 $\mathbf{r} = (r_1, \ldots, r_n)^T$ 对左乘增量的导数，$\mathbf{J}^T\mathbf{J}$ 是 $E$ 的 Hessian 的高斯–牛顿近似。新的估计再由与所计算更新的乘法得到

$$\boldsymbol{\xi}^{(n+1)} = \delta\boldsymbol{\xi}^{(n)} \circ \boldsymbol{\xi}^{(n)}. \tag{7}$$

In order to be robust to outliers arising e.g. from occlusions or reflections, different weighting-schemes [14] have been proposed, resulting in an iteratively re-weighted least-squares problem: In each iteration, a weight matrix $\mathbf{W} = \mathbf{W}(\boldsymbol{\xi}^{(n)})$ is computed which down-weights large residuals. The iteratively solved error function then becomes

为了对遮挡或反射等引起的离群点保持鲁棒，已有若干不同的加权方案被提出 [14]，从而得到迭代重加权最小二乘问题：在每次迭代中，计算一个权重矩阵 $\mathbf{W} = \mathbf{W}(\boldsymbol{\xi}^{(n)})$，以降低大残差的权重。于是迭代求解的误差函数变为

$$E(\boldsymbol{\xi}) = \sum_i w_i(\boldsymbol{\xi}) r_i^2(\boldsymbol{\xi}), \tag{8}$$

and the update is computed as

更新则按如下方式计算

$$\delta\boldsymbol{\xi}^{(n)} = -(\mathbf{J}^T\mathbf{W}\mathbf{J})^{-1}\mathbf{J}^T\mathbf{W}\mathbf{r}(\boldsymbol{\xi}^{(n)}). \tag{9}$$

Assuming the residuals to be independent, the inverse of the Hessian from the last iteration $(\mathbf{J}^T\mathbf{W}\mathbf{J})^{-1}$ is an estimate for the covariance $\boldsymbol{\Sigma}_{\boldsymbol{\xi}}$ of a left-multiplied error onto the final result, that is

若假定残差相互独立，则最后一次迭代中 Hessian 的逆 $(\mathbf{J}^T\mathbf{W}\mathbf{J})^{-1}$ 是左乘误差作用到最终结果上的协方差 $\boldsymbol{\Sigma}_{\boldsymbol{\xi}}$ 的一个估计，也即

$$\boldsymbol{\xi}^{(n)} = \boldsymbol{\epsilon} \circ \boldsymbol{\xi}_{\mathrm{true}} \quad\text{with}\quad \boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\boldsymbol{\xi}}). \tag{10}$$

Fig. 3: Overview over the complete LSD-SLAM algorithm.

图 3：完整 LSD-SLAM 算法的概览。

In practice, the residuals are highly correlated, such that $\boldsymbol{\Sigma}_{\boldsymbol{\xi}}$ is only a lower bound – yet it contains valuable information about the correlation between noise on the different degrees of freedom. Note that we follow a left-multiplication convention, equivalent results can be obtained using a right-multiplication convention. However, the estimated covariance $\boldsymbol{\Sigma}_{\boldsymbol{\xi}}$ depends on the multiplication order – when used in a pose graph optimization framework, this has to be taken into account. The left-multiplication convention used here is consistent with [23], while e.g. the default type-implementation in g2o [18] assumes right-multiplication.

在实践中，残差高度相关，因此 $\boldsymbol{\Sigma}_{\boldsymbol{\xi}}$ 只是一个下界——但它仍然包含关于不同自由度上噪声相关性的有价值信息。注意我们采用左乘约定，使用右乘约定可以得到等价的结果。不过，估计得到的协方差 $\boldsymbol{\Sigma}_{\boldsymbol{\xi}}$ 依赖于乘法次序——在位姿图优化框架中使用时，必须考虑这一点。这里采用的左乘约定与 [23] 一致，而例如 g2o [18] 中的默认类型实现则假定右乘。

2.3 Propagation of Uncertainty

2.3 不确定性传播

Propagation of uncertainty is a statistical tool to derive the uncertainty of the output of a function $f(\mathbf{X})$, caused by uncertainty on its input $\mathbf{X}$. Assuming $\mathbf{X}$ to be Gaussian distributed with covariance $\boldsymbol{\Sigma}_{\mathbf{X}}$, the covariance of $f(\mathbf{X})$ can be approximated (using the Jacobian $\mathbf{J}_f$ of $f$) by

不确定性传播是一种统计工具，用于由输入 $\mathbf{X}$ 上的不确定性，导出函数 $f(\mathbf{X})$ 输出的不确定性。假定 $\mathbf{X}$ 服从协方差为 $\boldsymbol{\Sigma}_{\mathbf{X}}$ 的高斯分布，则 $f(\mathbf{X})$ 的协方差可以用 $f$ 的雅可比 $\mathbf{J}_f$ 近似为

$$\boldsymbol{\Sigma}_f \approx \mathbf{J}_f \boldsymbol{\Sigma}_{\mathbf{X}} \mathbf{J}_f^T. \tag{11}$$

3 Large-Scale Direct Monocular SLAM

3 大规模直接法单目 SLAM

We start by giving an overview of the complete algorithm in Sec. 3.1, and briefly introduce the representation for the global map in Sec. 3.2. The three main components of the algorithm are then described in Sec. 3.3 (tracking of new frames), Sec. 3.4 (depth map estimation), Sec. 3.5 (keyframe-to-keyframe tracking) and finally Sec. 3.6 (map optimization).

我们首先在第 3.1 节给出完整算法的概览，并在第 3.2 节简要介绍全局地图的表示。算法的三个主要组成部分随后分别在第 3.3 节（新帧跟踪）、第 3.4 节（深度图估计）、第 3.5 节（关键帧到关键帧的跟踪）以及最后第 3.6 节（地图优化）中描述。

3.1 The Complete Method

3.1 完整方法

The algorithm consists of three major components: tracking, depth map estimation and map optimization as visualized in Fig. 3:

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

— The tracking component continuously tracks new camera images. That is, it estimates their rigid body pose $\boldsymbol{\xi} \in \mathfrak{se}(3)$ with respect to the current keyframe, using the pose of the previous frame as initialization.

— 跟踪部分持续跟踪新的相机图像。也就是说，它以上一帧的位姿为初值，估计新图像相对于当前关键帧的刚体位姿 $\boldsymbol{\xi} \in \mathfrak{se}(3)$。

— The depth map estimation component uses tracked frames to either refine or replace the current keyframe. Depth is refined by filtering over many per-pixel, small-baseline stereo comparisons coupled with interleaved spatial regularization as originally proposed in [9]. If the camera has moved too far, a new keyframe is initialized by projecting points from existing, close-by keyframes into it.

— 深度图估计部分利用已跟踪的帧来精化或替换当前关键帧。深度通过在许多像素级小基线立体比较上滤波，并与交错进行的空间正则化相结合来精化，这一做法最初在 [9] 中提出。若相机移动过远，则通过把已有邻近关键帧中的点投影到新帧上，来初始化一个新的关键帧。

— Once a keyframe is replaced as tracking reference – and hence its depth map will not be refined further – it is incorporated into the global map by the map optimization component. To detect loop closures and scale-drift, a similarity transform $\boldsymbol{\xi} \in \mathrm{sim}(3)$ to close-by existing keyframes (including its direct predecessor) is estimated using scale-aware, direct $\mathrm{sim}(3)$-image alignment.

— 一旦某一关键帧不再作为跟踪参考——因而其深度图不会再被继续精化——地图优化部分就把它纳入全局地图。为了检测回环和尺度漂移，该方法使用尺度感知的直接 $\mathrm{sim}(3)$ 图像对齐，估计它到邻近已有关键帧（包括其直接前驱）的相似变换 $\boldsymbol{\xi} \in \mathrm{sim}(3)$。

Initialization. To bootstrap the LSD-SLAM system, it is sufficient to initialize a first keyframe with a random depth map and large variance. Given sufficient translational camera movement in the first seconds, the algorithm “locks” to a certain configuration, and after a couple of keyframe propagations converges to a correct depth configuration. Some examples are shown in the attached video. A more thorough evaluation of this ability to converge without dedicated initial bootstrapping is outside the scope of this paper, and remains for future work.

初始化。要启动 LSD-SLAM 系统，只需用一张随机深度图并配以较大方差来初始化第一个关键帧即可。只要在开始的几秒内有足够的相机平移运动，算法就会“锁定”到某一构型，并在几次关键帧传播之后收敛到正确的深度构型。一些例子见所附视频。对这种无需专门初始引导即可收敛的能力作更充分的评估，超出本文范围，留待未来工作。

3.2 Map Representation

3.2 地图表示

The map is represented as a pose graph of keyframes: Each keyframe $\mathcal{K}_i$ consists of a camera image $I_i: \Omega_i \to \mathbb{R}$, an inverse depth map $D_i: \Omega_{D_i} \to \mathbb{R}^+$, and the variance of the inverse depth $V_i: \Omega_{D_i} \to \mathbb{R}^+$. Note that the depth map and variance are only defined for a subset of pixels $\Omega_{D_i} \subset \Omega_i$, containing all image regions in the vicinity of sufficiently large intensity gradient, hence semi-dense. Edges $E_{ji}$ between keyframes contain their relative alignment as similarity transform $\boldsymbol{\xi}_{ji} \in \mathrm{sim}(3)$, as well as the corresponding covariance matrix $\boldsymbol{\Sigma}_{ji}$.

地图被表示成关键帧的位姿图：每个关键帧 $\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$ 上有定义，该子集包含所有邻近足够大强度梯度的图像区域，因此是半稠密的。关键帧之间的边 $E_{ji}$ 包含它们作为相似变换 $\boldsymbol{\xi}_{ji} \in \mathrm{sim}(3)$ 的相对对齐，以及相应的协方差矩阵 $\boldsymbol{\Sigma}_{ji}$。

3.3 Tracking new Frames: Direct $\mathfrak{se}(3)$ Image Alignment

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

Starting from an existing keyframe $\mathcal{K}_i = (I_i, D_i, V_i)$, the relative 3D pose $\boldsymbol{\xi}_{ji} \in \mathfrak{se}(3)$ of a new image $I_j$ is computed by minimizing the variance-normalized photometric error

从已有关键帧 $\mathcal{K}_i = (I_i, D_i, V_i)$ 出发，新图像 $I_j$ 的相对三维位姿 $\boldsymbol{\xi}_{ji} \in \mathfrak{se}(3)$ 通过最小化方差归一化的光度误差来计算

$$E_p(\boldsymbol{\xi}_{ji}) = \sum_{\mathbf{p} \in \Omega_{D_i}} \left\| \frac{r_p^2(\mathbf{p}, \boldsymbol{\xi}_{ji})}{\sigma_{r_p}^2(\mathbf{p}, \boldsymbol{\xi}_{ji})} \right\|_\delta \tag{12}$$

with

其中

$$r_p(\mathbf{p}, \boldsymbol{\xi}_{ji}) := I_i(\mathbf{p}) - I_j(\omega(\mathbf{p}, D_i(\mathbf{p}), \boldsymbol{\xi}_{ji})) \tag{13}$$

$$\sigma_{r_p}^2(\mathbf{p}, \boldsymbol{\xi}_{ji}) := 2\sigma_I^2 + \left(\frac{\partial r_p(\mathbf{p}, \boldsymbol{\xi}_{ji})}{\partial D_i(\mathbf{p})}\right)^2 V_i(\mathbf{p}) \tag{14}$$

Fig. 4: Statistic normalization: (a) reference image. (b-d): tracked images and inverse variance $\sigma_{r_p}^{-2}$ of the residual. For pure rotation, depth noise has no effect on the residual noise and hence all normalization factors are the same. For $z$ translation depth noise has no effect for pixels in the center of the image, while for $x$ translation it only affects residuals with intensity-gradient in $x$ direction.

图 4：统计归一化：(a) 参考图像。(b–d)：被跟踪图像以及残差的逆方差 $\sigma_{r_p}^{-2}$。对于纯旋转，深度噪声对残差噪声没有影响，因此所有归一化因子都相同。对于 $z$ 方向平移，深度噪声对图像中心的像素没有影响；而对于 $x$ 方向平移，它只影响在 $x$ 方向上有强度梯度的残差。

where $\|\cdot\|_\delta$ is the Huber norm

其中 $\|\cdot\|_\delta$ 是 Huber 范数

$$\|r^2\|_\delta := \begin{cases} \dfrac{r^2}{2\delta} & \text{if } |r| \le \delta \\ |r| - \dfrac{\delta}{2} & \text{otherwise.} \end{cases} \tag{15}$$

applied to the normalized residual. The residual's variance $\sigma_{r_p}^2(\mathbf{p}, \boldsymbol{\xi}_{ji})$ is computed using covariance propagation as described in Sec. 2.3, and utilizing the inverse depth variance $V_i$. Further, we assume Gaussian image intensity noise $\sigma_I^2$. Minimization is performed using iteratively re-weighted Gauss-Newton optimization as described in Sec. 2.2.

它被作用在归一化残差上。残差的方差 $\sigma_{r_p}^2(\mathbf{p}, \boldsymbol{\xi}_{ji})$ 按第 2.3 节所述，用协方差传播计算，并利用逆深度方差 $V_i$。此外，我们假定图像强度噪声为高斯的，方差为 $\sigma_I^2$。最小化采用第 2.2 节所述的迭代重加权高斯–牛顿优化来完成。

In contrast to previous direct methods, the proposed formulation explicitly takes into account varying noise on the depth estimates: This is of particular relevance as for direct, monocular SLAM, this noise differs significantly for different pixels, depending on how long they were visible – which is in contrast to approaches working on RGB-D data, for which the uncertainty on the inverse depth is approximately constant. Figure 4 shows how this weighting behaves for different types of motion. Note that no depth information for the new camera image is available – therefore, the scale of the new image is not defined, and the minimization is performed on $\mathfrak{se}(3)$.

与以往的直接法不同，所提出的表述显式考虑了深度估计上变化的噪声：这一点尤其重要，因为对直接法单目 SLAM 而言，这一噪声会因像素被看到的时间长短而在不同像素上显著不同——这与处理 RGB-D 数据的方法形成对比，后者的逆深度不确定性近似为常数。图 4 展示了这种加权在不同类型运动下的表现。注意，新相机图像没有可用的深度信息——因此新图像的尺度并未定义，最小化是在 $\mathfrak{se}(3)$ 上进行的。

3.4 Depth Map Estimation

3.4 深度图估计

Keyframe Selection. If the camera moves too far away from the existing map, a new keyframe is created from the most recent tracked image. We threshold a weighted combination of relative distance and angle to the current keyframe:

关键帧选择。如果相机离已有地图太远，就从最近跟踪到的图像创建一个新关键帧。我们对到当前关键帧的相对距离与角度的加权组合设阈值：

$$\mathrm{dist}(\boldsymbol{\xi}_{ji}) := \boldsymbol{\xi}_{ji}^T \mathbf{W} \boldsymbol{\xi}_{ji} \tag{16}$$

where $\mathbf{W}$ is a diagonal matrix containing the weights. Note that, as described in the following section, each keyframe is scaled such that its mean inverse depth is one. This threshold is therefore relative to the current scale of the scene, and ensures sufficient possibilities for small-baseline stereo comparisons.

其中 $\mathbf{W}$ 是包含权重的对角矩阵。注意，如下一节所述，每个关键帧都被缩放，使其平均逆深度为 1。因此该阈值是相对于当前场景尺度的，并能保证有足够的机会进行小基线立体比较。

Fig. 5: Direct keyframe alignment on $\mathrm{sim}(3)$: (a)-(c): two keyframes with associated depth and depth variance. (d)-(f): photometric residual, depth residual and Huber weights, before minimization (left), and after minimization (right).

图 5：$\mathrm{sim}(3)$ 上的直接关键帧对齐：(a)–(c) 两个关键帧及其相应的深度和深度方差。(d)–(f) 光度残差、深度残差和 Huber 权重，最小化之前（左）与最小化之后（右）。

Depth Map Creation. Once a new frame is chosen to become a keyframe, its depth map is initialized by projecting points from the previous keyframe into it, followed by one iteration of spatial regularization and outlier removal as proposed in [9]. Afterwards, the depth map is scaled to have a mean inverse depth of one – this scaling factor is directly incorporated into the $\mathrm{sim}(3)$ camera pose. Finally, it replaces the previous keyframe and is used for tracking subsequent new frames.

深度图创建。一旦选定某一新帧作为关键帧，就通过把上一关键帧中的点投影到该帧上来初始化其深度图，再按 [9] 的做法做一次空间正则化和离群点剔除。随后，深度图被缩放，使平均逆深度为 1——这一缩放因子被直接纳入 $\mathrm{sim}(3)$ 相机位姿。最后，它替换上一关键帧，并用于跟踪随后的新帧。

Depth Map Refinement. Tracked frames that do not become a keyframe are used to refine the current keyframe: A high number of very efficient small-baseline stereo comparisons is performed for image regions where the expected stereo accuracy is sufficiently large, as described in [9]. The result is incorporated into the existing depth map, thereby refining it and potentially adding new pixels – this is done using the filtering approach proposed in [9].

深度图精化。未成为关键帧的已跟踪帧被用来精化当前关键帧：在预期立体精度足够高的图像区域，进行大量非常高效的小基线立体比较，具体做法见 [9]。结果被纳入已有深度图，从而精化它，并有可能加入新的像素——这采用 [9] 提出的滤波方法完成。

3.5 Constraint Acquisition: Direct $\mathrm{sim}(3)$ Image Alignment

3.5 约束获取：直接 $\mathrm{sim}(3)$ 图像对齐

Fig. 6: Two scenes with high scale variation. Camera frustums are displayed for each keyframe with their size corresponding to the keyframe's scale.

图 6：两个尺度变化很大的场景。为每个关键帧显示相机视锥，其大小对应于该关键帧的尺度。

Direct Image Alignment on $\mathrm{sim}(3)$. Monocular SLAM is – in contrast to RGB-D or Stereo-SLAM – inherently scale-ambivalent, i.e., the absolute scale of the world is not observable. Over long trajectories this leads to scale-drift, which is one of the major sources of error [23]. Further, all distances are only defined up to scale, which causes threshold-based outlier rejection or parametrized robust kernels (e.g. Huber) to be ill-defined. We solve this by using the inherent correlation between scene depth and tracking accuracy: The depth map of each created keyframe is scaled such that the mean inverse depth is one. In return, edges between keyframes are estimated as elements of $\mathrm{sim}(3)$, elegantly incorporating the scaling difference between keyframes, and, in particular for large loop-closures, allowing an explicit detection of accumulated scale-drift.

$\mathrm{sim}(3)$ 上的直接图像对齐。与 RGB-D 或双目 SLAM 不同，单目 SLAM 在固有意义上是尺度不定的，也即世界的绝对尺度不可观测。在长轨迹上这会导致尺度漂移，它是主要误差来源之一 [23]。此外，所有距离都只定义到相差一个尺度，这使得基于阈值的离群点剔除或带参数的鲁棒核（例如 Huber）定义不良。我们利用场景深度与跟踪精度之间固有的相关性来解决这一问题：每个新创建关键帧的深度图都被缩放，使平均逆深度为 1。作为交换，关键帧之间的边被估计为 $\mathrm{sim}(3)$ 的元素，从而自然纳入关键帧之间的尺度差，并且尤其在大回环时，能够显式检测累积的尺度漂移。

For this, we propose a novel method to perform direct, scale-drift aware image alignment on $\mathrm{sim}(3)$, which is used to align two differently scaled keyframes. In addition to the photometric residual $r_p$, we incorporate a depth residual $r_d$ which penalizes deviations in inverse depth between keyframes, allowing to directly estimate the scaled transformation between them. The total error function that is minimized becomes

为此，我们提出一种在 $\mathrm{sim}(3)$ 上做直接、感知尺度漂移的图像对齐的新方法，用来对齐两个尺度不同的关键帧。除了光度残差 $r_p$ 之外，我们还纳入深度残差 $r_d$，对关键帧之间逆深度的偏差进行惩罚，从而能够直接估计它们之间的带尺度变换。被最小化的总误差函数为

$$E(\boldsymbol{\xi}_{ji}) := \sum_{\mathbf{p} \in \Omega_{D_i}} \left\| \frac{r_p^2(\mathbf{p}, \boldsymbol{\xi}_{ji})}{\sigma_{r_p}^2(\mathbf{p}, \boldsymbol{\xi}_{ji})} + \frac{r_d^2(\mathbf{p}, \boldsymbol{\xi}_{ji})}{\sigma_{r_d}^2(\mathbf{p}, \boldsymbol{\xi}_{ji})} \right\|_\delta, \tag{17}$$

where the photometric residual $r_p^2$ and $\sigma_{r_p}^2$ is defined as in (13)–(14). The depth residual and its variance is computed as

其中光度残差 $r_p^2$ 与 $\sigma_{r_p}^2$ 按 (13)–(14) 定义。深度残差及其方差计算为

$$r_d(\mathbf{p}, \boldsymbol{\xi}_{ji}) := [\mathbf{p}']_3 - D_j([\mathbf{p}']_{1,2}) \tag{18}$$

$$\sigma_{r_d}^2(\mathbf{p}, \boldsymbol{\xi}_{ji}) := V_j([\mathbf{p}']_{1,2})\left(\frac{\partial r_d(\mathbf{p}, \boldsymbol{\xi}_{ji})}{\partial D_j([\mathbf{p}']_{1,2})}\right)^2 + V_i(\mathbf{p})\left(\frac{\partial r_d(\mathbf{p}, \boldsymbol{\xi}_{ji})}{\partial D_i(\mathbf{p})}\right)^2, \tag{19}$$

where $\mathbf{p}' := \omega_s(\mathbf{p}, D_i(\mathbf{p}), \boldsymbol{\xi}_{ji})$ denotes the transformed point. Note that the Huber norm is applied to the sum of the normalized photometric and depth residual – which accounts for the fact that if one is an outlier, the other typically is as well. Note that for tracking on $\mathrm{sim}(3)$, the inclusion of the depth error is required as the photometric error alone does not constrain the scale. Minimization is performed analogously to direct image alignment on $\mathfrak{se}(3)$ using the iteratively re-weighted Gauss-Newton algorithm (Sec. 2.2). In practice, $\mathrm{sim}(3)$ tracking is computationally only marginally more expensive than tracking on $\mathfrak{se}(3)$, as only little additional computations are needed.

其中 $\mathbf{p}' := \omega_s(\mathbf{p}, D_i(\mathbf{p}), \boldsymbol{\xi}_{ji})$ 表示变换后的点。注意 Huber 范数被作用在归一化光度残差与深度残差之和上——这考虑到若其中一个是离群，另一个通常也是。还要注意，在 $\mathrm{sim}(3)$ 上跟踪时，纳入深度误差是必需的，因为单靠光度误差并不能约束尺度。最小化与 $\mathfrak{se}(3)$ 上的直接图像对齐类似，采用迭代重加权高斯–牛顿算法（第 2.2 节）。在实践中，$\mathrm{sim}(3)$ 跟踪在计算上只比 $\mathfrak{se}(3)$ 跟踪略贵，因为只需很少的额外计算。

$^1$ We approximate the gradient of the depth map to be zero, which significantly speeds up the computation.

$^1$ 我们将深度图的梯度近似为零，这显著加快了计算。

Constraint Search. After a new keyframe $\mathcal{K}_i$ is added to the map, a number of possible loop closure keyframes $\mathcal{K}_{j_1}, \ldots, \mathcal{K}_{j_n}$ is collected: We use the closest ten keyframes, as well as a suitable candidate proposed by an appearance-based mapping algorithm [11] to detect large-scale loop closures. To avoid insertion of false or falsely tracked loop closures, we then perform a reciprocal tracking check: For each candidate $\mathcal{K}_{j_k}$ we independently track $\boldsymbol{\xi}_{j_k i}$ and $\boldsymbol{\xi}_{i j_k}$. Only if the two estimates are statistically similar, i.e., if

约束搜索。在新关键帧 $\mathcal{K}_i$ 被加入地图之后，收集若干可能的回环关键帧 $\mathcal{K}_{j_1}, \ldots, \mathcal{K}_{j_n}$：我们使用最近的十个关键帧，以及由基于外观的建图算法 [11] 提出的一个合适候选，以检测大规模回环。为避免插入错误的或跟踪错误的回环，我们随后进行互跟踪检验：对每个候选 $\mathcal{K}_{j_k}$，独立跟踪 $\boldsymbol{\xi}_{j_k i}$ 和 $\boldsymbol{\xi}_{i j_k}$。只有当两个估计在统计上相似时，也即当

$$e(\boldsymbol{\xi}_{j_k i}, \boldsymbol{\xi}_{i j_k}) := (\boldsymbol{\xi}_{j_k i} \circ \boldsymbol{\xi}_{i j_k})^T \left(\boldsymbol{\Sigma}_{j_k i} + \mathrm{Adj}_{j_k i}\boldsymbol{\Sigma}_{i j_k}\mathrm{Adj}_{j_k i}^T\right)^{-1}(\boldsymbol{\xi}_{j_k i} \circ \boldsymbol{\xi}_{i j_k}) \tag{20}$$

is sufficiently small, they are added to the global map. For this, the adjoint $\mathrm{Adj}_{j_k i}$ is used to transform $\boldsymbol{\Sigma}_{i j_k}$ into the correct tangent space.

足够小时，它们才被加入全局地图。为此，使用伴随 $\mathrm{Adj}_{j_k i}$ 把 $\boldsymbol{\Sigma}_{i j_k}$ 变换到正确的切空间。

Convergence Radius for $\mathrm{sim}(3)$ Tracking. An important limitation of direct image alignment lies in the inherent non-convexity of the problem, and hence the need for a sufficiently accurate initialization. While for the tracking of new camera frames a sufficiently good initialization is available (given by the pose of the previous frame), this is not the case when finding loop closure constraints, in particular for large loop closures.

$\mathrm{sim}(3)$ 跟踪的收敛域。直接图像对齐的一个重要局限，在于问题固有的非凸性，因而需要足够准确的初值。跟踪新的相机帧时，可以获得足够好的初值（由上一帧的位姿给出）；但在寻找回环约束时并非如此，尤其是对大回环而言。

One solution for this consists in using a very small number of keypoints to compute a better initialization: Using the depth values from the existing inverse depth maps, this requires aligning two sets of 3D points with known correspondences, which can be done efficiently in closed form using e.g. the method of Horn [13]. Still, we found that in practice the convergence radius is sufficiently large even for large-scale loop closures – in particular we found that the convergence radius can be substantially increased by the following measures:

一种解决办法是用极少数关键点来计算更好的初值：利用已有逆深度图中的深度值，这要求对齐两组对应关系已知的三维点，可以用例如 Horn 的方法 [13] 以闭式高效完成。尽管如此，我们发现在实践中，即使对大规模回环，收敛域也已经足够大——尤其是，我们发现通过以下措施可以显著增大收敛域：

— Efficient Second Order Minimization (ESM) [3]: While our results confirm previous work [17] in that ESM does not significantly increase the precision of dense image alignment, we observed that it does slightly increase the convergence radius.

— 有效二阶最小化（Efficient Second Order Minimization，ESM） [3]：我们的结果印证了已有工作 [17]，即 ESM 并不会显著提高稠密图像对齐的精度，但我们观察到它确实会略微增大收敛域。

— Coarse-to-Fine Approach: While a pyramid approach is commonly used for direct image alignment, we found that starting at a very low resolution of only $20 \times 15$ pixels – much smaller than usually done – already helps to increase the convergence radius.

— 由粗到精的方法： 虽然金字塔方法常用于直接图像对齐，但我们发现，从只有 $20 \times 15$ 像素这种非常低的分辨率开始——远低于通常做法——就已经有助于增大收敛域。

An evaluation of the effect of these measures is given in Sec. 4.3.

对这些措施效果的评估见第 4.3 节。

3.6 Map optimization

3.6 地图优化

The map, consisting of a set of keyframes and tracked $\mathrm{sim}(3)$-constraints, is continuously optimized in the background using pose graph optimization [18]. The error function that is minimized is – in accordance with the left-multiplication convention from Sec. 2.2 – defined by ($W$ defining the world frame)

由一组关键帧和已跟踪的 $\mathrm{sim}(3)$ 约束组成的地图，在后台用位姿图优化持续优化 [18]。被最小化的误差函数——与第 2.2 节的左乘约定相一致——定义为（$W$ 表示世界坐标系）

$$E(\boldsymbol{\xi}_{W1} \ldots \boldsymbol{\xi}_{Wn}) := \sum_{(\boldsymbol{\xi}_{ji},\boldsymbol{\Sigma}_{ji})\in E} (\boldsymbol{\xi}_{ji} \circ \boldsymbol{\xi}_{Wi}^{-1} \circ \boldsymbol{\xi}_{Wj})^T \boldsymbol{\Sigma}_{ji}^{-1} (\boldsymbol{\xi}_{ji} \circ \boldsymbol{\xi}_{Wi}^{-1} \circ \boldsymbol{\xi}_{Wj}). \tag{21}$$

4 Results

4 结果

We evaluate LSD-SLAM both quantitatively on publicly available datasets [25, 12] as well as on challenging outdoor trajectories, recorded with a hand-held monocular camera. Some of the evaluated trajectories are shown in full in the supplementary video.

我们既在公开可用的数据集 [25, 12] 上对 LSD-SLAM 做定量评估，也在用手持单目相机记录的困难户外轨迹上评估。部分被评估的轨迹在补充视频中完整展示。

4.1 Qualitative Results on Large Trajectories

4.1 长轨迹上的定性结果

We tested the algorithm on several long and challenging trajectories, which include many camera rotations, large scale changes and major loop closures. Figure 7 shows a roughly 500 m long trajectory which takes 6 minutes just before and after the large loop closure is found. Figure 8 shows a challenging trajectory with large variations in scene depth, which also includes a loop closure.

我们在若干长且困难的轨迹上测试了该算法，这些轨迹包含多次相机旋转、大幅度尺度变化以及重要回环。图 7 展示了一段大约 500 米、时长 6 分钟的轨迹，分别给出刚找到大回环之前和之后的结果。图 8 展示一条场景深度变化很大、同时也包含回环的困难轨迹。

Fig. 7: Loop closure for a long and challenging outdoor trajectory (after the loop closure on the left, before on the right). Also shown are three selected close-ups of the generated pointcloud, and semi-dense depth maps for selected keyframes.

图 7：一条长且困难的户外轨迹上的回环（左为回环之后，右为回环之前）。同时还给出生成点云的三处特写，以及若干关键帧的半稠密深度图。

Fig. 8: Accumulated pointcloud of a trajectory with large scale variation, including views with an average inverse depth of less than 20 cm to more than 10 m. After the loop closure (top-right), the geometry is consistently aligned, while before (top-left) parts of the scene existed twice, at different scales. The bottom row shows different close-ups of the scene. The proposed scale-aware formulation allows to accurately estimate both fine details and large-scale geometry – this flexibility is one of the major benefits of a monocular approach.

图 8：一条尺度变化很大的轨迹的累积点云，其中既有平均逆深度不足 20 厘米的视图，也有超过 10 米的视图。回环之后（右上），几何被一致对齐；回环之前（左上），场景的某些部分以不同尺度出现了两次。底行给出场景的不同特写。所提出的尺度感知表述能够同时精确估计细小结构和大规模几何——这种灵活性是单目方法的主要优点之一。

4.2 Quantitative Evaluation

4.2 定量评估

We evaluate LSD-SLAM on the publicly available RGB-D dataset [25]. Note that for monocular SLAM this is a very challenging benchmark, as it contains fast rotational movement, strong motion blur and rolling shutter artifacts. We use the very first depth map to bootstrap the system and get the correct initial scale. Table 9 shows the resulting absolute trajectory error, and compares it to other approaches.

我们在公开的 RGB-D 数据集 [25] 上评估 LSD-SLAM。需要注意，对单目 SLAM 而言这是一个非常困难的基准，因为它包含快速转动、强烈运动模糊和卷帘快门伪影。我们用最开始的第一张深度图来启动系统并得到正确的初始尺度。图 9 给出由此得到的绝对轨迹误差，并与其他方法进行比较。

Fig. 9: Results on the TUM RGB-D benchmark [25], and two simulated sequences from [12], measured as absolute trajectory RMSE (cm). For LSD-SLAM, we also show the number of keyframes created. 'x' denotes tracking failure, '-' no available data. For comparison we show respective results from semi-dense mono-VO [9], keypoint-based mono-SLAM [15], direct RGB-D SLAM [14] and keypoint-based RGB-D SLAM [7]. Note that [14] and [7] use depth information from the sensor, while the others do not.

图 9：TUM RGB-D 基准 [25] 以及来自 [12] 的两条仿真序列上的结果，以绝对轨迹 RMSE（厘米）度量。对 LSD-SLAM，我们还给出所创建的关键帧数量。‘x’ 表示跟踪失败，‘-’ 表示没有可用数据。作为比较，我们给出半稠密单目视觉里程计 [9]、基于关键点的单目 SLAM [15]、直接法 RGB-D SLAM [14] 以及基于关键点的 RGB-D SLAM [7] 的相应结果。注意 [14] 和 [7] 使用了传感器的深度信息，而其余方法没有。

Fig. 10: Convergence radius and accuracy of $\mathrm{sim}(3)$ direct image alignment with and without ESM minimization (indicated by light / dark) for a different number of pyramid levels (color). All frames of the respective sequence are tracked on frame 300 (left) and frame 500 (right), using the identity as initialization. The bottom plots show for which frames tracking succeeds; the top plots show the final translational error. ESM and more pyramid levels clearly increase the convergence radius, however these measures have no notable effect on tracking precision: if tracking converges, it almost always converges to the same minimum.

图 10：在使用和不使用 ESM 最小化（分别以浅色 / 深色表示）以及不同金字塔层数（颜色）时，$\mathrm{sim}(3)$ 直接图像对齐的收敛域与精度。相应序列中的所有帧都在第 300 帧（左）和第 500 帧（右）上跟踪，并以单位变换为初值。下方图显示哪些帧跟踪成功；上方图显示最终平移误差。ESM 和更多金字塔层数明显增大了收敛域，但这些措施对跟踪精度没有明显影响：一旦跟踪收敛，几乎总是收敛到同一个极小值。

4.3 Convergence Radius for $\mathrm{sim}(3)$ Tracking

4.3 $\mathrm{sim}(3)$ 跟踪的收敛域

We evaluate the convergence radius on two exemplary sequences, the result is shown in Fig. 10. Even though direct image alignment is non-convex, we found that with the steps proposed in Sec. 3.5, surprisingly large camera movements can be tracked. It can also be observed that these measures only increase the convergence radius, and have no notable effect on tracking precision.

我们在两条示例序列上评估收敛域，结果如图 10 所示。尽管直接图像对齐是非凸的，但我们发现，采用第 3.5 节提出的步骤后，出乎意料地大的相机运动也能被跟踪到。还可以看到，这些措施只增大收敛域，对跟踪精度没有明显影响。

5 Conclusion

5 结论

We have presented a novel direct (feature-less) monocular SLAM algorithm which we call LSD-SLAM, which runs in real-time on a CPU. In contrast to existing direct approaches – which are all pure odometries – it maintains and tracks on a global map of the environment, which contains a pose-graph of keyframes with associated probabilistic semi-dense depth maps. Major components of the proposed method are two key novelties: (1) a direct method to align two keyframes on $\mathrm{sim}(3)$, explicitly incorporating and detecting scale-drift and (2) a novel, probabilistic approach to incorporate noise on the estimated depth maps into tracking. Represented as point clouds, the map gives a semi-dense and highly accurate 3D reconstruction of the environment. We experimentally showed that the approach reliably tracks and maps even challenging hand-held trajectories with a length of over 500 m, in particular including large variations in scale within the same sequence (average inverse depth of less than 20 cm to more than 10 m) and large rotations – demonstrating its versatility, robustness and flexibility.

我们提出了一种新的直接法（无特征点）单目 SLAM 算法，称为 LSD-SLAM，可在 CPU 上实时运行。与已有的直接法——它们都只是纯里程计——不同，它维护并在一张全局环境地图上跟踪，该地图包含带有相应概率半稠密深度图的关键帧位姿图。所提方法的主要组成部分是两个关键新意：（1）一种在 $\mathrm{sim}(3)$ 上对齐两个关键帧的直接方法，显式纳入并检测尺度漂移；（2）一种把估计深度图上的噪声纳入跟踪的新的概率方法。以点云表示时，该地图给出半稠密且精度很高的环境三维重建。我们通过实验表明，即便是长度超过 500 米的困难手持轨迹，该方法也能可靠地跟踪和建图，尤其是同一条序列内包含很大的尺度变化（平均逆深度从不足 20 厘米到超过 10 米）以及大旋转——这展示了它的通用性、鲁棒性和灵活性。

References

参考文献

[1] Achtelik, M., Weiss, S., Siegwart, R.: Onboard IMU and monocular vision based control for MAVs in unknown in- and outdoor environments. In: Intl. Conf. on Robotics and Automation (ICRA) (2011)

[2] Akbarzadeh, A., m. Frahm, J., Mordohai, P., Engels, C., Gallup, D., Merrell, P., Phelps, M., Sinha, S., Talton, B., Wang, L., Yang, Q., Stewenius, H., Yang, R., Welch, G., Towles, H., Nistr, D., Pollefeys, M.: Towards urban 3d reconstruction from video. In: in 3DPVT. pp. 1–8 (2006)

[3] Benhimane, S., Malis, E.: Real-time image-based tracking of planes using efficient second-order minimization (2004)

[4] Comport, A., Malis, E., Rives, P.: Accurate quadri-focal tracking for robust 3d visual odometry. In: Intl. Conf. on Robotics and Automation (ICRA) (2007)

[5] Concha, A., Civera, J.: Using superpixels in monocular SLAM. In: Intl. Conf. on Robotics and Automation (ICRA) (2014)

[6] Eade, E., Drummond, T.: Edge landmarks in monocular slam. In: British Machine Vision Conf. (2006)

[7] Endres, F., Hess, J., Engelhard, N., Sturm, J., Cremers, D., Burgard, W.: An evaluation of the RGB-D slam system. In: Intl. Conf. on Robotics and Automation (ICRA) (2012)

[8] Engel, J., Sturm, J., Cremers, D.: Camera-based navigation of a low-cost quadrocopter. In: Intl. Conf. on Intelligent Robot Systems (IROS) (2012)

[9] Engel, J., Sturm, J., Cremers, D.: Semi-dense visual odometry for a monocular camera. In: Intl. Conf. on Computer Vision (ICCV) (2013)

[10] Forster, C., Pizzoli, M., Scaramuzza, D.: SVO: Fast semi-direct monocular visual odometry. In: Intl. Conf. on Robotics and Automation (ICRA) (2014)

[11] Glover, A., Maddern, W., Warren, M., Stephanie, R., Milford, M., Wyeth, G.: OpenFABMAP: an open source toolbox for appearance-based loop closure detection. In: Intl. Conf. on Robotics and Automation (ICRA). pp. 4730–4735 (2012)

[12] Handa, A., Newcombe, R., Angeli, A., Davison, A.: Real-time camera tracking: When is high frame-rate best? In: European Conference on Computer Vision (ECCV) (2012)

[13] Horn, B.: Closed-form solution of absolute orientation using unit quaternions. Journal of the Optical Society of America (1987)

[14] Kerl, C., Sturm, J., Cremers, D.: Dense visual SLAM for RGB-D cameras. In: Intl. Conf. on Intelligent Robot Systems (IROS) (2013)

[15] Klein, G., Murray, D.: Parallel tracking and mapping for small AR workspaces. In: Intl. Symp. on Mixed and Augmented Reality (ISMAR) (2007)

[16] Klein, G., Murray, D.: Improving the agility of keyframe-based SLAM. In: European Conference on Computer Vision (ECCV) (2008)

[17] Klose, S., Heise, P., Knoll, A.: Efficient compositional approaches for real-time robust direct visual odometry from RGB-D data. In: Intl. Conf. on Intelligent Robot Systems (IROS) (2013)

[18] Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., Burgard, W.: g2o: A general framework for graph optimization. In: Intl. Conf. on Robotics and Automation (ICRA) (2011)

[19] Li, M., Mourikis, A.: High-precision, consistent EKF-based visual-inertial odometry. International Journal of Robotics Research 32, 690–711 (2013)

[20] Newcombe, R., Lovegrove, S., Davison, A.: DTAM: Dense tracking and mapping in real-time. In: Intl. Conf. on Computer Vision (ICCV) (2011)

[21] Pizzoli, M., Forster, C., Scaramuzza, D.: REMODE: Probabilistic, monocular dense reconstruction in real time. In: Intl. Conf. on Robotics and Automation (ICRA) (2014)

[22] Schöps, T., Engel, J., Cremers, D.: Semi-dense visual odometry for AR on a smartphone. In: Intl. Symp. on Mixed and Augmented Reality (ISMAR) (2014)

[23] Strasdat, H., Montiel, J., Davison, A.: Scale drift-aware large scale monocular slam. In: Robotics: Science and Systems (RSS) (2010)

[24] Stühmer, J., Gumhold, S., Cremers, D.: Real-time dense geometry from a handheld camera. In: Pattern Recognition (DAGM) (2010)

[25] Sturm, J., Engelhard, N., Endres, F., Burgard, W., Cremers, D.: A benchmark for the evaluation of RGB-D SLAM systems. In: Intl. Conf. on Intelligent Robot Systems (IROS) (2012)