ORB-SLAM2源码笔记（1）——框架结构

最新推荐文章于 2025-10-12 17:17:05 发布

原创最新推荐文章于 2025-10-12 17:17:05 发布 · 1.2k 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #算法 #计算机视觉 #学习

本文详细解读了ORB-SLAM2的运行流程，包括单目、双目和RGBD模式的输入处理，关键帧跟踪、局部建图、闭环检测与融合，以及全局优化。重点介绍了rgbd_tum.cc中的核心代码实现，涉及跟踪、建图和性能分析部分。

参考博客

ORB-SLAM2代码详解01: ORB-SLAM2代码运行流程_ncepu_Chen的博客-优快云博客_orbslam2代码详解

ORB-SLAM2详解（二）代码逻辑_sylvester0510的博客-优快云博客_orbslam2代码详解

ORB-SLAM2整体框架如图所示

算法流程框架

输入。三种模式：单目、双目、RGBD

跟踪。刚初始化没有速度，所以参考关键帧跟踪。有了速度就进行恒速模型跟踪。都跟丢后进行重定位跟踪。

局部建图。输入跟踪线程里的关键帧。局部地图里的关键帧重新进行特征匹配，生成新的地图点。使用局部BA优化位姿和地图点，随后删除冗余的关键帧。

闭环。通过词袋检测是否闭环，计算当前关键帧和闭环候选关键帧的sim3位姿，然后执行闭环融合和本质图优化（帧与帧之间至少有100个共视地图点）。

全局BA。优化所有关键帧和地图点。

位置识别。导入离线训练好的字典，新输入的图像帧在线转换为词袋向量。应用与特征匹配、重定位、闭环。

地图。地图点+关键帧。关键帧根据共视地图点数目组成共视图（15个地图点），根据父子关系组成生成树。

以RGB-D为例，rgbd_tum.cc的源码：

int main(int argc, char **argv)
{
    if(argc != 5)
    {
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        return 1;
    }

    //按顺序存放需要读取的彩色图像、深度图像的路径，以及对应的时间戳的变量
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;

    //从命令行输入参数得到关联文件的路径
    string strAssociationFilename = string(argv[4]);

    //从关联文件中加载信息
    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

    //彩色图像和深度图像数据的一致性检查
    int nImages = vstrImageFilenamesRGB.size();
    if(vstrImageFilenamesRGB.empty())
    {
        cerr << endl << "No images found in provided path." << endl;
        return 1;
    }
    else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size())
    {
        cerr << endl << "Different number of images for rgb and depth." << endl;
        return 1;
    }

    //初始化ORB-SLAM2系统
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    // 创建一个容器保存跟踪时间
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

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

    cv::Mat imRGB, imD;
    //遍历图像序列中的每张图像
    for(int ni=0; ni<nImages; ni++)
    {
        //读取图像
        imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        // 判断图像是否为空
        if(imRGB.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
            return 1;
        }

#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

        // 追踪
        SLAM.TrackRGBD(imRGB,imD,tframe);

#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;

        //根据时间戳,准备加载下一张图片
        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);
    }

    //终止SLAM过程
    SLAM.Shutdown();

    //统计分析追踪耗时
    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;

    //保存最终的相机轨迹
    SLAM.SaveTrajectoryTUM("./CameraTrajectory2.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");   

    return 0;
}

//从关联文件中提取这些需要加载的图像的路径和时间戳
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    //输入文件流
    ifstream fAssociation;
    //打开关联文件
    fAssociation.open(strAssociationFilename.c_str());
    //一直读取,直到文件结束
    while(!fAssociation.eof())
    {
        string s;
        //读取一行的内容到字符串s中
        getline(fAssociation,s);
        //如果不是空行开始分解数据
        if(!s.empty())
        {
            //字符串流
            stringstream ss;
            ss << s;
            //字符串格式:时间戳 rgb图像路径 时间戳 深度图像路径
            double t;
            string sRGB, sD;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);
            ss >> t;
            ss >> sD;
            vstrImageFilenamesD.push_back(sD);
        }
    }
}

System类

System类是ORB-SLAM2系统的主类，通过调用其他模块实现跟踪、局部建图、闭环等功能。

构造函数

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer) : 
        mSensor(sensor), mpViewer(static_cast<Viewer *>(NULL)), mbReset(false), mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) {
	
	// step1. 初始化各成员变量
	// step1.1. 读取配置文件信息
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
	// step1.2. 创建ORB词袋
    mpVocabulary = new ORBVocabulary();
    // step1.3. 创建关键帧数据库,主要保存ORB描述子倒排索引(即根据描述子查找拥有该描述子的关键帧)
	mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
	// step1.4. 创建地图
    mpMap = new Map();

	// step2. 创建3大线程: Tracking、LocalMapping和LoopClosing
    // step2.1. 主线程就是Tracking线程,只需创建Tracking对象即可
	mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
	// step2.2. 创建LocalMapping线程及mpLocalMapper
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
	// step2.3. 创建LoopClosing线程及mpLoopCloser
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
            
	// step3. 设置线程间通信
	mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);
    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);
    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

这里需要注意，在创建Tracking对象时就已经产生了跟踪线程（构造函数内完成），所以Tracking线程也是主线程。主线程通过持有两个子线程的指针(mptLocalMapping和mptLoopClosing)控制子线程。

跟踪函数

针对3种不同的传感器（单目/双目/RGBD），System类中有3种追踪函数，分别为TrackRGBD、TrackStereo、TrackMonocular。它们通过调用本类中成员变量mpTracker下的GrabImageMonocular、GrabImageStereo、GrabImageRGBD计算相机位姿并保存跟踪到的地图点。

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);
    unique_lock<mutex> lock(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}

这里有点绕，梳理一下

mono_tum.cc调用System类里的TrackMonocular方法，TrackMonocular方法的实现用到了GrabImageMonocular方法，GrabImageMonocular方法是mpTracker中的一个成员函数，而mpTracker是在System初始化时就产生的Tracking类的对象。