【VSLAM系列】五：ORB-SLAM2论文笔记

ORB-SLAM2论文

ORB_SLAM里,位移向量tcw的方向是从左下标到右下标的,并且位于左下标坐标系下,Rcw是从世界坐标系到相机坐标系的旋转,RT表示R的逆旋转

• 基于单目、双目、RGB-D相机的SLAM系统，其中包括回环检测、重定位、以及地图重用功能。
• 我们的RGB-D结果显示，相比较基于迭代最近点法（ICP）或光度和深度误差最小法，我们通过使用光束法平差法（BA），可以达到更高的精度。
• 通过使用远近匹配双目点和单目观测，我们的双目结果比目前最好的直接双目SLAM的精度更高。
• 提出了一个轻量级的重定位模式，可以在无法建图时，有效地重新使用地图。

参考博客：
https://www.cnblogs.com/MingruiYu/p/12991119.html
https://blog.csdn.net/weixin_43503355/article/details/121618694

1 整体框架

3+1平行线程

a：加入了双目和RGBD信息的预处理，目的是获得一致的立体视觉，(ul,vl,ur)。数据处理如b所示。
b：在闭环约束优化后，又加入了全局BA。优化Essential矩阵是对检测到的闭环内的位姿和路标点进行优化，全局BA是对除了初始点意外的所有路标点和位姿都进行优化
与ORB_SLAM1相似ORB_SLAM2由3+1个平行线程组成，包括跟踪、局部建图、回环检测以及在回环检测后的全局BA优化。之所以说是3+1，因为第四个线程仅在回环检测并确认后才执行。前三个主要并行线程：
a：跟踪：通过寻找对局部地图特征进行匹配，利用纯运动BA最小化重投影误差进行定位每帧图片的相机；
b：局部建图：通过执行局部BA管理局部地图并优化；
c：回环检测：检测大的环并通过执行位姿图优化更正漂移误差。这个线程触发第四线程；
d：全局BA：在位姿图优化之后，计算整个系统最优结构和运动结果。
该系统运用了基于DBOM2嵌入式位置识别模型进行重定位，来防止跟踪失败、或已知地图场景重初始化、回环检测

2 单目、近处双目和远处双目特征点

ORB_SLAM是一种基于特征的方法，系统的所有运行都是基于输入图像的特征展开，而不依赖于所用的传感器是双目或者RGB-D。因此对双目和RGBD数据进行处理转化成相同格式（uL,vL,uR）的输入传给系统。

双目特征点通过三个坐标定义（uL,vL,uR），(uL,vL)是左图的坐标，uR是右图坐标。对于双目相机提取两幅图像当中的ORB特征，将左边每个的ORB特征匹配到右边的图像中。利用极线水平的条件，对双目图像进行校正。在左边的图像产生双目的ORB特征点，和一条水平的线向右边的图像进行匹配，通过修补相关性来重新定义亚像素。
对于RGB-D相机在ORB图像通道上提取ORB特征点，将该点坐标作为左图坐标，将深度转换为虚拟的右图坐标，转换关系如下式：其中d为深度；fx为水平焦距 ； b为结构光投影仪 与水平相机之间的基线。至此获得一致的立体视觉输入。
$$u_R=u_L-(f_{x}b)/d$$
单目的特征点通过左图的两个坐标xm=(uL,vL)定义，必须保证所有的ORB特征是一致的，这些点仅能够从多视图中三角测量化并且不能够提供尺度信息，但是可以提供旋转和平移的估计信息。
系统处理单目或者双目的特征点，分成远处特征点和近处特征点两类。特征点的匹配深度值小于40倍双目或者RGB-D的基线则为近处点，否则的话，是远特征点。
a：近特征点：能够由一帧图像直接三角测量化，得到的深度是精确的估计，并且能够提供尺度，平移和旋转的信息。
b：远特征点：仅仅能够提供精确的旋转信息，但是很少的尺度和平移信息。当提供多视图的时候，我们才能三角化那些远的点

双目匹配

1）步骤1：建立特征点搜索范围对应表，一个特征点在一个带状区域内搜索匹配特征点。 匹配搜索的时候，不仅仅是在一条横线上搜索，而是在一条横向搜索带上搜索,简而言之，原本每个特征点的纵坐标为1，这里把特征点体积放大，纵坐标占好几行， 例如左目图像某个特征点的纵坐标为20，那么在右侧图像上搜索时是在纵坐标为18到22这条带上搜索，搜索带宽度为正负2，搜索带的宽度和特征点所在金字塔层数有关

2）步骤2：对左目相机每个特征点，通过描述子在右目带状搜索区域找到匹配点, 再通过SAD做亚像素匹配，通过描述子匹配得到匹配点位置为scaleduR0,，通过SAD匹配找到修正量bestincR， 通过抛物线拟合找到亚像素修正量deltaR

3 单目和双目的BA优化

系统在追踪线程中使用BA来优化相机位姿（纯运动BA），在局部建图线程中优化关键帧和点的局部窗（局部BA），在回环检测后优化所有的关键帧和点（全局BA），使用g2o L-M优化,

纯运动BA（motion-only BA）优化相机的旋转矩阵 R∈SO(3)和位置 t∈R3，最小化相匹配的世界坐标系下的三维点 Xi∈R3 和关键点 xi*(⋅)之间的重投影误差（单目点 xim∈R2/双目点 xis∈R3*)
{R,t}=argminR,t∑i∈Xρ(∥x(⋅)i−π(⋅)(RXi+t)∥Σ2) \begin{equation} \lbrace \mathbf {R}, \mathbf {t}\rbrace = \text{argmin}_{\mathbf {R}, \mathbf {t}} \sum _{i\in \mathcal {X}} \rho \left(\left\Vert \mathbf {x}^i_\mathrm{(\cdot)}-\pi _\mathrm{(\cdot)}\left(\mathbf {R}\mathbf {X}^i + \mathbf {t}\right)\right\Vert ^2_\Sigma \right) \end{equation} {R,t}=argminR,t​i∈X∑​ρ(​x(⋅)i​−π(⋅)​(RXi+t)​Σ2​)​​

$${\pi }_{\mathrm{m}}\left(\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\right)=\left[\begin{array}{c}{f}_x\frac{X}{Z}+c_x\\ [0.2em]f_y\frac{Y}{Z}+c_y\end{array}\right],{\pi }_{\mathrm{s}}\left(\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\right)=\left[\begin{array}{c}{f}_x\frac{X}{Z}+c_x\\ [0.2em]f_y\frac{Y}{Z}+c_y\\ [0.2em]f_x\frac{X-b}{Z}+c_x\end{array}\right]$$

局部BA对一个关联可见的关键帧KL集合和这些关键帧中所有可见的点PL。所有不在KL中，但也观测到PL中的点的其它关键帧KF，也会参与到代价函数的计算中，但是不会被优化，局部BA相较于纯运动BA，增加了优化该点在地图中的3D坐标，即优化了一些主要的关键帧以及关键帧中可见的所有点。

全局BA是局部BA的一种特殊情况，在全局BA中，除了初始关键帧用来消除计算自由度而被固定之外，所有关键帧和地图点都会被优化。

回环检测如果在优化运行的同时发现了新的回环，就停止优化，转而去闭合回环，这将再次启动全局BA优化。当全局BA完成时，就需要将全局BA优化更新后的关键帧和点的集合，与在优化过程中插入的未更新的关键帧和点，进行融合

DetectLoop：检测闭环它的主要流程包括：
a： 如果地图中的关键帧数小于10，那么不进行闭环检测
b：获取共视关键帧，并计算他们和当前关键帧之间的BoW分数，求得最低分
c：通过上一步计算出的最低分数到数据库中查找出候选关键帧，这一步相当于是找到了曾经到过此处的关键帧们
d：对候选关键帧集进行连续性检测

CorrectLoop：根据闭环做校正主要流程包括：
a：如果有全局BA运算在运行的话，终止之前的BA运算。
b：使用传播法计算每一个关键帧正确的Sim3变换值
c：优化图
d：全局BA优化

4 关键帧插入策略

策略：频繁插入关键帧，之后再剔除冗余的关键帧。当环境中存在很大一块场景远离双目传感器时，需要有足够多的的近处点来精确地估计平移量，因此，当追踪的近处点数目低于τt =70/100并且此帧能够创建至少τc个新的近处立体点时，系统就会将此帧作为一个新的关键帧的筛选：

首先是直接不插入关键帧的条件，主要有三个：

1）若当前处于纯定位模式，不插入关键帧；　原因：定位模式是只定位不建图，只利用已有的地图点对当前相机姿态进行估计，而不更新地图点，因此没有插入关键帧的必要。
2）若局部地图被回环线程占用，不插入关键帧；　　原因：局部地图被占用，插入关键帧会影响全局闭环的优化。
3）若距离上一次重定位后还没经过20帧，不插入关键帧；　　　原因： 关键帧数量够多且刚刚重定位，这时系统比较稳定，不用插入。mMaxFrames是源码中预设的最大值 = 相机帧率。

排除了上面三种情况，接下来就需要对关键帧进一步检测，才能确定是否真的需要插入关键帧。进一步检查包含一个必不可少的条件和另外三选一的条件。

必须满足的条件：
1）内点数必须超过设定的最小阈值15，并且重叠度不能太大；　原因：保证关键帧跟踪的质量，同时，避免引入过多的信息冗余。
　　三选一的条件：
1）距离上次插入关键帧已经过去MAX：20帧；　原因：防止过去太多帧，跟踪丢失
2）距离上次插入关键帧至少过去MIN帧且局部建图线程处于空闲状态，MIN是ORBSLAM2预设的最小值；　　原因：防止图像重叠度太高，并且局部建图线程有空闲处理新的关键帧。
3）参考帧中有75%的点跟踪不到了；

关键帧的插入过程：

确定了当前帧是关键帧，那么就需要将其输入局部建图模块以及回环检测模块。但是在输入这两个模块之前，需要将关键帧的观测信息进行更新。
1）中断局部建图线程，即中断处理地图点、关键帧以及优化相机位姿； 原因：这是必须插入关键帧的情况，强制终止局部建图。
2）更新当前关键帧的位姿，取出旋转矩阵 R和位移向量t 以及相机中心 O；　原因：主要用于局部建图模块和回环检测模块中做重投影。
3）将当前帧所对应的地图点按深度信息进行排序；　原因：筛除超出深度阈值的地图点。
4）对当前关键帧中未关联的三维点，创建新的地图点，具体包括观测信息，更新地图点的描述子以及法向量和深度；　　原因：非常平凡的创建地图点的过程。
5）将所创建的地图点插入地图中，最后将更新后的关键帧也插入地图中。原因：插入关键帧的过程最后一个步骤。

KeyFrameCulling：剔除冗余关键帧-----删除关键帧–>严出
1：根据Covisibility Graph提取当前帧的共视关键帧
2：对所有的局部关键帧进行遍历，提取每个共视关键帧的MapPoints
3：遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到
4：该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧

Vins-Mono关键帧选取

1 与前一帧的平均视差。如果跟踪特征的平均视差超过某个阈值，我们会将此图像视为关键帧。（纯旋转导致的视差不能作为选取条件，使用IMU积分结果对旋转进行补偿）

2 另一个是跟踪质量。如果跟踪特征的数量低于一个阈值，我们把这一帧看做一个新的关键帧。