【SLAM】ORB-SLAM3解析——综述(1)

之前学习VINS和LIO-SAM的时候都是代码流，不是很重视看论文，现在有空学ORB-SLAM3了，这一次，先看一下论文。考虑到边上班边学，更新的会比较慢。看完论文之后，对这个系统最大的主观感受就是它的多图系统Atlas。机器人运行过程中是可能会跟丢的，一旦跟丢了就新建一个map，如果走到之前的位置，一旦发现和之前的map是同一个地方，就可以合并map。它的初始化和VINS很像，地图操作，回环检测，重定位很鲁棒。

github： https://github.com/UZ-SLAMLab/ORB_SLAM3
论文：   https://arxiv.org/pdf/2007.11898.pdf

相关代码解析如下(未完待续)：

【SLAM】ORB-SLAM3解析——综述(1)

【SLAM】ORB-SLAM3解析——关键帧Frame()的构建(2)

【SLAM】ORB-SLAM3解析——视觉里程计Track()(3)

【SLAM】ORB-SLAM3解析——Mapping(4)

【SLAM】ORB-SLAM2代码流程图

Abstract

ORB-SLAM3有以下特点：基于ORB-SLAM2和ORB-SLAM-VI开发的，支持V-SLAM(mono, stereo),VI-SLAM,RGBD-SLAM模式,相机可以是针孔或鱼眼。
它的第一大特点是基于特征点的VI-SLAM，依赖极大后验估计MAP，哪怕和IMU的初始化都用MAP，一说这个我就想到了最小二乘和BA。
它的第二大特点是具备multiple map system，所以说它的定位精度，回环检测，重定位能力很顶。

I.Introduction & II.Related Work

现在的V-SLAM和VO都依赖于极大后验估计MAP，具体的来说就是Bundle Adjustment最小化重投影误差。V-SLAM的目的既包括状态估计也包括建图，而VO更关注于状态估计。
V-SLAM的优势体现在它能利用上以下三个信息：

①短期数据匹配：短时间内一帧和一帧之间的联系来恢复运动。这是V-SLAM和VO的一个共同点，不过长时间会造成漂移。
②中期数据匹配：把当前相机状态和全局地图中没有漂移的那部分进行匹配。这比带回环检测的VO效果都好。
③长期数据匹配：回环检测。

说白了，就是当前的状态估计分别可以用 当前帧 与 次新帧，比较准的全局地图(同时也可能比较新) 和 比较久远的全局地图 分别匹配进行BA，这样定位精度很高没有drift，作者说这是ORB-SLAM3的优点。

它的重定位和一般用词袋模型的VIO/VO不一样，词袋模型是先根据词袋匹配，再判断位置一致性，这样的缺点是慢(VINS好像就是这么干的)。
ORB-SLAM3先根据位置找可能的回环(可能是对定位很自信吧)，然后再用3个共视关键帧确定回环，这么做会更精确，但是会有点费算力。
Atlas，也就是multiple map system，结合了重定位，地图识别，回环检测等技术，用这些方式可以把几块地图(可以是几次不同的运行)拼在一起。所以一旦跟丢了，系统就建一个新图，如果走着走着和之前匹配上了就可以再连起来。

ORB-SLAM3用ORB特征进行短期，中期的数据进行匹配；通过创建的共视图covisibility graph可以减少跟踪和建图的复杂度，回环检测和重定位用了词袋实现长期的数据匹配；作者说ORBSLAM3是唯一集齐以上3个手段的系统，是它定位准确的关键所在。
ORB-SLAM3的初始化是基于ORB-SLAM-VI并拓展成stereo的，基于极大后验估计，2s做到误差5%，15s做到误差1%。

III.System Overview 

ORB-SLAM3包括以下几个部分：
Atlas：也就是多地图系统，由一系列离散的地图组成。其中有一个active map，tracking线程基于这个地图进行定位，local map线程会把新的关键帧加到这个地图里；除此以外还有很多non-active map；还有一个DBoW2关键帧数据集用于重定位，回环检测和地图合并。
Tracking：处理传感器数据；通过最小化重投影误差实时计算最新位姿；选择关键帧；VI模式下，计算body速度和IMU误差；如果跟丢了，在Atlas多图系统中进行重定位，成功了就切换对应地图，失败了就新建一个地图。
Local Mapping：把新的关键帧和地图点加到active map里，删掉多余点和多余关键帧；在最新的滑窗内进行BA优化地图；初始化时估计IMU参数。
Loop和Map Merging：以关键帧的频率搜索active map和Atlas里其它map的共同区域，如果这个共同区域属于active map，就搞一次回环矫正，如果属于别的map，这两个map合并，然后变成active map；回环矫正后，会新建一个线程进行全局BA优化位姿。

上图是ORB-SLAM3的总体流程。Tracking是前端，包括IMU预积分，关键帧的构建，然后是Local Mapping，这两步就构成了最基础的里程计。图片中间是它最具特色的Atlas多图系统，包括DBoW2词袋数据库，当前正在使用的Acitive Map，和历史保存的多个Inactive Map。一旦在Loop中发现了Place recognition，就会把Active Map和Loop Map进行merging，再进行Full BA。 

IV.相机模型

整个系统是假设为针孔模型的，所以为了适用其它相机模型(鱼眼)，把相机抽象出来。对于鱼眼相机而言(FOV>180°)，去畸变会出现问题(外围物体放大，中间物体损失分辨率)。如果要像针孔相机那样去畸变，就要把照片外围裁掉，会损失信息。
对于双目而言，这个问题的影响更大，为了解决这个问题，他们把双目是为两个单目，而这两个单目之间有着固定的SE3和共视区。开始时通过共视区特征的三角化获得真实scale，之后就按照单目的玩法来。

V.VI-SLAM

A. 公式

相较于ORB-SLAM-VI，当前系统的初始化的速度更快了。在V-SLAM里，只用估计相机位姿，但是在VI-SLAM里，还需要估计body位姿，世界坐标系下的速度，IMU误差，同样需要i到i+1帧之间进行IMU预积分。公式(2)表示的是由IMU获得的状态关系所计算的残差。

公式(3)表示由相机模型获得的重投影误差，如果给定一系列的关键帧，状态量和路标点，那么优化问题就是最小化IMU误差和重投影误差，如公式(4)，是不是很眼熟。

B. IMU初始化

目标是获得body速度，重力方向，IMU bias。光看论文感觉这初始化的办法和VINS-Mono的很像啊。
初始化有3个关键点：
->纯单目初始化能提供非常精确的map，仅scale不知道；
->如果把scale当成一个被优化的变量，速度会更快；
->不能忽视IMU的误差。

所以，他们把IMU初始化作为一个MAP估计问题，并分为3步：
①仅有视觉的MAP估计：2s进行mono-slam初始化，以4Hz插入关键帧，获得10个相机位姿和几百个点，然后来一个纯视觉BA。
②仅有IMU的MAP估计：优化目标是重力向量，IMU bias，尺度，速度。公式(8)是优化目标。
③VI MAP估计：这部分论文说的不是很具体，到时候看看代码是咋实现的把。

对于初始化时运动量过小导致初始化失败的情况，他们就只优化尺度和重力，至于IMU bias就在优化部分处理了。

C. Tracking & Mapping 

Tracking:  最后2帧会被优化，路标点会被固定。
Mapping:  维护了一个滑窗，pose会被固定，路标点会被优化。

D. Track Loss

暂时跟丢了：根据IMU估计位姿，把已有地图点根据这个位姿投影到照片上进行匹配，然后优化位姿，连这都不行那就是跟丢了。
彻底跟丢了：重开一张新地图。

VI.地图合并和回环检测部分

短期和中期的数据匹配是通过重投影实现的，长期数据匹配依赖词袋。在传统的词袋模型回环检测中，先根据词袋确定潜在回环，为了提高准确率然后再根据位置关系筛选，从而做到100%准确率和30-40%的召回率。
但是这么玩的结果就是认出的回环帧会延后3帧，而且总在相同的位置找到回环(不懂)。

ORB-SLAM3的Atlas这么搞：
每来一个关键帧，就在Atlas里找关联，如果关联的关键帧在active map里(见D.)，就来一次回环矫正，否则就把active map和被关联的关键帧所在的map进行合并(见B.和C.)。
一旦回环关系找到了，就分别把回环两帧和各自(共视图里的)相邻帧捞出来，他们作为一个local window，在其中进行稠密的mid-term数据匹配。

A. 回环检测

ORBSLAM3这部分要比VINS复杂的多。
(1)利用DBoW2词袋找到与重定位帧Ka最像的3帧Km做为备胎回环帧，它们不能是重定位帧Ka的共视帧；
(2)对于每一个Km，都能借助DBoW2找到它与Ka之间共视路标点的对应关系，包括2D和3D坐标；
(3)根据匹配点+RANSAC计算Km帧到Ka帧的位姿变换Tam；我们知道RANSAC每次迭代会随机取少量的数据去拟合一个模型，用剩下的点计算和这个模型的距离确定是它是内点还是外点，最后内点数最多的模型为结果。在这里，重投影误差小于阙值的点会作为内点；
(4)Tam求出来后，各自的特征点相互之间重投影找到更多的匹配关系，然后玩一次非线性优化来优化Tam；
(5)为了避免假阳性，会在active map里再在Ka帧的共视帧找2个小伙伴，与回环帧验证回环关系；
(6)对于带IMU的模式，会增加pitch和roll约束限定匹配关系；

B. Visual模式下的Map合并

重定位帧Ka的map是Ma(active map),回环帧Km的map是Mm。B.和C.针对的是重定位帧回环帧在不同的map下这种情况。
(1)将Ma和Mm合并到同一个window下。上一步把Tma求出来了，那么就把Ka帧和它的共视帧，特征点都用Tma变换到Mm的坐标系下；
(2)合并之后就会有重复的路标点，那么就把Ka帧对应的重复点删掉，之后利用mid-term的数据匹配建立共视图；
(3)在这个window里进行BA，Mm中那些不在window里的关键帧的pose会被固定住，其余所有window中的路标点和关键帧pose都会被优化；
(4)全局位姿图优化。window里的pose会被固定，其余pose会被优化。

C. VI模式下的Map合并

和Visual模式下的Map合并基本一样，就是(1)(3)会有补充操作；
(1)如果Ma成熟了，那么Tma是SE(3)，否则是Sim(3)；
(3)优化项增加了速度和bias。Mm中，window之前紧挨着的5帧会被固定住不优化，所有的路标点，Km，Ka帧和他们的共视帧都会根据重投影模型被优化。

D. Loop Closing

本部分针对的是重定位帧回环帧都在active map下这种情况。对于匹配上的两帧同样会创建一个window，它们和共视路标点也会构造一个共视图，然后对map中其它帧进行一次位姿图优化。
最后根据mid-term和long-term的数据匹配关系进行一次全局BA。对于VI模式，全局BA仅在关键帧数量不多时进行，以控制计算量。

1. 系统入口

入口函数都在\Examples\目录下，这里以Stereo-Inertial\stereo_inertial_euroc.cc为例逐渐进入到系统内部，其它的入口函数都是类似的。我们看他的main(),开始时会创建一系列输入数据的容器，保存IMU的数据和image的路径。然后就会创建一个System对象：

// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, false);

之后，把一对image，时间戳＜image的IMU数据(a,w,time)组成一帧数据，进行当前轮次的track：

// Pass the images to the SLAM system
SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas);

1.1 System对象的构建

在路径\src\System.cc。构造函数中，会把整个SLAM系统涉及到的主要功能类(Tracking, Mapping, Loop Closing, Viewer)和数据结构类(Vocabulary, KeyFrame, Atlas)作为成员变量进行实例化。同时，会把相互依赖的功能类的指针传入到对应成员模块的构造函数中。

//Load ORB Vocabulary
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);

//Create KeyFrame Database
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

//Create the Atlas
mpAtlas = new Atlas(0);

//Create Drawers. These are used by the Viewer
mpFrameDrawer = new FrameDrawer(mpAtlas);
mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile, settings_);

//Initialize the Tracking thread
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);

//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence);
mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);

//Initialize the Loop Closing thread and launch
mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); // mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);

//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);

mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);

mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);

//Initialize the Viewer thread and launch
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);

以上几个功能类中，需要注意的是LocalMapping，LoopClosing和Viewer是另外开了3个线程做各自的事情。

1.2 SLAM算法入口函数TrackStereo()

在路径\src\System.cc。根据输入image关于stereo，mono，rgbd的不同，分别执行：

// Proccess the given stereo frame. Images must be synchronized and rectified.
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");

// Process the given rgbd frame. Depthmap must be registered to the RGB frame.
// Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Input depthmap: Float (CV_32F).
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");

// Proccess the given monocular frame and optionally imu data
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");

这里我们以Stereo模式为例。在这个函数里面，首先是对image的预处理，然后确定SLAM运行的模式是纯定位还是定位+建图模式，是否reset系统等，不过，最核心的内容是这4行：

if (mSensor == System::IMU_STEREO)
    for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
        mpTracker->GrabImuData(vImuMeas[i_imu]);

Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed,imRightToFeed,timestamp,filename);

GrabImuData()的作用是把IMU数据放到mlQueueImuData中。GrabImageStereo()中，最核心的也只有2行：

mCurrentFrame = Frame();
Track();

分别是构建最新帧和进行track。