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)