在阅读本文之前，最好先看过《视觉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
论文[2]提出LSD-SLAM，可分为三大模块：Tracking、Depth Map
Estimation和Map Optimization，如下图所示。其中Depth Map
Estimation部分详细的算法是在文献[1]中介绍的。 
0 Initialization
在这三大模块之前，必然是先初始化。初始化就是选一个关键帧，然后设置随机的深度，方差设置为一个较大的值。然后给予相机充分的运动，不断更新深度，直到收敛。这部分具体原理下面第2节会提到，可以先跳过。
初始化后，就可以开始下面三大模块了。
1 Tracking
Tracking部分比较简单，就是《十四讲》中讲的直接法，即最小化光度误差。每当获取一张新的帧时，就把它和当前的关键帧进行\(\mathfrak{se}(3)\)上的配准。先看下下面的公式，其中一个关键帧可以表示为\(，K_i=(I_i, D_i, V_i)\)其中\(I_i\)表示图像，\(D_i\)表示其逆深度图，\(V_i\)表示逆深度图方差： \[ 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} \] 由于不同像素、不同深度、不同角度的不确定性是不同的，在计算光度误差时，如果按照一样的权重计算也是不合适的。这个和单目SLAM、RGBDSLAM是不同的，因为论文认为直接法SLAM中不同点的深度估计不确定性差异很大：如果一个点经常被观测到，深度的不确定性就会很小，反之就会很大。因此必须在tracking时考虑到这个误差。
因此，LSDSLAM使用方差归一化后的光度误差（variance-normalized photometric error）。这个方差\(\sigma_{r_p(\mathbf{p},\mathbf\xi_{ji})}^2\)可以通过高斯分布的方差传播（也就是误差传播定律）计算得到，由公式(3)可以看出，该方差与两个图像的灰度方差\(\sigma_I^2\)和逆深度方差计算得到。整个LSDSLAM中，我觉得很重要的一个地方就是对误差的不确定性建模和分析，下面会看到其他地方也用到了精细的误差建模。这块也是该论文的创新点之一。
定义好了误差项，然后就可以去优化求解了。为了抗粗差，本文优化时使用了Huber核函数。同时，还使用了迭代变权高斯牛顿法去做优化，对残差大的项降低权重。
2 Depth Map Estimation
这部分主要是用Tracking跟踪后的帧更新或构建深度图，分两种情况： * 不构建关键帧时，则更新当前关键帧的深度图（Depth Map Refinement） * 构建关键帧时，则构建新关键帧的深度图（Depth Map Creation）
其中，是否构建关键帧是通过计算当前帧与当前关键帧的距离判断的，大于一定距离时才构建新的关键帧。距离定义如下，\(\mathbf W\)为对角阵，目的是加权：
\[ dist(\mathbf\xi_{ji}):=\mathbf\xi_{ji}^T \mathbf W \mathbf\xi_{ji} \tag{4} \]
在进一步介绍之前，有必要重申一下一些定义：关键帧（key frame）是相隔一定距离的具有代表性的帧，每一个关键帧对应一个深度图；参考帧（reference frame）是用于和当前帧做立体匹配的帧，一个关键帧后面一般有许多参考帧。接下来就分别介绍一下上述的两种情况。
2.1 深度图的更新 Depth Map Refinement
当我们获得一个新的帧，并且判断不构建新的关键帧，则进行当前关键帧深度图的更新过程。这部分论文中称为基于立体匹配的深度更新（Stereo-Based Depth Map Update），具体可分为以下四个步骤： * 根据一些标准选择出“好的”像素 * 为每个像素自适应选择最合适的参考帧 * 极线上进行立体匹配 * 深度观测融合
原论文打乱了顺序，这里我们按上述顺序依次介绍。
2.1.1 选择出“好的”像素
这部分论文中称为不确定性估计（Uncertainty Estimation），主要通过建模（对逆深度的不确定度进行估计），并以此为依据挑选出好的像素。我们知道在直接法中，逆深度与四个变量有关，可以表示成它们的的函数：
\[ d^*=d(I_0,I_1,\xi,\pi)\tag{5} \]
其中\(I_0\)，\(I_1\)分别为参考帧和当前帧两个图像，\(\xi\)表示两帧间位姿变换，\(\pi\)为相机内参。为进一步分析各个变量的影响，将整个立体匹配过程分为三步： 1.计算在参考帧中的极线 2.在极线上找到最好的匹配位置\(\lambda^\*\in\mathbb{R}\)(视差) 3.通过\(\lambda^\*\)求出逆深度\(d^\*\)
这三个步骤分别涉及三个误差：几何视差误差，由$\(和\)\(中的噪声将影响第１步极线的位置，从而导致匹配点位置的误差；**光度视差误差**，在图像\)I_0\(和\)I_1$上的噪声将影响第２步匹配位置的求取；逆深度计算误差，逆深度误差除了与上述两个误差有关外，还与基线、像素位置等有关。接下来将对这几个误差一一建模分析。
a.几何视差误差
几何视差误差（Geometric disparity error），记为\(\epsilon_\lambda\)。这一部分论文从几何和代数上分析了几何视差误差的具体形式。 首先我们从代数上分析。理论上，几何视差误差可以准确地计算出，但计算复杂度较大，不够高效，故使用了近似的误差模型进行了代替。首先，极线可定义为： \[ L:=\begin{Bmatrix}l_0+\lambda\begin{pmatrix}l_x\\l_y\end{pmatrix}\mid\lambda\in S\end{Bmatrix}\tag{6} \]
其中，\(\lambda\)为视差，\(l:=(l_x,l_y)^T\)是归一化的极线方向向量，\(l_0\)是极线上对应无穷远点的图像点（但我认为当作极点更容易理解，不影响结果，只是视差的具体数值不一样）。设我们要解求的点在极线和灰度等值线（isocurve）的交点上，在局部我们将等值线近似为直线，同时梯度方向近似不变，则有： \[ l_0+\lambda\begin{pmatrix}l_x\\l_y\end{pmatrix}\stackrel{!}＝g_0+\gamma\begin{pmatrix}-g_y\\g_x\end{pmatrix} \quad\quad \gamma\in\mathbb{R}\tag{7} \]
这里\(g_0\)为等值线上的一点，\(g_0\)处的灰度值等于或接近待匹配像素的灰度值。\(g:=(g_x,g_y)^T\)表示归一化梯度方向向量，则\((-g_y,g_x)^T\)表示归一化等值线方向向量。故等式左边是沿极线搜索到的匹配点坐标，等式右边表示该点位于等值线上。由于下面会分析图像噪声的影响，所以这里假设\(g_0\)和\(g\)不受噪声影响。现在用这些相关量将\(\lambda^\*\)表示出来(将式(7)两边同左乘\(g^T\)并移项得): \[ \lambda^*(l_0)=\frac{\langle g,g_0-l_0 \rangle}{\langle g,l \rangle}\tag{8} \]
由高斯分布方差传播可得： \[ \sigma_{\lambda(\xi,\pi)}^2=J_{\lambda^*(l_0)}\begin{pmatrix}\sigma_l^2&0\\0&\sigma_l^2\end{pmatrix}J_{\lambda^*(l_0)}^{T} \tag{9} \]
由式(8)得： \[ J_{\lambda^*(l_0)}=\frac{\partial\lambda^*(l_0)}{\partial l_0}=-\frac{g^T}{\langle g,l \rangle} \tag{10} \]
代入式(9)得： \[ \sigma_{\lambda(\xi,\pi)}^2=\frac{\langle g,g\rangle\cdot\sigma_l^2}{\langle g,l\rangle^2}=\frac{\sigma_l^2}{\langle g,l\rangle^2} \tag{11} \]
其中，\(\sigma_{l}^2\)为极线位置误差\(\epsilon_l\)的方差（\(\epsilon_l\)可以看作位姿扰动的影响，其示意图见下面）。总之，从式(11)我们可以得出以下结论： * 位姿扰动造成的误差\(\sigma_{l}^2\)越大，则几何视差误差越大 * 梯度\(g\)和极线\(l\)的夹角越小（内积越大），几何视差误差越小
代数上分析完后，再从几何上分析一下，毕竟数形结合才能更好理解，否则这个地方理解起来有点抽象。如下图所示，\(P\_0\)是假设没有误差时匹配到的点，\(L'\)是无误差时的极线。但是由于位姿扰动影响，在图上表现为\(P\_0\)偏移了\(\epsilon\_l\)这么远（论文认为位姿扰动只影响极点位置计算，但极线方向不受影响，所以极点\(l\_0\)等极线上的点都偏移了这么远），论文假设\(l\_0\)受各向同性的高斯噪声，因此\(P\_0\)偏移后的位置可能出现在蓝色虚圆的任意一点\(P\_1\)。偏移后极线为\(L\)，虚直线为等值线。那么现在，在极线\(L\)上要匹配到同样灰度值的点，就得多走\(\epsilon\_\lambda\)这么一段距离，这就是几何视差误差。
从图上我们可以清晰看出梯度方向与极线方向夹角大小对几何视差误差的影响，可以得出和上面代数分析部分一样的两个结论。
b.光度视差误差
光度视差误差（Photometric disparity error）这一部分论文也从几何和代数上分析了几何视差误差的具体形式。首先我们从代数上分析。在立体匹配时我们使用SSD误差（见2.1.3节），即： \[ \lambda^*＝\min_{\lambda}(i_{ref}-I_p(\lambda))^2 \tag{12} \]
其中\(i_{ref}\)是参考帧像素灰度，\(I_p(\lambda)\)是极线上视差\(\lambda\)处的像素灰度。假设实际中可以获得一个比较好的初值\(\lambda_0\)，则将\(I_p(\lambda)\)一阶泰勒展开： \[ I_p(\lambda)\approx I_p(\lambda_0)+g_p\Delta \lambda \tag{13} \]
则误差函数变为： \[ \lambda^*＝\min_{\lambda}(i_{ref}-I_p(\lambda_0)-g_p\Delta\lambda)^2 \tag{14} \]
可以解出\(\Delta\lambda\)，则有： \[ \lambda^*(I)=\lambda_0+\Delta\lambda=\lambda_0+(i_{ref}-I_p(\lambda_0))g_p^{-1} \tag{15} \]
注意这里\(g_p\)是图像\(I_p\)极线上的梯度，因此是一维的。这里同样不考虑梯度的噪声，只考虑两个图像的噪声，则有： \[ \sigma_{\lambda(I)}^2=J_{\lambda^*(I)}\begin{pmatrix}\sigma_i^2&0\\0&\sigma_i^2\end{pmatrix}J_{\lambda^*(I)}^T=\frac{2\sigma_i^2}{g_p^2} \tag{16} \]
这里由于同时对关键帧和参考帧的灰度都计算了噪声，二者误差相互独立，因此会有２这个系数。故也可以这么求： \[ \sigma_{\lambda(I)}^2=\text{Var}(\lambda^*(I))=(\text{Var}(i_{ref})+\text{Var}(I_p))g_p^{-2}=\frac{2\sigma_i^2}{g_p^2} \tag{17} \]
其中\(\sigma_i^2\)是图像的高斯噪声的方差。从上式可得结论： * 图像噪声越大，光度视差误差越大 * 极线方向上梯度越大，光度视差误差越小
代数上分析完后，再从几何上分析一下。如下图所示，比较直观。直线斜率的绝对值表示极线上梯度大小，当梯度值越大时，可以看出光度视差误差越小。直接法因为是靠图像梯度来不断调整位姿的，因此梯度必须较大，这样才能在优化中较快较好地收敛。这部分内容在《十四讲》中也有提到。
c.逆深度计算误差
这部分主要是讲基线、像素位置等对逆深度误差的影响。首先，视差\(\lambda\)越大，我们知道物体的深度越小，那么逆深度越大。论文中认为当旋转角度较小时，\(\lambda\)和\(d\)近似成正比例关系。而\(\lambda\)误差方差可以表示为几何视差误差和光度视差误差方差之和（因为两者独立），故就有下面的式子： \[ \sigma_{d,\text{obs}}^2=\alpha^2\begin{pmatrix}\sigma_{\lambda(\xi,\pi)}^2+\sigma_{\lambda(I)}^2\end{pmatrix}\tag{18} \]
其中\(\alpha\)是权重。更准确说，\(\alpha\)是当前像素的深度观测值不确定性，其定义如下： \[ \alpha:=\frac{\partial{d}}{\partial{\lambda}} \tag{19} \]
其中分子为逆深度搜索范围，分母为极线段搜索范围。如果一个小的极线上的变化会导致深度变化大，那就是说此时不确定性大，故权重大一些。这个概念我们在学习三角化时早已领略过：即平移太小，三角化的精度不够。因此，平移小的像素，就让它误差的权重大一些，我们不太想要这种像素。\(\alpha\)具体计算公式推导可以参考其他博客。考虑到在极线上搜索匹配点的时候，是使用了多个点，因此这里给出了逆深度误差的上限： \[ \sigma_{d,\text{obs}}^2\le\alpha^2\begin{pmatrix}\text{min}[\sigma_{\lambda(\xi,\pi)}^2]+\text{min}[\sigma_{\lambda(I)}^2]\end{pmatrix} \tag{20} \]
总结一下：几何视差误差与梯度方向有关，光度视差误差与梯度大小有关，权重\(\alpha\)与基线长度焦距像素位置等有关。这三个因素共同影响立体观测精度，我们可以据此只把好的像素的观测结果保留，并利用计算得到的方差进行深度图的融合更新（见2.1.4节）。 #### 2.1.2 为每个像素自适应选择参考帧 挑选好像素后，需要为每个像素找合适的参考帧。为什么需要找合适的参考帧？因为我们知道在立体视觉中： * 小基线，匹配的准确度高，三角化的精度低 * 大基线，匹配的准确度低(有局部最小值，易误匹配)，三角化的精度高
因此，必须尽可能选择距离当前帧较远的帧作为参考帧，但是当极线上超过一定距离还没有匹配到，就需要退而求其次，选择距离较近的、新获得的这些帧作为参考帧。论文提出一种自适应的方法，下面使用例子来说明这个过程： * 当前0s我们获得一个帧（current frame） * 直接法估计了位姿，并挑选了一些好的像素，如右上角图所示 * 找最老的帧，比如直接从当前的关键帧开始匹配，但可能没有匹配到，故使用比较新的帧来做极线搜索，结果找到了-4.8s的帧，那么这一小部分像素的参考帧就是-4.8s时的这个帧了。这部分像素因为跨度比较大，因此像素年龄最大，立体观测的精度应该也是最高的。 * 但是别的部分的像素还没找到属于自己的参考帧，因此退而求其次，找更新一点的帧，结果找到了-3.9s的帧 * 因为时间越近的帧差别越小，如-0.4s的帧和0s的帧差异很小，所以一定会匹配成功（短基线准确度高），只是精度没那么高罢了。所以反复进行上面的过程直到所有好的像素都找到对应的参考帧。 * 计算深度并进一步更新地图。
2.1.3 立体匹配策略
在立体匹配的过程中，LSDSLAM选择极线上５个连续采样点进行匹配。如果具有深度先验信息，可以限制在\(2\sigma\)对应的极线段内搜索，缩小搜索范围。由于这５个点是相邻的，在极线段上移动的时候，每次只需要更新一个点的值，这样比常规匹配一个patch更高效。立体匹配的相似度标准有很多，论文中使用了SSD（Sum of Squared Distance）误差： \[ E_{SSD}(u)=\sum_{i}[I_1(x_i+u)-I_0(x_i)]^2 \tag{21} \]
2.1.4 深度观测融合
即把当前观测的深度融合到关键帧的深度地图中去。这里有两种情况：当对应像素点没有深度先验时则由新的观测值构建新的先验；当已经有先验值的话，则把新观测值融合到先验中去。在这个融合的过程，对应卡尔曼滤波中更新的过程，即使用两个高斯分布乘法的方式，在《十四讲》chap 13中也有介绍。对于给定先验\(\mathcal{N}(d_p,\sigma_p^2)\)以及有噪声的观测值\(\mathcal{N}(d_o,\sigma_o^2)\)，具体后验估计公式如下： \[ \mathcal{N}\begin{pmatrix}\frac{\sigma_p^2 d_o + \sigma_o^2d_p}{\sigma_p^2 + \sigma_o^2}, \frac{\sigma_p^2\sigma_o^2}{\sigma_p^2 + \sigma_o^2}\end{pmatrix}\tag{22} \]
2.2 深度图的构建 Depth Map Creation
在构建新的关键帧时，需要构建新关键帧的深度图，其实是把上一个关键帧的深度图传递（或叫传播）到当前帧。首先需要把上一个关键帧的地图点投影到当前关键帧，然后对于地图点的不确定性，不能再使用之前的了，需要更新一下。这里采用近似方式计算，如公式(24)所示：
假设两帧间的旋转是很小的，逆深度就可以近似为： \[ d_1(d_0)=(d_0^{-1}-t_z)^{-1} \tag{23} \]
这里的\(t_z\)是相机沿着光轴方向的位移，同样根据协方差传播率则有： \[ \sigma_{d_1}^2=J_{d_1}\sigma_{d_0}^2J_{d_1}^T+\sigma_p^2=\begin{pmatrix}\frac{d_1}{d_0}\end{pmatrix}^4\sigma_{d_0}^2+\sigma_p^2 \tag{24} \]
这里\(\sigma_p^2\)的为预测不确定性，就像卡尔曼滤波中的预测白噪声一样。
但是有可能地图点投影时，两个地图点投影到当前关键帧的一个像素上了，即发生了碰撞冲突。LSDSLAM采用以下策略处理：
- 当两个地图点的逆深度观测值的差值小于\(2\sigma\)的时候，则认为观测有效，根据(22)融合
- 否则，离相机较远的这个深度估计认为是被遮挡了的，并且舍弃该逆深度信息。
传播到新关键帧后，将深度图缩放到平均的逆深度为单位1。不同关键帧深度图之间的尺度是由第3节恢复的，现在先忽略。
2.3 其他
论文还进行了一些其他处理，如把每个像素与其周边的加权深度作为改点的深度值，假如两个邻接深度之间的差值远大于\(2\sigma\)，他们便不做这个处理。处理之后，各自的方差不变。这里其实是一个Edge-preserving smoothing，也叫地图的正则化，目的是为了使深度图更加平滑。另外，论文中还用了outlier removal，这些就不介绍了。
3 Map Optimization
3.1 帧间对齐与尺度恢复
这部分在论文中叫建图一致性约束（constraint acquisition），目的就是为了解决单目尺度漂移问题。因为我们之前2.2节提到每个关键帧对应的深度图的平均逆深度为单位1，因此需要帧间需要做一个\(\mathfrak{sim}(3)\)的对齐，才能恢复和保证不同关键帧之间的相对尺度。与Tracking中类似，只不过对齐时的误差函数除了光度项，还加上了尺度项（或叫深度项）\(r_d^2(\mathbf{p},\mathbf\xi_{ji})\)： \[ E(\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} + \frac{r_d^2(\mathbf{p},\mathbf\xi_{ji})}{\sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2} \Biggr\|_\delta \tag{25} \]
这里的光度残差和方差的定义和\(\mathfrak{se}(3)\)跟踪是一样的，见上文公式(1)-(3)。而深度项误差函数和其方差定义如下： \[ r_d(\mathbf{p},\mathbf\xi_{ji}) = [\mathbf{p}']_3-D_j([\mathbf{p}']_{1,2}) \tag{26} \]
$$ \sigma_{r_d(\mathbf{p},\mathbf\xi_{ji})}^2 = V_j([\mathbf{p}']_{1,2})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_j([\mathbf{p}']_{1,2})}\end{pmatrix} + V_i(\mathbf{p})\begin{pmatrix}\frac{{\partial}r_d(\mathbf{p},\mathbf\xi_{ji})}{{\partial}D_i(\mathbf{p})}\end{pmatrix} \tag{27} $$其中\(\mathbf{p}':=\omega\_s(\mathbf{p},D\_i(\mathbf{p}),\mathbf\xi\_{ji})\)表示从图像帧\(i\)变换到图像帧\(j\)上的地图点。由于不同地图点的不确定性不同，在计算深度误差时，也进行了方差归一化加权。由公式(27)可以看出，深度项的方差取决于两个关键帧逆深度图的方差。
定义完了误差函数，就可以解算了。求解时同样使用了Huber核函数和迭代变权高斯牛顿算法。
3.2 位姿图优化
上面说完了理论，现在才到了真正的地图优化部分，这里用的就是我们熟知的位姿图优化。当一个关键帧被新的关键帧取代后，它的深度图不会再被更新，这时把该关键帧的深度图（即局部地图）加入到全局地图中。然后，需要做位姿图优化。位姿图优化的概念，已经成为许多SLAM系统的标配，在做优化之前，必须进行回环检测。
空间上，LSDSLAM选取与该关键帧最近的10个关键帧（包括直接相邻的上一帧）；外观上，使用fabmap寻找相似的关键帧。然后将这些关键帧与当前关键帧进行相似性的比较。我们知道，回环检测对于准确率的要求比召回率高。因此，使用双向跟踪（Reciprocal tracking check）判断两帧之间的\(\mathfrak{sim}(3)\)的马氏距离平方是否足够小，如果足够小，表示找到一个回环！这里计算\(\mathfrak{sim}(3)\)帧间位姿的计算就是采用3.1节的方法。马氏距离平方公式如下： $$ \begin{equation} e\left(\mathbf\xi_{j_k i}, \mathbf\xi_{i j_k}\right) := \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)^T \left(\mathbf\Sigma_{j_k i} + \text{Adj}_{j_k i}\mathbf\Sigma_{i j_k}\text{Adj}_{j_k i}^T\right)^{-1} \left(\mathbf\xi_{j_k i} \circ \mathbf\xi_{i j_k}\right)\label{eq:mdis} \end{equation} \tag{28} $$
回环约束找到后，就可以执行\(\mathfrak{sim}(3)\)上的位姿图优化了，即优化： \[ E(\xi_{W1}\cdots\xi_{Wn})=\sum_{(\xi_{ji},\Sigma)\in\varepsilon}(\xi_{ji}\circ\xi_{Wi}^{-1}\circ\xi_{Wj})^T\Sigma_{ji}^{-1}(\xi_{ji}\circ\xi_{Wi}^{-1}\circ\xi_{Wj}) \tag{29} \]
在大的回环检测时，为了更好地优化、增大收敛半径，论文提出可以使用关键点提供初始值、高效二阶最小化（Efficient Second Order Minimization，ESM）和由粗到细（Coarse-to-Fine）这三种方法，这里就不再详细介绍了。
个人理解错误的地方还请不吝赐教，转载请标明出处