在阅读本文之前，最好先看过《视觉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\)分别为参考帧和当前帧两个图像，\(\xi\)表示两帧间位姿变换，\(\pi\)为相机内参。为进一步分析各个变量的影响，将整个立体匹配过程分为三步：
1.计算在参考帧中的极线
2.在极线上找到最好的匹配位置\(\lambda^*\in\mathbb{R}\)(视差)
3.通过\(\lambda^*\)求出逆深度\(d^*\)

这三个步骤分别涉及三个误差：几何视差误差，由\(\xi\)和\(\pi\)中的噪声将影响第１步极线的位置，从而导致匹配点位置的误差；光度视差误差，在图像\(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\)具体计算公式推导可以参考[4]。考虑到在极线上搜索匹配点的时候，是使用了多个点，因此这里给出了逆深度误差的上限：
\[ \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 为每个像素自适应选择参考帧

挑选好像素后，需要为每个像素找合适的参考帧。为什么需要找合适的参考帧？因为我们知道在立体视觉中：

• 小基线，准确度accuracy高，精度precision低
• 大基线，准确度accuracy低(有局部最小值，易误匹配)，精度precision高

因此，必须尽可能选择距离当前帧较远的帧作为参考帧，但是当极线上超过一定距离还没有匹配到，就需要退而求其次，选择距离较近的、新获得的这些帧作为参考帧。论文提出一种自适应的方法，下面使用例子来说明这个过程：

• 当前0s我们获得一个帧（current frame）
• 直接法估计了位姿，并挑选了一些好的像素，如右上角图所示
• 找最老的帧，比如直接从当前的关键帧开始匹配，但可能没有匹配到，故使用比较新的帧来做极线搜索，结果找到了-4.8s的帧，那么这一小部分像素的参考帧就是-4.8s时的这个帧了。这部分像素因为跨度比较大，因此像素年龄最大，立体观测的精度应该也是最高的。
• 但是别的部分的像素还没找到属于自己的参考帧，因此退而求其次，找更新一点的帧，结果找到了-3.9s的帧
• 因为时间越近的帧差别越小，如-0.4s的帧和0s的帧差异很小，所以一定会匹配成功（短基线准确度高），只是精度没那么高罢了。所以反复进行上面的过程直到所有好的像素都找到对应的参考帧。
• 计算深度并进一步更新地图。

2.1.3 立体匹配策略

立体匹配是在当前帧（current frame）和参考帧（reference frame）之间进行的，而不是关键帧（key frame），不过关键帧也可以作为一部分像素的参考帧。
在立体匹配的过程中，论文采用了极线段上５个采样点计算SSD误差的方式。采用５个点主要的方式很大程度上提高了匹配的效率。由于这５个点是相邻的，在极线段上移动的时候，每次只需要更新一个点的值，这就非常高效了。在立体匹配的误差计算方式上有很多中方法，论文中使用了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 深度图的传播

在构建新的关键帧时，进行深度图的传播。假设两帧间的旋转是很小的，逆深度就可以近似为：
\[ 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\)的为预测不确定性。需要注意代码中在实际求逆深度的时候，是考虑旋转的，把参考关键帧上的点通过\(\mathfrak{se}(3)\)变换到当前新的关键帧上来，然后求逆深度。但是求方差则是用上述近似方法求的。
一个像素点只能有一个逆深度的先验值，在得到一个新的观测值的时候，根据方差来判断对新的观测值融合或者舍弃，论文中称这部分为Collision handling：

• 当两个逆深度观测值的差值小于\(2\sigma\)的时候，则认为观测有效，根据(22)融合
• 否则，离相机较远的这个深度估计认为是被遮挡（occluded）了的（逆深度假设无效），并且舍弃该逆深度信息。

2.3 其他

论文还进行了一些其他处理，如把每个像素与其周边的加权深度作为改点的深度值，假如两个邻接深度之间的差值远大于\(2\sigma\)，他们便不做这个处理。处理之后，各自的方差不变。这里其实是一个Edge-preserving smoothing，也叫地图的正则化。另外，论文中还用了outlier removal。

3 Map Optimization

这部分在论文中叫建图一致性约束（constraint acquisition），是算法的核心（解决尺度问题）。因为长距离会出现尺度漂移，因此建图一致性约束也就是做闭环检测和全局优化。我们先看一下关键帧之间的\(\mathfrak{sim}(3)\)位姿估计。
首先，将每个关键帧地图的平均逆深度归一化为１。然后最小化误差函数来对齐不同的关键帧，误差函数如下，其实相比\(\mathfrak{se}(3)\)跟踪时的误差项多了一个尺度项：
\[ 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)\)跟踪是一样的:
\[ r_p(\mathbf{p},{\mathbf\xi_{ji}}) := I_i({\mathbf{p}}) - I_j(\omega({\mathbf{p}}, D_i({\mathbf{p}}), \mathbf\xi_{ji})) \tag{26} \]

\[ \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{27} \]

以及深度残差和方差定义如下，可以看做深度图的光度残差：

\[ r_d(\mathbf{p},\mathbf\xi_{ji}) = [\mathbf{p}']_3-D_j([\mathbf{p}']_{1,2}) \tag{28} \]

\[ \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{29} \]

这里的\(\mathbf{p}':=\omega_s(\mathbf{p},D_i(\mathbf{p}),\mathbf\xi_{ji})\)表示从图像帧\(i\)变换到图像帧\(j\)上的点。这里在求解\(\mathfrak{sim}(3)\)的时候也是同求\(\mathfrak{se}(3)\)一样，使用了迭代变权重高斯牛顿算法。此外，同样也使用了Huber核函数。
下面看一下回环检测部分。由于论文中采用的是直接法，虽然代码中有fabmap检测闭环的部分，但是其默认检测闭环的方式是帧与帧之间做双向跟踪（Reciprocal tracking check）。首先搜索最近的10个关键帧及一些外观较像的帧作为候选帧，对每一个候选帧都计算其与当前关键帧彼此跟踪的\(\mathfrak{sim}(3)\)，然后计算两者间的马氏距离的平方：
\[ \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{30} \]

详细的介绍可以参考[5]。当距离足够小时表明相似度较高，就将这一帧插入全局地图中。最后执行图优化（g2o中的pose graph optimization），边为连接关系，节点为关键帧，即优化：
\[ 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{31} \]

在大的回环检测时，为了更好地优化、增大收敛半径，论文提出可以使用关键点提供初始值、高效二阶最小化（Efficient Second Order Minimization，ESM）和由粗到细（Coarse-to-Fine）这三种方法，这里就不再详细介绍了。

个人理解错误的地方还请不吝赐教，转载请标明出处