SLAM | 视觉SLAM中直接法开源算法：LSD-SLAM、DSO
点击上方“AI算法修炼营”，选择加星标或“置顶”
标题以下，全是干货
前面的话
VSLAM 是利用多视图几何理论，根据相机拍摄的图像信息对相机进行定位并同时构建周围环境地图。VSLAM 前端为视觉里程计和回环检测，相当于是对图像数据进行关联；后端是对前端输出的结果进行优化，利用滤波或非线性优化理论，得到最优的位姿估计和全局一致性地图。
前面已经介绍了系列一：VSLAM的前端：视觉里程计和回环检测，系列二：VSLAM中的后端优化和建图，系列三：VSLAM中特征点法开源算法：PTAM和ORB-SLAM。
在前面的系列文章中，我们也介绍过特征点法与直接法的区别。并且我们知道，直接法不依赖特征的提取和匹配，直接通过两帧之间的像素灰度值构建光度误差来求解相机运动，并且直接法可以在特征缺失的场合下使用。
今天，我们主要来关注VSLAM中直接法开源算法。
4 直接法开源算法
4.1 LSD-SLAM
DTAM是直接法的鼻祖， 是 2011 年提出的单目 SLAM算法，对每个像素点进行概率的深度测量，有效降低了位姿的不确定性。该方法通过整幅图像的对准来获得稠密地图和相机位姿，但是需要 GPU 加速，超出了本文的讨论范围。
基于同样的原理， TUM 机器视觉组的 Engel 等人于2013 年提出了基于直接跟踪的视觉里程计(semi-dense visual odometry)系统，该 VO 系统是第一个不采用特征的实时的视觉里程计。后来他将地图优化融入该 VO 系统并扩展为LSD-SLAM，得到了不采用特征的实时SLAM 系统。 该系统通过对图像光度直接配准和使用概率模型来表示半稠密深度图，生成具有全局一致性的地图。
它有如下创新点：
a) 使用随机深度初始化策略类似于滤波器方法的思路来完成初始化。将图像中的像素以随机的深度初始化，并利用新产生的数据不断迭代优化直至收敛，当初始场景的深度方差收敛到最小值时，认为初始化完成。
b) 通过假设图像像素逆深度服从高斯分布，对每个像素深度独立计算，通过卡尔曼滤波更新深度估计， 将深度图的噪声融合到图像跟踪中，构建半稠密和高精度的三维环境地图；
c) 为了避免尺度上的漂移，将估计的深度均值归一化，而且考虑深度和极线的夹角，在关键帧的直接配准上，采用sim3 来衡量其变换，并将光度残差和深度残差一起放入优化函数中：
其中：两项分别为被归一化的光度残差和深度残差；
表示 Huber 核函数，避免误差太大而覆盖其他的正确值。
4.1.1 主要结构
LSDSLAM 具有图像跟踪、深度估计和地图优化三个线程。
图像跟踪。 主要计算当前帧与参考帧之间的相对变换，有精确方式和快速方式，都采用加权的高斯牛顿优化方法。关于跟踪失败后的重定位，在文章里没有，但是开源代码里有实现：将当前帧和邻近的关键帧连接起来，计算坐标变换关系，通过打分和遍历整个附近帧来判断是否完成重定位。
深度估计。 当相机移动超过了阈值，那么需要创建关键帧，将之前关键帧的点投影到当前新的关键帧上，通过 sim3变换得到该关键帧的深度估计。当跟踪帧没有变为关键帧，文章中用它来更新图像的像素深度：先采用自适应的方法确定搜索范围，然后通过卡尔曼滤波不断地利用观测值对深度进行修正。
地图优化。 其目的是利用闭环解决尺度漂移的问题。首先去寻找所有可能相似的关键帧，并计算视觉意义上的相似度，并由 appearance-based mapping 算法筛选出来的候选帧，而且需要进行跟踪检测，当完成闭环约束以后，再通过全局优化得到全局一致性地图，其包括关键帧组成的姿态图和对应的半稠密深度图。
4.1.2 优缺点
LSDSLAM 是直接法中比较完整的 SLAM 系统，能够在普通 CPU 上实现半稠密 SLAM（梯度明显的像素），后续Engel 对LSDSLAM 进行了功能拓展，使其能够支持双目相机和全景相机。
但是它存在一定缺点：对相机内参和曝光非常敏感，而且准确性方面不及 ORBSLAM，速度方面不及 DSO。
作者后续研究了光度标定，将其扩展应用于 DSO系统（https://github.com/JakobEngel/dso）。
4.2 DSO
DSO为 Engel 在 2016 年发布的一个视觉里程计方法，因为没有闭环，所以只能算 SLAM 的一个模块（后续应该会完善），文中宣称速度可以达到传统特征点法的五倍。
直接法因为是比较两帧图像之间的像素差异，需要满足光度不变，但是这是一个很强的假设，尤其是针对普通的自动曝光相机。在做 DSO 工作之前， Engel 先研究了光度标定相关工作， 因为他认为对相机的曝光时间、暗角、伽马响应等参数进行标定后，能够让直接法更加鲁棒。这个过程建模了相机的成像过程，对于由相机曝光不同所引起的图像明暗变化会有更好的表现。
DSO 是一种结合直接法和稀疏法的视觉里程计，它不检测和计算特征点， 而是采样图像内具有强度梯度的像素点；它将光度误差模型和所有模型参数融入到优化函数中进行联合优化， 而且该系统结合曝光时间、 透镜晕影以及非线性响应函数的影响提出了完整的光度标定方法，并在多个数据集上进行了测试，达到了很好的精度和速度， 可以说是 LSDSLAM 的升级版。
进一步地， Engel 小组研究了双目的 DSO，但并没有开源代码（吴佳田、颜沁睿等做了相应的工作（https://github.com/HorizonAD/stereo_dso）），而且包括他们自己在内的很多研究者在 DSO 的基础上扩展，尝试给 DSO 添加回环检测和地图重用的模块。
4.2.1 具体流程
将相机内参和曝光参数作为优化变量引入优化函数，并推导了其相对于残差的雅可比是 DSO 的最大创新之处。其流程如下图所示。
首先是两帧图像对齐初始化和地图点的更新：地图点在一开始被观测到时，其深度是未知的，随着相机的运动， DSO 会采用沿着极线搜索方式在每张图像上追踪这些地图点，跟踪过程会确定每个地图点的逆深度和变化范围；
然后通过相机视野改变、相机平移和曝光时间显著改变这些参数是否达到阈值来构建关键帧。在后端优化过程中， DSO采用由相个关键帧组成滑动窗口的方式，不断地计算需删除的关键帧和添加关键帧， 并且将每个先前关键帧中的地图点投影到新关键帧中， 形成残差项，同时在新的关键帧中更新地图点和删除外点。
与 LSD-SLAM 类似， DSO 的前端线程通过匹配新帧与关键帧来跟踪相机的位姿，然后通过一定的条件判断新帧是否会成为关键帧，并将新关键帧插入后端优化线程。 DSO 的后端在一个滑动窗口内对维护的 5~7 个关键帧进行优化。在各关键帧中提取深度收敛的像素点，投影至其他关键帧中，得到光度误差。
最后滑动窗口通过优化光度误差之和
与传统的捆集调整对全局的关键帧和特征点进行优化不同，DSO 中的滑动窗口优化保存一定数量的关键帧，需要将多余的帧进行边缘化处理。滑动窗口优化中的边缘化通过更新信息矩阵，将被删除帧的信息作为先验信息保存在信息矩阵中，控制了优化的计算量，又实现了良好的优化效果。
3.2.2 具体细节
一、在DSO中，有哪些地方用了边缘化？首先，DSO的BA，也和传统BA一样。因此DSO在解BA时，边缘化了所有点的信息，计算优化的更新量。然而，与传统BA不同的是，DSO的左上角部分，即公式中的 ，并非为对角块，而是有先验的。传统BA中，这部分为对角块，主要原因是不知道相机运动的先验，而DSO的滑动窗口，则通过一定手段计算了这个先验。
这里的先验主要来自两个部分：
边缘化某个点时，这个点的共视帧之间产生先验；
边缘化某个帧时，在窗口内其他帧之间产生先验；
这里的“边缘化”，是通过舒尔补，用矩阵的一部分去消元另一部分。然而实际操作的含义却有所不同。在BA的边缘化中，我们希望用边缘化加速整个问题的求解，但是解完问题后，这些帧和点仍旧是存在于窗口中的！而滑动窗口中的边缘化，是指我们不再需要这个点/这个帧。当它被边缘化时，我们将它的信息传递到了之后的先验中，而不会再利用这个点/这个帧了！请读者务必理清这层区别，否则在理解过程中会遇到问题。我们不妨将后者称为“永久边缘化”，以示区分。
那么DSO如何永久边缘化某个帧或点？它遵循以下几个准则：
如果一个点已经不在相机视野内，就边缘化这个点；
如果滑动窗口内的帧数量已经超过设定阈值，那么选择其中一个帧进行边缘化；
当某个帧被边缘化时，以它为主导的地图点将被移除，不再参与以后的计算。否则这个点将与其他点形成结构先验，破坏BA中的稀疏结构。
在边缘化的过程，DSO维护了帧与帧间的先验信息（见EnergyFunctional::HM和bM），并将这些信息利用到BA的求解中去。
二、First-Estimate-Jacobian
在边缘化的时候，我们需要不断迭代计算H矩阵和残差b，而迭代过程中，状态变量会被不断更新，计算雅克比时我们要fix the linearization point。也就是，计算雅克比时求导变量要固定，而不是用每次迭代更新以后的x去求雅克比，这就是所谓的用第一次得到的雅克比（First Estimate Jacobians）。
在DSO实际使用时，仅在优化的初始时刻计算几何和光度的雅可比，并让它们在随后的迭代过程中保持不变，即First-Estimate-Jacobian。FEJ强制让每个点在同一个时刻线性化，从而回避了零空间降维的问题，同时可以降低很多计算量。
DSO中的每个地图点，由前面定义的pattern产生8个残差项，这些残差项也会共享FEJ。由于几何和光度函数都比较光滑，实践当中FEJ近似是十分好用的。
3.2.4 优缺点
DSO采用最小化光度误差的优化算法，并且考虑了光度标定模型，其优化范围不是所有帧，而是由最近帧及其前几帧形成的滑动窗口。与现有的直接法不同，优化所有模型参数（包括相机内参，相机外参和逆深度值），但算法效率与间接法中常用的bundle adjustment算法相近。同时，采用和其他直接法相同的地图点表示方法，即三维点被表示为相应坐标下的逆深度值，因此只有一个自由度。
在大部分数据集上，DSO均有较好的表现。虽然DSO要求全局曝光相机，但即使是卷帘快门的相机，只要运动不快，模糊不明显，DSO也能顺利工作。但是，如果出现明显的模糊、失真，DSO也会丢失。
直接法相比传统特征点法，最大的贡献在于，直接法以更整体、更优雅的方式处理了数据关联问题。特征点法需要依赖重复性较强的特征提取器，以及正确的特征匹配，才能得正确地计算相机运动。在环境纹理较好，角点较多时，这当然是可行的——不过直接法在这种环境下也能正常工作。然而，如果环境中出现了下列情况，对特征点法就不那么友善：
环境中存在许多重复纹理；
环境中缺乏角点，出现许多边缘或光线变量不明显区域；
直接法给予我们追踪边缘、平滑区块的能力，但同时也要付出代价——正确的直接法追踪需要有一个相当不错的初始估计，还需要一个质量较好的图像。由于DSO严重依赖于使用梯度下降的优化问题求解，而它成功的前提，是目标函数从初始值到最优值之间一直是下降的。在图像质量不佳或者相机初始位姿给的不对的情况下，这件事情往往无法得到保证，所以DSO也会丢失。
DSO 初始化不仅需要较好的初始估计，还比较依赖梯度下降的优化策略， 而它成功的前提要求目标函数是单调的，但这件事情往往无法得到保证。进一步说，如果想在 DSO 上加重定位功能，首先需要保存所有帧，然后需要对相机位姿有一个比较准确的初始估计。但这通常是困难的，因为不知道误差累积了多少。而在特征点法中，地图重用则相对简单。
结语
直接法是采用大量的像素信息来优化求解相机位姿， 与特征点法相比是数量替代质量的过程，本文只需存储空间中所有的特征点和它们的特征描述，然后匹配当前图像中看到的特征，计算位姿即可。从这个角度来看：直接法应该更擅长求解连续图像的定位，而特征点法则更适合全局匹配与回环检测。
该篇作为SLAM系列的第四篇，主要介绍了直接法开源算法——LSD-SLAM和DSO。接下来，SLAM系列文章会持续更新，会继续和大家介绍更多主流框架，VIO多传感器融合以及激光SLAM等内容，欢迎大家持续关注~
获取文中论文原文和阅读笔记完整版，请关注公众号留言或扫描下方个人微信。
参考：
1. D.Caruso, J. Engel, and D. Cremers. Large-scale direct SLAM for omnidirectionalcameras. In International Conference onIntelligent Robot Systems (IROS), 2015. 4
2. SLAM中的marginalization 和 Schur complement
3. https://arxiv.org/pdf/1708.07878.pdf
4. 《视觉SLAM十四讲：从理论到实践 第2版》高翔等人
5. https://zhuanlan.zhihu.com/p/29177540 知乎文章
6. https://www.cnblogs.com/luyb/p/6077478.html
目标检测系列秘籍一：模型加速之轻量化网络秘籍二：非极大值抑制及回归损失优化秘籍三：多尺度检测秘籍四：数据增强秘籍五：解决样本不均衡问题秘籍六：Anchor-Free
语义分割系列一篇看完就懂的语义分割综述最新实例分割综述：从Mask RCNN 到 BlendMask
面试求职系列决战春招！算法工程师面试问题及资料超详细合集
一起学C++系列内存分区模型、引用、函数重载
竞赛与工程项目分享系列如何让笨重的深度学习模型在移动设备上跑起来基于Pytorch的YOLO目标检测项目工程大合集点云配准领域全面资料、课程、数据集合集分享10万奖金天文数据挖掘竞赛！0.95高分Baseline分享目标检测应用竞赛：铝型材表面瑕疵检测
SLAM系列视觉SLAM前端：视觉里程计和回环检测视觉SLAM后端：后端优化和建图模块视觉SLAM中特征点法开源算法：PTAM、ORB-SLAM
视觉注意力机制系列Non-local模块与Self-attention之间的关系与区别？视觉注意力机制用于分类网络：SENet、CBAM、SKNetNon-local模块与SENet、CBAM的融合：GCNet、DANetNon-local模块如何改进？来看CCNet、ANN