ORBSLAM3閱讀筆記1 System

主要參考：https://blog.csdn.net/qq_41861406/article/details/124657714
System.cc這個文件主要是SLAM系統的初始化、跟蹤線程的入口、地圖的保存和加載以及軌跡的保存。

System構造函數

在主函數的調用：

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

/// @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<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)
{
    // 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;
    else if(mSensor==IMU_MONOCULAR)
        cout << "Monocular-Inertial" << endl;
    else if(mSensor==IMU_STEREO)
        cout << "Stereo-Inertial" << endl;
    else if(mSensor==IMU_RGBD)
        cout << "RGB-D-Inertial" << endl;

    //Check settings file
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       exit(-1);
    }
    //查看配置文件版本
    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;
        }
    }

    //是否激活迴環
    node = fsSettings["loopClosing"];
    bool activeLC = true;
    if(!node.empty())
    {
        activeLC = static_cast<int>(fsSettings["loopClosing"]) != 0;
    }
    //詞袋文件路徑
    mStrVocabularyFilePath = strVocFile;

    // 多地圖管理功能，加載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
        mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

        //Create the Atlas
        cout << "Initialization of Atlas from scratch " << endl;
        //創建多地圖，參數0表示初始化關鍵幀id爲0
        mpAtlas = new Atlas(0);
    }
    else
    {
        //Load ORB Vocabulary
        cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

        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;

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

        cout << "Load File" << endl;

        // Load the file with an earlier session
        //clock_t start = clock();
        cout << "Initialization of Atlas from file: " << mStrLoadAtlasFromFile << endl;
        //加載多地圖系統
        bool isRead = LoadAtlas(FileType::BINARY_FILE);

        if(!isRead)
        {
            cout << "Error to load the file, please try with other session file or vocabulary file" << endl;
            exit(-1);
        }
        //mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);


        //cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " << mpKeyFrameDatabase->mnNumWords << endl;

        loadedAtlas = true;

        mpAtlas->CreateNewMap();

        //clock_t timeElapsed = clock() - start;
        //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000);
        //cout << "Binary file read in " << msElapsed << " ms" << endl;

        //usleep(10*1000*1000);
    }

    // 如果是有imu的傳感器類型，設置mbIsInertial = true;以後的跟蹤和預積分將和這個標誌有關
    if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR || mSensor==IMU_RGBD)
        mpAtlas->SetInertialSensor();

    //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)
    // ?(1)創建跟蹤線程（主線程）,不會立刻開啓,會在對圖像和imu預處理後在main主線程種執行
    //this 是 C++ 中的一個關鍵字，也是一個 const 指針，它指向當前對象，通過它可以訪問當前對象的所有成員。
    cout << "Seq. Name: " << strSequence << endl;
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);

    //Initialize the Local Mapping thread and launch
     //?(2)創建並開啓local mapping線程，線程函數爲LocalMapping::Run()
    mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR,
                                     mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence);
    //initFr表示初始化幀的id，代碼裏設置爲0
    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)創建並開啓閉環線程,線程主函數爲LoopClosing::Run()
    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);

    //usleep(10*1000*1000);

    //Initialize the Viewer thread and launch
    // ?(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;
    }

    // Fix verbosity
    //設置打印信息的等級，只有小於這個等級的信息纔會被打印
    Verbose::SetTh(Verbose::VERBOSITY_QUIET);

}

• 跟蹤線程的入口，在main函數中被調用

  std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);//在SyncWithImu中被調用

TrackMonocular TrackRGBD TrackStereo

/// @brief 單目/單目VIO跟蹤
/// @param im 灰度圖像
/// @param timestamp 圖像時間戳
/// @param vImuMeas 上一幀到當前幀圖像的IMU測量值
/// @param filename 調試用的文件名
/// @return 當前幀的位姿Tcw
Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)
{

    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbShutDown)
            return Sophus::SE3f();
    }
    //確保是單目或者VIO模式
    if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular nor Monocular-Inertial." << endl;
        exit(-1);
    }
    //根據是否需要重置圖像尺寸，返回重定義尺寸的灰度圖
    cv::Mat imToFeed = im.clone();
    if(settings_ && settings_->needToResize()){
        cv::Mat resizedIm;
        cv::resize(im,resizedIm,settings_->newImSize());
        imToFeed = resizedIm;
    }

    // Check mode change
    {
        // 獨佔鎖，主要是爲了mbActivateLocalizationMode和mbDeactivateLocalizationMode不會發生混亂
        unique_lock<mutex> lock(mMutexMode);

        // mbActivateLocalizationMode爲true會關閉局部地圖線程，僅跟蹤模式
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();//請求停止局部建圖線程

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            //告訴Tracking線程，現在只進行跟蹤只計算相機位姿，不進行局部地圖的更新
            mpTracker->InformOnlyTracking(true);
            // 關閉線程可以使得別的線程得到更多的資源
            // 設置爲false，避免重複進行以上操作
            mbActivateLocalizationMode = false;
        }
        //關閉純定位模式，重新打開局部建圖線程
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset 檢查是否重置
    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset)
        {
            mpTracker->Reset();
            mbReset = false;
            mbResetActiveMap = false;
        }
        // 如果檢測到重置活動地圖的標誌爲true,將重置地圖
        else if(mbResetActiveMap)
        {
            cout << "SYSTEM-> Reseting active map in monocular case" << endl;
            mpTracker->ResetActiveMap();
            mbResetActiveMap = false;
        }
    }
    // 如果是單目VIO模式，把IMU數據存儲到隊列mlQueueImuData
    if (mSensor == System::IMU_MONOCULAR)
        for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
            mpTracker->GrabImuData(vImuMeas[i_imu]);
    
    //計算相機位姿
    Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);

    // 更新跟蹤狀態和參數
    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;//記錄跟蹤狀態
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//當前幀的地圖點
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//當前幀的去畸變後關鍵點

    return Tcw;//返回世界座標系到相機的位姿
}

TrackStereo與單目基本相同

/// @brief 雙目或者雙目+IMU模式
/// @param imLeft Left
/// @param imRight Right
/// @param timestamp 時間戳
/// @param vImuMeas 上一幀到當前幀圖像的IMU測量值
/// @param filename 調試用的文件名
/// @return 當前幀的位姿Tcw
Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)

多了一個圖像需要矯正時進行矯正

//圖像需要矯正時進行矯正
    cv::Mat imLeftToFeed, imRightToFeed;
    if(settings_ && settings_->needToRectify()){
        cv::Mat M1l = settings_->M1l();
        cv::Mat M2l = settings_->M2l();
        cv::Mat M1r = settings_->M1r();
        cv::Mat M2r = settings_->M2r();

        cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR);
        cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR);
    }

TrackRGBD與單目基本相同

/// @brief RGBD模式跟蹤
/// @param im 圖像
/// @param depthmap 深度圖 
/// @param timestamp 時間戳
/// @param vImuMeas 上一幀到當前幀圖像的IMU測量值
/// @param filename 調試用的文件名
/// @return 當前幀的位姿
Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, const vector<IMU::Point>& vImuMeas, string filename)

保存位姿的一些函數

/// @brief 以TUM格式保存所有成功定位幀的位姿
/// @param filename 
void System::SaveTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
    //不能用於單目估計是因爲沒有尺度信息
    if(mSensor==MONOCULAR)
    {
        cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
        return;
    }

    vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();//獲取地圖集中所有關鍵幀
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//按照ID排序

    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    Sophus::SE3f Two = vpKFs[0]->GetPoseInverse();//第一幀的位姿，也就是世界座標系

    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
    // We need to get first the keyframe pose and then concatenate the relative transformation.
    // Frames not localized (tracking failure) are not saved.
    //對於每一幀，我們都有一個參考關鍵幀（lRit）、時間戳（lT）和一個在跟蹤失敗時爲真的標誌（lbL）。
    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
    // which is true when tracking failed (lbL).
    // !每一幀都有一個參考關鍵幀，相對於參考關鍵幀有一個相對位姿，所以每一幀的位姿是按相對參考關鍵幀位姿我們需要先獲得關鍵幀姿勢，然後連接相對變換,再進行保存。
    list<ORB_SLAM3::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();//參考關鍵幀
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();//時間戳
    list<bool>::iterator lbL = mpTracker->mlbLost.begin();//是否跟蹤失敗
    //相對參考關鍵幀的位姿
    for(list<Sophus::SE3f>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
    {
        if(*lbL)//如果跟蹤失敗，則跳過，不保存這一幀
            continue;

        KeyFrame* pKF = *lRit;//取出參考關鍵幀

        Sophus::SE3f Trw;

        // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
        while(pKF->isBad())//如果參考關鍵幀是壞的(可能是檢查冗餘時被剔除了)，則用其父幀，並記錄父幀到原參考關鍵幀的位姿變換
        {
            Trw = Trw * pKF->mTcp;//父幀到原參考關鍵幀的位姿變換
            pKF = pKF->GetParent();//父幀
        }

        Trw = Trw * pKF->GetPose() * Two;//參考關鍵幀的位姿

        //參考關鍵幀到當前幀的相對位姿*參考關鍵幀位姿=當前幀位姿
        Sophus::SE3f Tcw = (*lit) * Trw;
        Sophus::SE3f Twc = Tcw.inverse();

        Eigen::Vector3f twc = Twc.translation();//相機到世界的平移
        Eigen::Quaternionf q = Twc.unit_quaternion();//以四元數的形式輸出

        f << setprecision(6) << *lT << " " <<  setprecision(9) << twc(0) << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
    }
    f.close();
    // cout << endl << "trajectory saved!" << endl;
}

/// @brief 以TUM格式保存所有關鍵幀的位姿
/// @param filename 
void System::SaveKeyFrameTrajectoryTUM(const string &filename)

/// @brief 以EuRoC格式保存所有幀的位姿
/// @param filename 
void System::SaveTrajectoryEuRoC(const string &filename)

// @brief 以EuRoC格式保存所有幀的位姿
/// @param filename 
/// @param pMap 
void System::SaveTrajectoryEuRoC(const string &filename, Map* pMap)

/// @brief 以EuRoC格式保存所有關鍵幀的位姿
/// @param filename 
void System::SaveKeyFrameTrajectoryEuRoC(const string &filename)

/// @brief 以EuRoC格式保存所有關鍵幀的位姿
/// @param filename 
/// @param pMap 
void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, Map* pMap)

/// @brief 以KITTI格式保存所有幀的位姿
/// @param filename 
void System::SaveTrajectoryKITTI(const string &filename)