视觉SLAM的前段后端最详细的梳理（硕士入门知识框架更新）

视觉SLAM的组成结构

视觉里程计（VO）

-特征点法 -构建稀疏点云地图

  特征点法提取图像中稀疏的特征点，通过描述子完成帧间匹配，然后根据特征点间 2D 到 2D、2D 到 3D 或是 3D 到 3D 的约束关系使用对极几何、PNP 或 ICP 等算法求解位姿。

Klein G, Murray D. Parallel Tracking and Mapping for Small AR Workspaces[C]// IEEE & Acm International Symposium on Mixed & Augmented Reality. 2008.

  Klein 提出的 PTAM（Parallel Tracking and Mapping）创新性的实现了跟踪与建图过程的并行化，并首次有了前端与后端的概念：前端为跟踪线程，根据获取的图像数据实时估计位姿；后端引入了关键帧机制，实现对地图的非线性优化。但 PTAM 也 存在着应用场景小、特征易跟丢等缺陷。

Mur-Artal R , Montiel J M M , Tardos J D . ORB-SLAM: a Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.

Mur-Artal R , Tardos J D . ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras[J]. IEEE Transactions on Robotics, 2017:1-8.

  2015 年 Mur-Artal 等人提出的 ORB-SLAM，相较于 PTAM 的双线程，它采用了并行跟踪，局部建图，闭环检测三线程：

  并行跟踪线程用于完成基于模型评分的地图初始化、特征匹配及位姿估计；

  局部建图部分提出了一个宽进严出的关键帧筛选策略，并对构建的局部地图进行优化；

  闭环检测使用词袋模型判断当前场景是否曾经出现过，并实现了对全局地图进行优化，对于要求全局一致性的应用场景有着优秀的效果。

  目前在基于特征点法的 SLAM 系统中，ORB-SLAM 中被认为非常稳定与出色，后续的一些研究也以它为标准。

-直接法 -构建稠密或半稠密的地图 --需要使用 RGB-D 传感器

  直接法引入了光流跟踪的思想，在光度不变假设的基础上，以最小化光度误差为优化目标， 对位姿变量进行求解。

Engel J, Schöps T, Cremers D. LSD-SLAM: Large-Scale Direct Monocular SLAM[J]. 2014.

Engel J, Koltun V, Cremers D. Direct Sparse Odometry.[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2018, 40(3):611-625.

  Engle 等人于 2014 年提出 的 LSD-SLAM（Large Scale Direct Monocular SLAM），实现了一种 CPU 级别的单目半稠密地图重建，并且在单目深度估计上考虑到三角化的不确定性而采用了一种新颖的滤波形式 。

  LSD-SLAM 对于相机的快速运动、曝光变换鲁棒性较低，2016 年 Engle 等人提出了另一个单目稀疏 直接法视觉里程计 DSO（Direct Sparse Odometry），针对直接法易受光照干扰的缺点，DSO 采 用光度标定模型，它在优化中会动态的估计光度参数，从而使得算法对曝光变换更加鲁棒。

Forster C , Pizzoli M , Scaramuzza D . SVO: Fast Semi-Direct Monocular Visual Odometry[C]// IEEE International Conference on Robotics & Automation. IEEE, 2014.

  SVO（Semi-direct Visual Odometry）是由 Forster 等人于 2014 年提出的半直接法视觉里程计， 应用在携带俯视视角单目相机的无人机平台。因为它首先对图像提取角点，然后再对角点采用 直接法求位姿，所以称之为半直接法。SVO 在纹理丰富的平面场景中有着较好的定位精度，并且能达到极快的运行速度。

视觉里程计–总结：

  特征点法适应于帧间的大运动，即只要帧间存在视野重叠就能通过特征描述子进行全局匹配。

  直接法的本质是进行一个最优化，当帧间存在大运动或者光照变换、 图像模糊等情况，易陷入局部最优或是丢失对地图点的跟踪。

  因此特征点法具有更好的鲁棒性， 然而特征点、描述子的提取会占据大量时间，在保证点个数、匹配效率的情况下就很难达到实时性，目前特征点法在保证实时性的情况下多使用 ORB 特征提取算法。

  直接法能利用图像中的更多的信息，包括一些不能提取特征点的区域，在 GPU 下能够实现稠密重建，而稀疏直接法能在 CPU 上达到一个较优的实时性。

SLAM 后端研究

  SLAM 问题的数学描述通常为运动方程与观测方程两部分，对于不同的传感器，不同应用场景，分别有着不同的参数化方程及状态量。本质上可将后端优化看作一个状态估计问题：如何根据带有噪声的观测求系统状态的最优估计，在实现方式上分为滤波器和非线性优化法。

滤波器法

  滤波器法假设状态间具有马尔科夫性，即当前时刻状态仅于上一时刻状态有关。基于滤波器法的 SLAM 并没有明显的区分出后端的概念，其核心思想可通过贝叶斯滤波模型进行统一描述，即对状态量不断的进行迭代与更新。

  如果运动和观测方程属于线性方程，且状态与噪声服从高斯分布， 即可得到卡尔曼滤波。 对于非线性的系统， Smith 等人通过扩展卡尔曼滤波器 （Extended Kalman Filter，EKF）解决该问题，对系统方程进行一阶线性近似。

  对于视觉 SLAM， 地图点以及位姿量随着系统运行会越来越多，那么 EKF 需要维护和更新的均值与协方差规模随之也会越来越大，则会大量占用存储空间，因此不适用于大规模环境。同时，SLAM 的运动与观测通常具有严重的非线性，线性近似仅在小范围内成立，而较远处则会造成严重的非线性误差。

Sim R , Elinas P , Little J J . A Study of the Rao-Blackwellised Particle Filter for Efficient and Accurate Vision-Based SLAM[J]. International Journal of Computer Vision, 2007, 74(3):303-318.

  针对非线性非高斯系统，研究者们也提出了粒子滤波（Particle Filter，PF） 方法，用粒子来描述位姿或地图点，通过对粒子的随机采样来近似状态的概率密度分布。

  PF 方法的优点在于状态估计对数据关联不敏感，且在线性近似方面具有更好的表现，缺点是粒子计算的复杂度高， 在多次迭代中易退化， 因此同样不适合在大规模环境中应用。

非线性优化

  非线性优化法考虑系统所有的状态量，在带有噪声的观测中对其做一个最优估计，即最大化其后验概率，从而可推导出以位姿、地图点作为优化变量，以噪声方差作为优化目标的最小二乘问题。

  就视觉 SLAM 而言，其概念类似于运动恢复结构（Structure From Motion，SFM）中的光束平差法（Bundle Adjustment，BA） ，通过同时调整位姿及地图点，使其以最优的方式去满足相机模型，收束至光心，因为考虑到过去所有的观测数据，这样的全局优化能有效减小累计误差，但同时也有着很大的计算量，因此早期 BA 算法只能用于离线的 SFM 等应用中。

  随着研究的深入，研究者们发现：每一个地图点并不是在所有位姿下均能观测到。利用这种稀疏性，反映在矩阵运算中可通过消元等数学技巧来加速求解，这使得非线性优化法能应用于实时的 SLAM 系统中。紧接着，研究者们将非线性优化与图论所结合，称作图优化。

  当前国外 SLAM 研究大量采用基于图优化的后端，例如 PTAM、ORB-SLAM、LSD-SLAM 等。

后端方式的差异（有五种优化方式）：

  1.卡尔曼滤波器：从k-1时刻后验推k时刻先验，从k时刻先验推k时刻后验；

  2.扩展卡尔曼滤波器：对卡尔曼滤波器进行修正，针对不是线性的情况，采用一阶泰勒展开近似线性。

  3.BA优化：把一路上的所有坐标点与位姿整体放在一起作为自变量进行非线性优化

  4.PoseGraph优化：先通过一路递推方式算出的各点位姿，通过数学方式计算得到一个位姿的变换A，再通过单独拿出两张图像来算出一个位姿变换B，争取让B=A

  5.因子图优化：保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，需要重新计算的再去计算，从而避免冗余计算。

单目视觉里程计的初始化：

  单目视觉里程计在没有先验地图信息的情况下，只能通过传感器获取图像帧，帧与帧之间对应像素点需要满足对极约束，这是种 2D 到 2D 的约束关系。

两种模型完成初始化的运动估计：

  （1）本质矩阵模型：适用于参与模型计算的地图点落在非同一平面；对本质矩阵进行奇异值分解可得到 4 组旋转、平移，只有一组满足实际的物理意义，即点在两帧中具有正向深度。此根据本质矩阵恢复出的 $t$ 不是在真实尺度下的平移，而恢复的旋转矩阵由于本身具有正交约束，所以具有唯一性。

  （2）单应矩阵模型：适用于参与模型计算的地图点落在同一平面，或近似平面，这种情况下若使用本质矩阵会发生退化，恢复出的位姿误差较大。

  两种模型可应对不同的场景，考虑到平面场景的点在两帧间的视差波动较小，且点的深度与视差成反比，所以对于距离较远的非平面的点之间视差的波动同样较小，可近似为平面。而非平面场景，点之间的深度具有较大差异，视差的差异也会较大。因此，可根据视差的方差描述平面或非平面场景，选择不同的模型恢复运动。

  根据旋转矩阵与平移向量三角化出所有匹配点的深度，即得到若干初始的地图点。 前文提到，通过这种 2D 到 2D 的对极约束恢复出的运动并不是在真实尺度下的运动，而是未知尺度下的运动，因此会设定一个尺度。尺度设定通常有两种方法，一种是将平移向量归一化，另一种是对所有地图点的平均深度进行归一化，第二种相对第一种能适应不同规模场景。

  通过对单目视觉里程计的初始化，设定了一个尺度，恢复出前两帧的运动并初始化若干地图点，目的是为了解决 2D 到 2D 约束存在不确定尺度的问题，因此后续的运动估计通过跟踪已知深度的地图点得到，在初始化设定的尺度下求解 3D 到 2D 的约束问题。

运动估计：

  运动估计中涉及的帧、特征点、地图点，它们之间的对应关系为：一帧图像包含有若干特征点，一个特征点对应一个 图点，一个地图点由于不止会被一帧所观测，因此对应若干特征点。在对连续帧进行运动估计的过程中，需要持续不断的恢复场景中的地图信息，以确保视觉里程计能够跟踪到足够多的地图点，建立 3D 到 2D 的约束关系。

运动估计主要分为三步：

  （1）稀疏图像对齐：采用直接法估计前后两帧间的初始位姿。

  （2）局部地图跟踪：建立在第一步的初始位姿上，将局部地图投影至当前帧，筛选若干投影点进行一个亚像素精度的优化，若投影点满足优化条件则认为对当前地图点成功跟踪，并为当前地图点在该帧建立特征。

  （3）位姿及结构优化：针对局部地图点与特征间建立了 3D 到 2D 的约束，从而估计出更准确的位姿，以及优化地图点。

半直接法运动估计的优化方案：

  对于稀疏图形对齐，通过构建以帧间位姿为自变量的灰度残差函数，其只能采用非线性优化的方式在初始位姿上进行迭代更新目标函数使得最优。其核心在于初始值的设定及雅克比矩阵$J（x）$。每一次迭代均需要计算迭代值处的雅克比矩阵，这种优化方式属于前向组合优化。

Baker S, Matthews I. Lucas-Kanade 20 Years On: A Unifying Framework[J]. International Journal of Computer Vision, 2004, 56(3):221-255.

  1）Baker 提出一种后向组合优化算法，其采用了近似优化的思想，即固定当前帧投影点而对参考帧的特征点进行估计。这种后向组合优化的优点在于只用求一次雅克比矩阵，从而减少了计算量。

  2）针对帧间较大运动，对前后两帧下采样构建高斯图像金字塔，从顶层到底层分辨率由粗到细，从顶层金字塔开始进行位姿迭代优化，每一层优化的结果作为下一层金字塔优化的初值。

  3）在构建残差函数时，将像素点附近的图像块加入残差计算，而非只考虑单像素灰度差异。

关于解决累计误差的一些方法：

  视觉里程计根据前后两帧得到了帧间位姿，理论上可一直按照稀疏图像对齐的半直接法估计运动，然而帧间求得的位姿会存在误差，并且上一帧的误差会累积至下一帧的运动估计中。如图所示，长时间运行的视觉里程计会存在较大的累积误差而导致漂移，破坏了全局一致性。

  累积误差通常会放入后端中进行一个全局的优化，不影响前端运动估计的实时性。而对于无人机的应用需求来说，避障、或是短时的路径规划则看重于局部一致性。

  这里可以构建局部地图，然后筛选若干局部地图的地图点投影至当前帧，做一个亚像素精度的优化，满足优化条件则认为对该地图点跟踪成功，并在优化后的投影位置为该帧建立与地图点对应的特征。局部地图跟踪（如图利用五点法对局部进行跟踪）的目的是为了对位姿做进一步的优化。

目前存在的问题：

  1）稀疏图像对齐本质是基于灰度不变假设的非线性优化问题，该问题的求解可以理解为是一个不断朝着最优方向迭代的过程。当帧间为小运动时，目标函数可看作凸函数，具有唯一极值点。而当受到光照影响，或存在大运动时，目标函数具有非凸性，则会陷入局部最优。

  2）局部地图跟踪是在第一步估计的初始位姿下进行，要求初始位姿较为准确。若初始位姿误差较大，则局部地图跟踪会陷入局部最优，或是投影点因不满足跟踪条件导致跟踪丢失。

  3）局部地图跟踪采用了一种均匀跟踪策略，但是某些场景下特征并不是均匀分布，而是 集中分布在图像中某些区域。均匀跟踪策略在这些场景中会丢失对较多地图点的跟踪，从而导 致视觉里程计不能稳定运行。

可能存在的改进方法：

  1）融入特征匹配来改进运动估计：：半直接法采用 FAST 角点作为特征，可以用 ORB 特征点替代 FAST 角点，为了减小特征点法全局匹配带来的时间开销，在投影点处选取像素的矩形区域，然后在该图像区域中提取 ORB 特征点，并与该地图点在参考关键帧中对应的特征进行匹配。通过特征匹配的方式，可以修正投影点较大的位置偏差，接着再进行投影点的亚像素位置优化。

  2）特征提取时，可通过建立哈希表来记录图像中每个像素是否提取过特征，而对于重复区域不再提取，这样可最大程度的减小时间开销。

  3）特征匹配是通过计算描述子间的距离，得到满足最近与次近比的最近距离，并与距离阈值作比较得到匹配结果。为了保证特征匹配结果的正确性，应将阈值设得较低。

关键帧的建图及后端优化：

  1）对关键帧提取特征，并对特征通过深度估计建 立场景中对应的地图点，持续为视觉里程计提供新的地图信息以保证运动估计的鲁棒性。

  2）关键帧保存了已有的地图信息及运动轨迹，将其放入后端优化，达到减小误差，提高一致性的目的。

选取关键帧的方法：时间/空间/内容 变化

  时间： 简单的关键帧策略是每隔固定的时间采样一帧作为关键帧

  空间： SVO-通过空间变换选取关键帧，如果当前帧与上一关键帧的位姿变换超过阈值则判定为新的关键帧，这种方法相对于时间采样法更为鲁棒，选取的关键帧更具代表性。

  其具体方法是：首先得到当前帧所观测地图点的平均深度，然后遍历局部地图关键帧，得到每一帧与当前帧在 $X、Y、Z$ 轴上的平移量 $t_x、t_y、t_z$ ，若平移量均大于平均深度的 ，则当前帧作为关键帧插入地图中。

  SVO 的关键帧策略适用于无人机携带俯视相机运动，或视野为平面场景，但不适用于视野为非平面场景的前向运动，因为非平面场景中的地图点平均深度不能代表场景的深度信息，一些较远的地图点会使得平均深度较大，则平移量不能满足阈值，关键帧提取不及时则会导致跟踪丢失。

  内容： 基于视觉内容的方法是通过判断当前帧与关键帧间的相似性来选取新的关键帧。ORB-SLAM 结合了视觉内容与时间采样两种方法，通过三个参数来选取新的关键帧：距离上一关键帧插入的时间；当前帧的特征点数量；与上一关键帧间的匹配点数量占比。

  点特征的选取通常参考五个评价指标：算法执行速度、旋转 不变性、尺度不变性、模糊不变性、光照不变性。

  FAST 算法采用了机器学习的方法，利用 ID3 决策树来提升检测速度及通用性。

  ORB 算法在 FAST 角点基础上增加了方向信息，目的是为了改进旋转不变性。

  ORB 特征算法在实时性上占据突出的优势， 且对于旋转、模糊、光照具有良好的不变性。

  可以通过 BRIEF 描述子间的汉明距离来表示特征点间的相似度，汉明距离越小，则特征点间相似度越高，从而完成特征匹配。

  梯度特征：稀疏图像对齐、局部地图跟踪阶段的优化均利用了图像的梯度信息，因此除了角点，还可以利用图像中带有明显梯度的特征参与运动估计。

特征的深度估计：

  在单目 SLAM 中，深度估计作为其重要的一环，不同于双目或 RGB-D 传感器能直接获得像素的深度信息，它是通过移动的立体视觉估计像素深度，具体可分为三角测量和滤波器两种方法。

  极线匹配： 极线匹配是在运动已知的情况下完成关键帧与当前帧的特征匹配，其中帧间位姿可由前端视觉里程计得到。

  在确定匹配点时，单个像素点灰度差异不稳定且相似点较多， 因此采用图像块的匹配方式。而深度估计是在系统的建图线程进行，不同于前端对实时性的高要求，因此采用去均值的归一化互相关（Normalized Cross Correlation，NCC）块匹配算法，其具有较高的精度及计算复杂度。

  三角测量法会存在不确定性，这种不确定性可理解为：假设观测值存在某个误差，这个误差会对测量值造成多大的变化，是否在可接受的范围？究其原因，这种不确定性是由于匹配精度而引发的。

  因此可以从减小匹配误差出发，对极线搜索的图像块作亚像素精度的双线性插值，在极线匹配后接着对匹配点像素位置做最小二乘优化，得到亚像素精度的匹配点，如果该点对应特征为梯度点，则约束其优化方向为梯度方向。

  深度滤波器： 根据前文所述，对特征的单次深度估计可通过极线匹配加三角测量完成，但除了三角测量的不确定性，其在极线搜索中的分布也通常存在的较多局部极值，这种非凸性可能会造成单次测量中存在错误的匹配。因此深度估计也可建模为一个状态估计问题，利用滤波的思路去求解，将当前观测值不断融合上一次估计值去缩小不确定度，得到最终收敛的深度。

图优化的后端研究：

  在后端非线性优化中，需要考虑局部或全局的地图点与位姿，特别是地图点，属于大规模的优化。图优化理论将非线性优化与图论所结合，王忠立等人对图优化方法进行了总结，主要包括基于最小二乘法的优化方法，基于松弛迭代的优化方法，基于随机梯度下降的优化方法，以及基于流形的优化方法。

  前三种优化方法在欧氏空间中进行，对于位姿中自带约束的旋转矩阵进行估计时，可能会出现奇异，第四种方法将 SLAM 的最小二乘问题在流形空间中优化，特殊欧氏群 $SE（3）$ 即是连续、光滑的流形，通过李代数 $\mathfrak{s}\mathfrak{e}(3)$ 表示位姿参与优化，去掉了旋转矩阵的自身约束，避免了奇异值问题。

  对于流形空间中的图优化，G2o 提供了一个基于 C++编程语言的通用框架，并在优化问题中得到广泛应用。其通过建立顶点（Vertex）与边（Edge）直观的表示优化问题，一个顶点代表一个优化变量，一条边用于连接顶点，代表顶点间的约束关系。

  SLAM 的图优化示例，其中三角形、圆形顶点分别对应地图点、位姿，蓝色线对应运动方程，红色线对应观测方程。

视觉与IMU融合的分类：

• 松耦合和紧耦合：

  松耦合（Loosely Coupled）:

   松耦合是指IMU和相机分别进行自身的运动估计，然后对其位姿估计结果进行融合,两个模块更新频率不一致 , 模块之间存在一定的信息交换，在松耦合方式中以惯性数据为核心 , 视觉测量数据修正惯性测量数据的累积误差。
紧耦合（Tightly Coupled）:

  紧耦合是指把IMU的状态与相机的状态合并在一起，共同构建运动方程和观测方程，然后进行状态估计,IMU的尺度度量信息可以用于辅助视觉中的尺度的估计。

• 滤波法和优化法：

  滤波：MSCKF(Multi-State Constraint KF），ROVIO

  图优化：VINS，OKVIS，ORB-SLAM

视觉与IMU的融合的优势：

• 视觉与IMU的融合可以借助IMU较高的采样频率，进而提高系统的输出频率。
• 视觉与IMU的融合可以提高视觉的鲁棒性，如视觉SLAM因为某些运动或场景出现的错误结果。
• 视觉与IMU的融合可以有效的消除IMU的积分漂移。
• 视觉与IMU的融合能够校正IMU的Bias。
• 单目与IMU的融合可以有效解决单目尺度不可观测的问题。
• 可以应对快速的运动变化，相机在快速运动过程中会出现运动模糊。

存在的问题：

  1. 使用 IMU 对相机在快门动作期间内估计相机的运动，但是由于 CMOS 相机的快门时间戳和 IMU 的时间戳的同步比较困难，且相机的时间戳不太准确 ；

优秀的开源：

vinsmono、vinsfusion、 vi_orbslam2