LSDSLAM算法解析

在阅读本文之前，最好先看过《视觉SLAM十四讲》中直接法(chap 8)和单目稠密建图(chap 13)部分，并对李群李代数和立体匹配相关知识有一些了解，这对于理解LSD-SLAM算法有一定帮助

LSD-SLAM算法主要在以下两个论文中提出：
[1] 2013 Semi-dense Visual Odometry for a Monocular Camera
[2] 2014 LSD-SLAM: Large-Scale Direct Monocular SLAM
另外，以下是本文参考的比较好的LSD-SLAM算法解析的链接：
[3] LSD-SLAM笔记之SE3Tracking
[4] LSD-SLAM笔记之DepthMap
[5] LSD-SLAM笔记之一致性约束
[6] SVO & LSD_SLAM(贺一家)视频

LSD-SLAM可以分为三大模块：Tracking、Depth Map Estimation和Map Optimization，如下图所示。三个部分分别位于论文[2]中3.3，3.4和第3章其余部分。但Depth Map Estimation部分详细的算法是在[1]中介绍的。Map Optimization部分是整个算法的核心。下面我们来分别看一下这三大模块。

1 Tracking

Tracking部分比较简单，就是《十四讲》中讲的直接法，每当获取一张新的帧时，就对它进行Tracking。直接法即最小化光度误差，论文进行了一些小的改进，如使用的是方差归一化后的光度误差（variance-normalized photometric error）：

\[ E_p(\mathbf\xi_{ji}) =\sum_{\mathbf{p}\in\Omega_{D_i}} \Biggl\|\frac{r_p^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2}\Biggr\|_\delta \tag{1} \]

\[ r_p(\mathbf{p},{\mathbf\xi_{ji}}) := I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji})) \tag{2} \]

\[ \sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2 := 2\sigma_I^2 + (\frac{\partial{r_p(\mathbf{p}, \mathbf\xi_{ji})}}{\partial{D_i(\mathbf{p})}})^2V_i(\mathbf{p}) \tag{3} \]
可以看出改进的地方就是在分母部分加了个方差（方差的推导可以参考[3]），然后还使用了Huber核函数，总之目的就是为了更好更快地解求位姿。需要注意的是Tracking部分求的是\(\mathfrak{se}(3)\)上的位姿。

2 Depth Map Estimation

这部分主要是用Tracking跟踪后的帧更新或构建深度图，分两种情况：

• 构建关键帧时，则构建新的深度图（Depth Map Creation）
• 不构建关键帧时，则更新当前关键帧的深度图（Depth Map Refinement）

其中，是否构建关键帧是通过计算当前帧与当前关键帧的距离判断的，大于一定距离时才构建新的关键帧。距离定义如下，\(\mathbf W\)为对角阵，目的是加权：

\[ dist(\mathbf\xi_{ji}):=\mathbf\xi_{ji}^T \mathbf W \mathbf\xi_{ji} \tag{4} \]

在进一步介绍之前，有必要重申一下一些定义：关键帧（key frame）是相隔一定距离的具有代表性的帧，每一个关键帧对应一个深度图；参考帧（reference frame）是用于和当前帧做立体匹配的帧，一个关键帧后面一般有许多参考帧。接下来就分别介绍一下上述的两种情况。

2.1 深度图的更新

当我们获得一个新的帧，并且判断不构建新的关键帧，则进行当前关键帧深度图的更新过程。这部分论文中称为基于立体匹配的深度更新（Stereo-Based Depth Map Update），具体可分为以下四个步骤：

• 根据一些标准选择出“好的”像素
• 为每个像素自适应选择最合适的参考帧
• 极线上进行立体匹配
• 深度观测融合

原论文打乱了顺序，这里我们按上述顺序依次介绍。

2.1.1 选择出“好的”像素

这部分论文中称为不确定性估计（Uncertainty Estimation），主要通过建模（对逆深度的不确定度进行估计），并以此为依据挑选出好的像素。我们知道在直接法中，逆深度与四个变量有关，可以表示成它们的的函数：

\[ d^*=d(I_0,I_1,\xi,\pi)\tag{5} \]

其中\(I_0\)，\(I_1\)分别为参考帧和当前帧两个图像，