ORBSLAM3 运行流程 ORB_SLAM3::System SLAM()函数

ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true);

在rgbd_tum.cc里新建了System类 SLAM ：

首先看构造函数：

/**
 * @brief 系统的构造函数，将会启动其他的线程
 * @param strVocFile 词袋文件所在路径
 * @param strSettingsFile 配置文件所在路径
 * @param sensor 传感器类型
 * @param bUseViewer 是否使用可视化界面
 * @param initFr initFr表示初始化帧的id,开始设置为0
 * @param strSequence 序列名,在跟踪线程和局部建图线程用得到
 */
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer, const int initFr, const string &strSequence):
    mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false), mbResetActiveMap(false),
    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)
{
   //...
}

一、致敬

第一部分是输出原作者信息（大佬nb），并输出传感器类型：

// Output welcome message
    cout << endl <<
    "ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza." << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, 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;                 // RGBD相机   
    else if(mSensor==IMU_MONOCULAR)
        cout << "Monocular-Inertial" << endl;    // 单目 + imu
    else if(mSensor==IMU_STEREO)
        cout << "Stereo-Inertial" << endl;       // 双目 + imu
    else if(mSensor==IMU_RGBD)
        cout << "RGB-D-Inertial" << endl;        // RGBD相机 + imu

二、读取配置文件

第一部分

 cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    // 如果打开失败，就输出错误信息
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       exit(-1);
    }

strSettingsFile是argv[2]，即传进来的第三个参数，也就是yaml文件。

File.version默认是1.0，如果版本不匹配，settings_ 被设置为 nullptr，表示没有有效的设置对象。然后直接从配置文件中读取地图加载和保存路径。通过fsSettings获取这些路径，并进行相应的字符串转换。当前版本，这里的主要操作是新建class Settings类，这个类的主要作用就是加载各种参数，包括orbslam3里新增的针对各种不同的相机模型的处理操作也是在这个类的构造函数里调用readCamera()函数实现。

 cv::FileNode node = fsSettings["File.version"];
    if(!node.empty() && node.isString() && node.string() == "1.0")
    {
        settings_ = new Settings(strSettingsFile,mSensor);

        // 保存及加载地图的名字
        mStrLoadAtlasFromFile = settings_->atlasLoadFile();
        mStrSaveAtlasToFile = settings_->atlasSaveFile();

        cout << (*settings_) << endl;
    }
    else
    {
        settings_ = nullptr;
        cv::FileNode node = fsSettings["System.LoadAtlasFromFile"];
        if(!node.empty() && node.isString())
        {
            mStrLoadAtlasFromFile = (string)node;
        }

        node = fsSettings["System.SaveAtlasToFile"];
        if(!node.empty() && node.isString())
        {
            mStrSaveAtlasToFile = (string)node;
        }
    }

第二部分

由于系统默认不加载地图册，所以else不用看：

1. 激活回环检测：检查了配置文件中的loopClosing字段，如果设置为0，关闭回环检测，以决定是否激活回环检测功能。

   // 是否激活回环，默认是开着的
       node = fsSettings["loopClosing"];
       bool activeLC = true;
       if(!node.empty())
       {
           activeLC = static_cast(fsSettings["loopClosing"]) != 0;
       }
   

2. 加载ORB词袋：系统从给定路径加载ORB词袋。创建一个关键帧数据库，它利用加载的ORB词汇表来组织和搜索关键帧。

3. 关键帧数据库的创建：创建了一个关键帧数据库，它利用加载的ORB词汇表来组织和搜索关键帧。

   //词袋路径
   mStrVocabularyFilePath = strVocFile;
   
       // ORBSLAM3新加的多地图管理功能，这里加载Atlas标识符
       bool loadedAtlas = false;
   
       if(mStrLoadAtlasFromFile.empty())
       {
           //Load ORB Vocabulary
           cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
   
           // 建立一个新的ORB字典
           mpVocabulary = new ORBVocabulary();
           // 读取预训练好的ORB字典并返回成功/失败标志
           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;
   
           //Create KeyFrame Database
           // Step 4 创建关键帧数据库
           mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
   
           //Create the Atlas
           // Step 5 创建多地图，参数0表示初始化关键帧id为0
           cout << "Initialization of Atlas from scratch " << endl;
           mpAtlas = new Atlas(0);
       }

4. 地图集（Atlas）的初始化：根据配置文件，系统要么从头开始创建一个新的地图集，要么从文件中加载先前会话的地图集。地图集是用于存储和管理多个地图的数据结构。这里新建一个class Atlas，他的构造函数只有一个操作，就是创建新地图

   void Atlas::CreateNewMap()
   {
       // 锁住地图集
       unique_lock lock(mMutexAtlas);
       cout << "Creation of new map with id: " << Map::nNextId << endl;
       // 如果当前活跃地图有效，先存储当前地图为不活跃地图后退出
       if (mpCurrentMap)
       {
           // mnLastInitKFidMap为当前地图创建时第1个关键帧的id，它是在上一个地图最大关键帧id的基础上增加1
           if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
               mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; // The init KF is the next of current maximum
   
           // 将当前地图储存起来，其实就是把mIsInUse标记为false
           mpCurrentMap->SetStoredMap();
           cout << "Stored map with ID: " << mpCurrentMap->GetId() << endl;
   
           // if(mHasViewer)
           //     mpViewer->AddMapToCreateThumbnail(mpCurrentMap);
       }
       cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;
   
       mpCurrentMap = new Map(mnLastInitKFidMap);  //新建地图
       mpCurrentMap->SetCurrentMap();              //设置为活跃地图
       mspMaps.insert(mpCurrentMap);               //插入地图集
   }
   

三、创建各个线程

1、追踪线程

// 如果是有imu的传感器类型，设置mbIsInertial = true;以后的跟踪和预积分将和这个标志有关
    if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR || mSensor==IMU_RGBD)
        mpAtlas->SetInertialSensor();

    // Step 6 依次创建跟踪、局部建图、闭环、显示线程
    //Create Drawers. These are used by the Viewer
    // 创建用于显示帧和地图的类，由Viewer调用
    mpFrameDrawer = new FrameDrawer(mpAtlas);
    mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile, settings_);

    //Initialize the Tracking thread
    //(it will live in the main thread of execution, the one that called this constructor)
    // 创建跟踪线程（主线程）,不会立刻开启,会在对图像和imu预处理后在main主线程种执行
    cout << "Seq. Name: " << strSequence << endl;
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);

查看构造函数，这部分只是单纯的加载相机和词袋的各类参数，所以不会立即开启。

Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer,
    Atlas *pAtlas, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq)
    : mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false),
    mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB),
    mbReadyToInitializate(false), mpSystem(pSys), mpViewer(NULL), bStepByStep(false),
    mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0),
    mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr), mpLastKeyFrame(static_cast(NULL))
{
    // Load camera parameters from settings file
    // Step 1 从配置文件中加载相机参数
    if(settings){
        newParameterLoader(settings);
    }
    else{
        cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

        bool b_parse_cam = ParseCamParamFile(fSettings);
        if(!b_parse_cam)
        {
            std::cout << "*Error with the camera parameters in the config file*" << std::endl;
        }

        // Load ORB parameters
        bool b_parse_orb = ParseORBParamFile(fSettings);
        if(!b_parse_orb)
        {
            std::cout << "*Error with the ORB parameters in the config file*" << std::endl;
        }

        bool b_parse_imu = true;
        if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO || sensor==System::IMU_RGBD)
        {
            b_parse_imu = ParseIMUParamFile(fSettings);
            if(!b_parse_imu)
            {
                std::cout << "*Error with the IMU parameters in the config file*" << std::endl;
            }

            mnFramesToResetIMU = mMaxFrames;
        }

        if(!b_parse_cam || !b_parse_orb || !b_parse_imu)
        {
            std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
            try
            {
                throw -1;
            }
            catch(exception &e)
            {

            }
        }
    }

    initID = 0; lastID = 0;
    mbInitWith3KFs = false;
    mnNumDataset = 0;

    // 遍历下地图中的相机，然后打印出来了
    vector vpCams = mpAtlas->GetAllCameras();
    std::cout << "There are " << vpCams.size() << " cameras in the atlas" << std::endl;
    for(GeometricCamera* pCam : vpCams)
    {
        std::cout << "Camera " << pCam->GetId();
        if(pCam->GetType() == GeometricCamera::CAM_PINHOLE)
        {
            std::cout << " is pinhole" << std::endl;
        }
        else if(pCam->GetType() == GeometricCamera::CAM_FISHEYE)
        {
            std::cout << " is fisheye" << std::endl;
        }
        else
        {
            std::cout << " is unknown" << std::endl;
        }
    }

#ifdef REGISTER_TIMES
    vdRectStereo_ms.clear();
    vdResizeImage_ms.clear();
    vdORBExtract_ms.clear();
    vdStereoMatch_ms.clear();
    vdIMUInteg_ms.clear();
    vdPosePred_ms.clear();
    vdLMTrack_ms.clear();
    vdNewKF_ms.clear();
    vdTrackTotal_ms.clear();
#endif
}

 2、局部建图线程

 //创建并开启local mapping线程
    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);
    mpLocalMapper->mInitFr = initFr;

    // 设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃
    if(settings_)
        mpLocalMapper->mThFarPoints = settings_->thFarPoints();
    else
        mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
    if(mpLocalMapper->mThFarPoints!=0)
    {
        cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
        mpLocalMapper->mbFarPoints = true;
    }
    else
        mpLocalMapper->mbFarPoints = false;

    //Initialize the Loop Closing thread and launch
    // mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR
    

3、闭环线程

//Initialize the Loop Closing thread and launch
    // mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR
    // 创建并开启闭环线程
    mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); // mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);

 构造函数就是简单的初始化一下。

LocalMapping::LocalMapping(System* pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName):
    mpSystem(pSys), mbMonocular(bMonocular), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false),
    mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true),
    mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9,9))
{

    /*
     * mbStopRequested:    外部线程调用，为true，表示外部线程请求停止 local mapping
     * mbStopped:          为true表示可以并终止localmapping 线程
     * mbNotStop:          true，表示不要停止 localmapping 线程，因为要插入关键帧了。需要和 mbStopped 结合使用
     * mbAcceptKeyFrames:  true，允许接受关键帧。tracking 和local mapping 之间的关键帧调度
     * mbAbortBA:          是否流产BA优化的标志位
     * mbFinishRequested:  请求终止当前线程的标志。注意只是请求，不一定终止。终止要看 mbFinished
     * mbResetRequested:   请求当前线程复位的标志。true，表示一直请求复位，但复位还未完成；表示复位完成为false
     * mbFinished:         判断最终LocalMapping::Run() 是否完成的标志。
     */
    mnMatchesInliers = 0;

    mbBadImu = false;

    mTinit = 0.f;

    mNumLM = 0;
    mNumKFCulling=0;

#ifdef REGISTER_TIMES
    nLBA_exec = 0;
    nLBA_abort = 0;
#endif

}

 函数主体部分在mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);里

链接：

4、显示线程

 // 创建并开启显示线程
    if(bUseViewer)
    //if(false) // TODO
    {
        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
        mptViewer = new thread(&Viewer::Run, mpViewer);
        mpTracker->SetViewer(mpViewer);
        mpLoopCloser->mpViewer = mpViewer;
        mpViewer->both = mpFrameDrawer->both;
    }