0 系统整体流程

1 输出信息

2 初始化文件

3 初始化并运行追踪线程Tracking

Step1：地图初始化

Step2：初始化成功（mbOnlyTracking）

Step3：局部地图跟踪TrackLocalMap()

Step4：跟踪成功

Step5：跟踪失败

Step6：记录位姿信息

4 初始化并运行局部建图线程LocalMapping

Step1：局部建图线程时设置不接受关键帧

Step2：处理列表中的关键帧

Step3：根据地图点的观测情况剔除质量不好的地图点

Step4：当前关键帧与相邻关键帧通过三角化产生新的地图点

Step5：检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点

Step6：进行局部地图的BA并剔除当前帧相邻的关键帧中冗余的关键帧

5 初始化并运行回环检测线程LoopClosing

Step1：查询闭环检测队列是否有新关键帧

Step2：闭环检测

Step3：计算Sim3()变换ComputeSim3()

Step4：闭环矫正

Step5：全局BA

0 系统整体流程

首先加载图片LoadImages，加载SLAM系统，运行System::System，System::System为该系统的运行函数，后面1-5点细讲，处理完后SLAM.Shutdown()，最后保存TUM格式的相机轨迹。

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
#include<unistd.h>
using namespace std;

/**
 * @brief 获取图像文件的信息
 * @param[in]  strImagePath     图像文件存放路径
 * @param[in]  strPathTimes     时间戳文件的存放路径
 * @param[out] vstrImages       图像文件名数组
 * @param[out] vTimeStamps      时间戳数组
 */
void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps);

int main(int argc, char **argv)
{
    // step 0 检查输入参数个数是否足够
    if(argc != 5)
    {
        cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_image_folder path_to_times_file" << endl;
        return 1;
    }

    // step 1 加载图像
    // Retrieve paths to images
    // 图像序列的文件名字符串序列
    vector<string> vstrImageFilenames;
    // 时间戳
    vector<double> vTimestamps;
    LoadImages(string(argv[3]),             // path_to_image_folder
               string(argv[4]),             // path_to_times_file
               vstrImageFilenames,          // 读取到的图像名称数组
               vTimestamps);                // 时间戳数组

    // 当前图像序列中的图像数目
    int nImages = vstrImageFilenames.size();

    if(nImages<=0)
    {
        cerr << "ERROR: Failed to load images" << endl;
        return 1;
    }

    // step 2 加载SLAM系统
    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(
        argv[1],                            // path_to_vocabulary
        argv[2],                            // path_to_settings
        ORB_SLAM2::System::MONOCULAR,       // 单目模式
        true);                              // 启用可视化查看器

    // step 3 运行前准备
    // Vector for tracking time statistics
    // 统计追踪一帧耗时 (仅Tracker线程)
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    // step 4 依次追踪序列中的每一张图像
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        // step 4.1 读根据前面获得的图像文件名读取图像,读取过程中不改变图像的格式 
        im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        // step 4.2 图像的合法性检查
        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 <<  vstrImageFilenames[ni] << endl;
            return 1;
        }

        // step 4.3 开始计时
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        // step 4.4 追踪当前图像
        SLAM.TrackMonocular(im,tframe);

        // step 4.5 追踪完成,停止当前帧的图像计时, 并计算追踪耗时

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        // step 4.6 根据图像时间戳中记录的两张图像之间的时间和现在追踪当前图像所耗费的时间,继续等待指定的时间以使得下一张图像能够
        // 按照时间戳被送入到SLAM系统中进行跟踪
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

    // step 5 如果所有的图像都预测完了,那么终止当前的SLAM系统
    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    // step 6 计算平均耗时
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    // step 7 保存TUM格式的相机轨迹
    // 估计是单目时有尺度漂移, 而LGA GBA都只能优化关键帧使尺度漂移最小, 普通帧所产生的轨迹漂移这里无能为力, 我猜作者这样就只
    // 保存了关键帧的位姿,从而避免普通帧带有尺度漂移的位姿对最终误差计算的影响
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    return 0;
}

// 从文件中加载图像序列中每一张图像的文件路径和时间戳
void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps)
{
    // 打开文件
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImages.reserve(5000);
    // 遍历文件
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        // 只有在当前行不为空的时候执行
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            // 生成当前行所指出的RGB图像的文件名称
            vstrImages.push_back(strImagePath + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            // 记录该图像的时间戳
            vTimeStamps.push_back(t/1e9);

        }
    }
}

System::System的框架图如下：

ORB-SLAM2总共有三个线程: Tracking , Local Mapping ，Loop Closing线程(实际上其实还多了个Viewer线程,用于显示界面)。
输入：有三种模式可以选择：单目模式、双目模式和RGB-D模式。
跟踪：Tracking 线程主要负责定位相机以及决定是否插入一个新的关键帧。将输入的每一个帧进行提取ORB Features，并且以此估计相机位姿。
局部建图：Local Mapping 线程主要负责处理新的关键帧以及执行局部BA以此来优化相机位姿。此外，负责新的 Map Points 的创建和剔除掉冗余的KeyFrames。
闭环：Loop Closing 线程主要负责回环检测和全局地图优化，减少位姿累计误差漂移，将误差均匀化，使得总体误差最小。
全局BA：优化所有的关键帧及其地图点。
位置识别：需要导入离线训练好的字典，这个字典是由视觉词袋模型构建的。新输入的图像帧需要先在线转化为词袋向量，主要应用于特征匹配、重定位、闭环。
地图：地图主要由地图点和关键帧组成。关键帧之间根据共视地图点数目组成了共视图，根据父子关系组成了生成树。

1 输出信息

输出欢迎信息
输出当前传感器类型：ORB-SLAM2支持单目、双目、RGB-D相机
检查配置文件，打开失败输出调试信息

    // 1 输出信息
    // 输出欢迎信息
    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << endl;

    // 输出当前传感器类型
    cout << "Input sensor was set to: ";

    if(mSensor==MONOCULAR)
        cout << "Monocular" << endl;
    else if(mSensor==STEREO)
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl;

    // 检查配置文件
    cv::FileStorage fsSettings(strSettingsFile.c_str(), 	//将配置文件名转换成为字符串
    						   cv::FileStorage::READ);		//只读
    //如果打开失败，就输出调试信息
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       //然后退出
       exit(-1);
    }

    // 加载ORB字典
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
    
    // 建立一个新的ORB字典
    mpVocabulary = new ORBVocabulary();
    // 获取字典加载状态
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
    // 如果加载失败，就输出调试信息
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        //然后退出
        exit(-1);
    }
    // 否则则说明加载成功
    cout << "Vocabulary loaded!" << endl << endl;

2 初始化文件

创建ORB字典
创建KeyFrameDatabase(*mpVocabulary)
帧绘制器FrameDrawer(mpMap)
地图绘制器MapDrawer(mpMap, strSettingsFile)

	2 初始化文件
	// 加载ORB字典
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
    
    // 建立一个新的ORB字典
    mpVocabulary = new ORBVocabulary();
    // 获取字典加载状态
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
    // 如果加载失败，就输出调试信息
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        //然后退出
        exit(-1);
    }
    // 否则则说明加载成功
    cout << "Vocabulary loaded!" << endl << endl;

    // 检查关键帧数据库
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

    // 创建地图
    mpMap = new Map();

    //这里的帧绘制器和地图绘制器将会被可视化的Viewer所使用
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

3 初始化并运行追踪线程Tracking

void Tracking::Track()
{
    // track包含两部分：估计运动、跟踪局部地图
    ...
}

Step1：地图初始化

双目和RGBD相机共用一个初始化函数StereoInitialization()，单目初始化利用MonocularInitialization()

StereoInitialization()

（1）初始化要求当前帧的特征点超过500：初始化要求当前帧的特征点超过500，满足要求开始初始化。

（2）设置当前帧的状态：设置当前帧的位姿，同时将当前帧为初始关键帧pKFini，向地图mpMap中添加该初始关键帧。

（3）为每个特征点构造MapPoint：为每个特征点构造MapPoint，地图点的深度必须>0。通过反投影得到该特征点的世界坐标系下3D坐标。为该MapPoint添加属性：观测到该MapPoint的关键帧，该MapPoint的描述子，该MapPoint的平均观测方向和深度范围。

（4）在地图中添加该MapPoint并更新信息：在地图中添加该MapPoint，设置哪些特征点可以观测到该MapPoint，并将MapPoint添加到当前帧的mvpMapPoints中，建立特征点和MapPoint之间的联系。

（5）更新局部地图和其他与之相关的信息：在局部地图中更新该初始关键帧的信息，同时更新其他与之相关的信息。

（6）初始化成功

MonocularInitialization()

（1）判断单目初始器是否被创建，没有则创建，得到第一帧

（2）如果单目初始化器已经被创建，判断当前帧特征点数大于100，得到第一帧

（3）在第一帧mInitialFrame与第二帧mCurrentFrame中找匹配的特征点对

（4）如果两帧之间的匹配点太少，重新初始化

（5）通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints

（6）将三角化得到的3D点包装成MapPoints

Step2：初始化成功（mbOnlyTracking）

如果初始化成功，利用mbOnlyTracking判断跟踪模式是正常SLAM模式（定位+地图更新）还是仅定位模式；正常SLAM模式分为参考关键帧跟踪TrackReferenceKeyFrame()、恒速模型跟踪TrackWithMotionModel()、重定位跟踪Relocalization()

参考关键帧跟踪TrackReferenceKeyFrame()

（1）将当前普通帧的描述子转化为BoW向量：词袋BoW加速当前帧与参考帧之间的特征点匹配

（2）当前帧与参考帧之间的特征点匹配：匹配数目小于设定值，认为跟踪失败

（3）初始化当前帧的位姿：将上一帧的位姿态作为当前帧位姿的初始值，通过优化3D-2D的重投影误差来获得位姿

（4）剔除优化后的匹配点中的外点

（5）判断是否跟踪成功：判断匹配的特征点是否满足设定值，满足则成功

恒速模型跟踪TrackWithMotionModel()

恒速模型跟踪和参考关键帧跟踪流程类似，区别在于恒速模型跟踪有上一帧的位姿。

（1）更新上一帧的位姿：对于双目或RGB-D相机，还会根据深度值生成临时地图点

（2）根据上一帧特征点对应地图点进行投影匹配

（3）优化当前帧位姿

（4）剔除地图点中外点

（5）判断是否跟踪成功：判断匹配的特征点是否满足设定值，满足则成功

重定位跟踪Relocalization()

恒速模型跟踪和参考关键帧跟踪都失效后，开启重定位跟踪

（1）计算当前帧特征点的词袋向量

（2）找到与当前帧相似的候选关键帧：通过BoW进行匹配

（3）通过EPnP算法估计姿态

（4）通过PoseOptimization对姿态进行优化求解：只优化位姿,不优化地图点的坐标，返回的是内点的数量

（5）判断是否求解成功：防止误匹配，需要经过一系列的判断，直到找到能够匹配上的关键帧，如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解。

Step3：局部地图跟踪TrackLocalMap()

局部地图跟踪用于对位姿进一步优化

（1）更新局部关键帧和局部地图点：局部关键帧mvpLocalKeyFrames包含一些列的共视高级证，局部地图点mvpLocalMapPoints是由是所有局部关键帧的地图点。

（2）在局部地图中查找与当前帧匹配的MapPoints：其实也就是对局部地图点进行跟踪，得到更多的匹配关系

（3）更新局部所有MapPoints后对位姿再次优化：前面新增了更多的匹配关系，BA优化得到更准确的位姿

（4）统计跟踪局部地图后匹配数目：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果

（5）决定是否跟踪成功：通过判断匹配点是数目看是否成功跟踪

Step4：跟踪成功

如果跟踪成功，更新速度、位姿、地图点等，并插入新关键帧

Step5：跟踪失败

如果跟踪失败，只能重新Reset

Step6：记录位姿信息

用于最后保存所有的轨迹

4 初始化并运行局部建图线程LocalMapping

void LocalMapping::Run()
{
    // 标记状态，表示当前run函数正在运行，尚未结束
    mbFinished = false;
    // 主循环
    while(1)
    {
        ...
    }
}

Step1：局部建图线程时设置不接受关键帧

Step2：处理列表中的关键帧

包括计算BoW、更新观测、描述子、共视图，插入到地图等

（1）从缓冲队列中取出一帧关键帧

（2）计算该关键帧特征点的词袋向量

（3）处理关键帧中有效的地图点信息：更新normal，描述子等信息，对当前处理的这个关键帧中的所有的地图点展开遍历

（4）更新关键帧间的连接关系

（5）将该关键帧插入到地图中

Step3：根据地图点的观测情况剔除质量不好的地图点

（1）根据相机类型设置不同的观测阈值

（2）遍历检查新添加的地图点：已经是坏点的地图点仅从队列中删除，跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除，从该点建立开始，到现在已经过了不小于2个关键帧，从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点

Step4：当前关键帧与相邻关键帧通过三角化产生新的地图点

增加新的地图点使得跟踪更稳

（1）在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧：nn表示搜索最佳共视关键帧的数目，不同传感器设定的值不同。

（2）遍历相邻关键帧实现三角化：搜索匹配并用极线约束剔除误匹配，最终实现三角化

（3）判断相机运动的基线是不是足够长：因为太短的基线下能够恢复的地图点不稳定，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点。

（4）根据两个关键帧的位姿计算它们之间的基础矩阵

（5）生成新的匹配点对：通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对

（6）对每对匹配通过三角化生成3D点：进行一系列的检测才将新产生的点放入检测队列中

Step5：检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点

（1）获得当前关键帧在共视图中权重排名前nn的邻接关键帧

（2）存储一级相邻关键帧及其二级相邻关键帧

（3）将当前帧的地图点分别投影到两级相邻关键帧进行地图点融合：寻找匹配点对应的地图点进行融合，称为正向投影融合

（4）将两级相邻关键帧地图点分别投影到当前关键帧进行地图点融合：寻找匹配点对应的地图点进行融合，称为反向投影融合，和第（3）步相反

（5）更新当前帧地图点的描述子、深度、平均观测方向等属性

（6）更新当前帧与其它帧的共视连接关系

Step6：进行局部地图的BA并剔除当前帧相邻的关键帧中冗余的关键帧

（1）根据共视图提取当前关键帧的所有共视关键帧

（2）判断共视关键帧的地图点是否为冗余地图点：遍历该共视关键帧的所有地图点，其中能被其它至少3个关键帧观测到的地图点为冗余地图点

（3）判断共视关键帧是否为冗余关键帧：如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧

Step7：将当前帧加入到闭环检测队列中

5 初始化并运行回环检测线程LoopClosing

// 回环线程主函数
void LoopClosing::Run()
{
    mbFinished =false;

    // 线程主循环
    while(1)
    {
        // Check if there are keyframes in the queue
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来
        if(CheckNewKeyFrames())
        {
            if(DetectLoop())
            {
               if(ComputeSim3())
               {
                   CorrectLoop();
               }
            }
        }
        // 查看是否有外部线程请求复位当前线程
        ResetIfRequested();

        // 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
        if(CheckFinish())
            break;

        //usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));

	}
    // 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作
    SetFinish();
}