ORB-SLAM2:an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras——阅读笔记(二)

III ORB-SLAM2

用于双目和RGB-D相机的ORB-SLAM2建立在单目基于特征的ORB-SLAM上，其主要组成被总结。Fig. 2显示系统的概述图。

系统包含三个并行线程：

1. tracking跟踪 通过寻找和局部地图的特征匹配以及应用纯运动BA最小化重投影误差，利用每帧图像定位相机位姿；
2. Local mapping局部建图 执行局部BA，管理局部地图并优化局部地图；
3. loop closing回环检测 检测回环，并通过执行位姿图优化，纠正累计漂移。
   位姿图优化后，这个线程会启动第四个线程执行全局BA，计算最优结构和运动解。
   该系统嵌入了基于DBOW2的位置识别模块，用于重新定位，在跟踪失败（例如遮挡）的情况下或用于在已经建图的场景中重新初始化，以及用于回环检测。系统维护了一个共视图，该图链接了观察公共点的任意两个关键帧，并维护了连接所有关键帧的最小生成树。这些图结构允许检索关键帧的局部窗，以便跟踪和局部建图，以及允许工作在大场景中，当闭环时，能用于位姿图优化。
   这个系统利用相同的ORB特征进行跟踪、建图、位置识别任务。这些特征具有旋转和尺度不变性，并对相机自动增益、自动曝光和光线变化提供良好的不变性。此外，能实时提取和匹配ORB特征，在词袋位置识别中表现良好的精度和性能。
   下面是立体深度信息如何被利用，系统组成部分的影响。

A Monocular，Close Stereo and Far Stereo Keypoints单目，近处双目和远处双目关键点

ORB-SLAM2作为基于特征的方法进行预处理，在显著的关键点位置提取特征，如图Fig. 2b所示。然后，输入图像被丢弃，所有系统操作基于这些特征，以便系统不依赖于双目或者RGB-D。系统处理单目和双目关键点，进一步分为近处点和远处点。
Stereo Keypoints 双目关键点 三维坐标表示 $x_s = (u_L, v_L, u_R)$，左边图像坐标 $(u_L, v_L)$，右边图像水平坐标 $u_R$。对于双目相机，在两边图像中提取ORB，并对左右图像的ORB进行匹配。假设立体校正图像，这可以非常有效地完成，使得极线是水平的。然后，利用左边ORB的坐标和右边匹配的水平坐标产生双目关键点，其是通过分片相关细化的子像素。对于RGB-D相机，在RGB图像上提取ORB特征，如Strasdat提出的。对于每个特征坐标 $(u_L, v_L)$，转换深度值 $d$ 为虚右坐标：
$u_R = u_L - \frac{f_xb}{d}$
$f_x$ 水平焦距长度
$b$ 在结构光投影仪和红外照相机之间的基线，对于Kinect和Asus Xtion大约8cm。深度传感器的不确定性表现为虚拟直角坐标系的不确定性。这样，来自双目和RGB-D的特征输入被系统其余部分公平对待。
如果双目关键点的相关深度小于双目/RGB-D基线的40倍时，双目关键点被分类为近处点，其余的被分类为远处点。当深度被精确估计，并提供尺度、平移、旋转信息时，可从一个帧中安全的三角化近处的关键点。另一方面，远处的关键点提供了精确的旋转信息，但尺度和平移信息较弱。当远处的关键点被多个视图支持时，我们三角化远处的关键点。
Monocular Keypoints单目关键点 由左边图像的坐标 $(u_L, v_L)$ 定义，并对应于所有的双目匹配无法找到或者在RGB-D中无效深度情况下的ORB特征。这些点只从多个视图中三角化，不提供尺度信息，但有助于旋转和平移估计。

B System Bootstrapping系统自荐

使用双目或RGB-D相机的主要优点，仅从一帧中获得深度信息，将不需要像单目相机的情况中特定结构用于运动初始化。在系统启动时，我们使用第一帧创建一个关键帧，将其姿势设置为初始状态，并从所有双目关键点创建一个初始化地图。

C Bundle Adjustment with Monocular and Stereo Constraints

系统执行BA优化在跟踪线程中的相机位姿（Motion-only BA），优化局部建图线程中关键帧的局部窗以及匹配点（local BA），闭环后优化所有关键帧和匹配点（full BA）。
应用g2o中的Lavenberg-Marquardt方法。
Motion-only BA 优化相机旋转矩阵 $R \in SO(3)$ 和位置 $t \in R^3$ ，最小化特征点 $x_{(.)}^i$ 与匹配的世界坐标系下的3D点 $X^i \in R^3$ 之间的重投影误差，不论单目 $x_m^i \in R^2$ 还是双目 $x_s^i \in R^3$ ， $i \in \chi$ 为所有匹配项集合：

$\rho$ 鲁棒Huber代价函数； $\Sigma$ 和特征点尺度相关的协方差矩阵
投影函数 $\pi_{(.)}$， 单目为 $\pi_m$ 矫正双目为 $\pi_s$

$(f_x, f_y)$ 是焦距长度 $(c_x, c_y)$ 是焦点， $b$ 是基线。
Local BA 优化共视关键帧集合 $K_L$ 以及这些关键帧中所有可视点 $P_L$。所有不在 $K_L$中的其他关键帧 $K_F$中在 $P_L$ 中的可视点作用于代价函数，但是优化过程保持不变。定义 $\chi_k$ 作为 $P_L$ 中的点和关键帧 $k$ 中特征点的匹配点集合，优化问题如下：

Full BA 是局部BA的特殊情况，优化所有在地图中的关键帧和匹配点，除了用于消除测量自由度的原始关键帧之外。

D Loop Closing and Full BA

回环检测包含两步：
首先需要检测和验证一个回环，然后对回环进行校正，优化一个位姿图。
相比于单目ORB-SLAM，其尺度可能发生漂移，立体/深度信息使得尺度可观测，几何验证和姿态图优化不再需要处理尺度漂移，并且是基于刚体变换而不是相似性。
在ORB-SLAM2中，我们在位姿图之后加入了一个完整的BA优化来实现最优解。这种优化代价很高，因此我们在一个单独的线程中执行它，允许系统同时创建地图和检测回环。 然而，这带来了将BA输出与地图的当前状态融合的困难。如果在全局BA运行时检测到新的闭环，我们将中止全局BA并进行闭环，然后再次启动完整的全局BA优化。当全局BA结束时，将全局BA优化的关键帧和点的子集进行更新，并与在全局BA运行时未更新的关键帧和插入的点进行融合。通过生成树将更新关键帧的校正（例如，未优化姿态到已经优化姿态之间的转换）传递到未更新的关键帧中，未更新点根据参考关键帧的矫正进行变换。

E Keyframe Insertion

ORB-SLAM2遵循单目ORB-SLAM中引入的策略，即经常插入关键帧，然后剔除冗余的关键帧。近距离双目点和远距离双目点之间的区别允许我们引入关键帧插入的新条件，在场景的大部分远离双目传感器的具有挑战性的环境中，如图Fig. 3所示。在如此环境下，需要足够的近处点去精确估计平移，因此，如果被跟踪的近处点的数量下降到 $\tau_t$ 以下，并且帧可以创建至少 $\tau_c$ 个新的近处双目点，则系统将插入新的关键帧。经验值为 $\tau_t = 100 , \tau_c = 70$

F Localization Mode

引入一种定位模式：只要环境没有重大变化时，适用于轻量级以及在地图已知情况下长期的运行。
在定位模式中，停止局部建图和回环检测线程，始终通过追踪线程进行重定位。追踪模块使用视觉里程计匹配当前帧特征点和先前帧由双目或深度相机得到的3D点云。在未知地图区域会产生累计漂移，在已知地图中能实现零漂移定位。

参考博文：https://blog.csdn.net/qq_41839222/article/details/87945367