【SLAM】ORB-SLAM2的源码阅读（一）：系统的整体构建-北京尧图网络

春节后又要开工啦加油ヾ(◍°∇°◍)ORB-SLAM2在SLAM界算的上是火遍大江南北无数更新的版本在github上发布这么厉害的代码怎么样也得拜读一下。原谅我的脑力有限一次性是没法读完这整套代码的当然在当中也会有不理解的地方或者理解错误的地方如果理解有误还请批评指正。ORB-SLAM2源自于ORB-SLAM,为什么有名因为完善集合了单目双目RGB-D多种模型。更重要的一点是人家开源了安装很方便并且做了运行各种大数据库的demo基本上只要编译通过什么TUMKITTI , EuRoc都可以很快的使用并且心里感觉就是哇SLAM很酷炫。但是话说回来一般很酷炫的东西要想吃透理解就是一件不是很酷炫的事儿了可能还有点痛苦毕竟看代码不像看小说书不仅逻辑思维要清晰还要明白一行行代码后的具体意义。一句话还是得耐住性子才行。ORB-SLAM2的代码量不小先要理解大概的结构在进行细节上的理解。下面的代码来自ORB-SLAM2。这是基本上构建出了SLAM的基本框架。#ifndef SYSTEM_H #define SYSTEM_H #includestring #includethread #includeopencv2/core/core.hpp #include Tracking.h #include FrameDrawer.h #include MapDrawer.h #include Map.h #include LocalMapping.h #include LoopClosing.h #include KeyFrameDatabase.h #include ORBVocabulary.h #include Viewer.h namespace ORB_SLAM2 { class Viewer; class FrameDrawer; class Map; class Tracking; class LocalMapping; class LoopClosing; class System { public: // Input sensor enum eSensor{ MONOCULAR0, STEREO1, RGBD2 }; public: // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. System(const string strVocFile, const string strSettingsFile, const eSensor sensor, const bool bUseViewer true); // Proccess the given stereo frame. Images must be synchronized and rectified. // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). cv::Mat TrackStereo(const cv::Mat imLeft, const cv::Mat imRight, const double timestamp); // Process the given rgbd frame. Depthmap must be registered to the RGB frame. // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Input depthmap: Float (CV_32F). // Returns the camera pose (empty if tracking fails). cv::Mat TrackRGBD(const cv::Mat im, const cv::Mat depthmap, const double timestamp); // Proccess the given monocular frame // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale. // Returns the camera pose (empty if tracking fails). cv::Mat TrackMonocular(const cv::Mat im, const double timestamp); // This stops local mapping thread (map building) and performs only camera tracking. void ActivateLocalizationMode(); // This resumes local mapping thread and performs SLAM again. void DeactivateLocalizationMode(); // Returns true if there have been a big map change (loop closure, global BA) // since last call to this function bool MapChanged(); // Reset the system (clear map) void Reset(); // All threads will be requested to finish. // It waits until all threads have finished. // This function must be called before saving the trajectory. void Shutdown(); // Save camera trajectory in the TUM RGB-D dataset format. // Only for stereo and RGB-D. This method does not work for monocular. // Call first Shutdown() // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset void SaveTrajectoryTUM(const string filename); // Save keyframe poses in the TUM RGB-D dataset format. // This method works for all sensor input. // Call first Shutdown() // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset void SaveKeyFrameTrajectoryTUM(const string filename); // Save camera trajectory in the KITTI dataset format. // Only for stereo and RGB-D. This method does not work for monocular. // Call first Shutdown() // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php void SaveTrajectoryKITTI(const string filename); // TODO: Save/Load functions // SaveMap(const string filename); // LoadMap(const string filename); // Information from most recent processed frame // You can call this right after TrackMonocular (or stereo or RGBD) int GetTrackingState(); std::vectorMapPoint* GetTrackedMapPoints(); std::vectorcv::KeyPoint GetTrackedKeyPointsUn(); private: // Input sensor eSensor mSensor; // ORB vocabulary used for place recognition and feature matching. ORBVocabulary* mpVocabulary; // KeyFrame database for place recognition (relocalization and loop detection). KeyFrameDatabase* mpKeyFrameDatabase; // Map structure that stores the pointers to all KeyFrames and MapPoints. Map* mpMap; // Tracker. It receives a frame and computes the associated camera pose. // It also decides when to insert a new keyframe, create some new MapPoints and // performs relocalization if tracking fails. Tracking* mpTracker; // Local Mapper. It manages the local map and performs local bundle adjustment. LocalMapping* mpLocalMapper; // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs // a pose graph optimization and full bundle adjustment (in a new thread) afterwards. LoopClosing* mpLoopCloser; // The viewer draws the map and the current camera pose. It uses Pangolin. Viewer* mpViewer; FrameDrawer* mpFrameDrawer; MapDrawer* mpMapDrawer; // System threads: Local Mapping, Loop Closing, Viewer. // The Tracking thread lives in the main execution thread that creates the System object. std::thread* mptLocalMapping; std::thread* mptLoopClosing; std::thread* mptViewer; // Reset flag std::mutex mMutexReset; bool mbReset; // Change mode flags std::mutex mMutexMode; bool mbActivateLocalizationMode; bool mbDeactivateLocalizationMode; // Tracking state int mTrackingState; std::vectorMapPoint* mTrackedMapPoints; std::vectorcv::KeyPoint mTrackedKeyPointsUn; std::mutex mMutexState; }; }// namespace ORB_SLAM #endif // SYSTEM_H#include System.h #include Converter.h #include thread #include pangolin/pangolin.h #include iomanip //使用命名空间ORM_SLAM2 namespace ORB_SLAM2 { //System的构造函数对一些参数进行设定 System::System(const string strVocFile, const string strSettingsFile, const eSensor sensor, const bool bUseViewer):mSensor(sensor), mpViewer(static_castViewer*(NULL)), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) { // Output welcome message 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(mSensorMONOCULAR) cout Monocular endl; else if(mSensorSTEREO) cout Stereo endl; else if(mSensorRGBD) cout RGB-D endl; //Check settings file //读取对应的参数设定的文件如果运行过ORB-SLAM2就会知道对应的就是相机内参、帧率、基线双目 //深度阈值对应ORB Extractor的参数设定还有Viewer线程的参数设定XXX.yaml这种文件类型 cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); if(!fsSettings.isOpened()) { cerr Failed to open settings file at: strSettingsFile endl; exit(-1); } //Load ORB Vocabulary //下载对应的词袋模型对应的是.txt文件类型 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); //Create the Map mpMap new Map(); //Create Drawers. These are used by the Viewer mpFrameDrawer new FrameDrawer(mpMap); mpMapDrawer new MapDrawer(mpMap, strSettingsFile); //Initialize the Tracking thread //(it will live in the main thread of execution, the one that called this constructor) //初始化Tracking线程 mpTracker new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor); //Initialize the Local Mapping thread and launch //初始化Local Mapping 线程 mpLocalMapper new LocalMapping(mpMap, mSensorMONOCULAR); mptLocalMapping new thread(ORB_SLAM2::LocalMapping::Run,mpLocalMapper); //Initialize the Loop Closing thread and launch //初始化闭环检测线程 mpLoopCloser new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!MONOCULAR); mptLoopClosing new thread(ORB_SLAM2::LoopClosing::Run, mpLoopCloser); //Initialize the Viewer thread and launch //初始化可视化线程 if(bUseViewer) { mpViewer new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile); mptViewer new thread(Viewer::Run, mpViewer); mpTracker-SetViewer(mpViewer); } //Set pointers between threads mpTracker-SetLocalMapper(mpLocalMapper); mpTracker-SetLoopClosing(mpLoopCloser); mpLocalMapper-SetTracker(mpTracker); mpLocalMapper-SetLoopCloser(mpLoopCloser); mpLoopCloser-SetTracker(mpTracker); mpLoopCloser-SetLocalMapper(mpLocalMapper); } //运行的系统设定为双目 cv::Mat System::TrackStereo(const cv::Mat imLeft, const cv::Mat imRight, const double timestamp) { if(mSensor!STEREO) { cerr ERROR: you called TrackStereo but input sensor was not set to STEREO. endl; exit(-1); } // Check mode change { unique_lockmutex lock(mMutexMode); if(mbActivateLocalizationMode) { mpLocalMapper-RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper-isStopped()) { usleep(1000); } mpTracker-InformOnlyTracking(true); mbActivateLocalizationMode false; } if(mbDeactivateLocalizationMode) { mpTracker-InformOnlyTracking(false); mpLocalMapper-Release(); mbDeactivateLocalizationMode false; } } // Check reset { unique_lockmutex lock(mMutexReset); if(mbReset) { mpTracker-Reset(); mbReset false; } } cv::Mat Tcw mpTracker-GrabImageStereo(imLeft,imRight,timestamp); unique_lockmutex lock2(mMutexState); mTrackingState mpTracker-mState; mTrackedMapPoints mpTracker-mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn mpTracker-mCurrentFrame.mvKeysUn; return Tcw; } //运行系统设定为深度相机 cv::Mat System::TrackRGBD(const cv::Mat im, const cv::Mat depthmap, const double timestamp) { if(mSensor!RGBD) { cerr ERROR: you called TrackRGBD but input sensor was not set to RGBD. endl; exit(-1); } // Check mode change { unique_lockmutex lock(mMutexMode); if(mbActivateLocalizationMode) { mpLocalMapper-RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper-isStopped()) { usleep(1000); } mpTracker-InformOnlyTracking(true); mbActivateLocalizationMode false; } if(mbDeactivateLocalizationMode) { mpTracker-InformOnlyTracking(false); mpLocalMapper-Release(); mbDeactivateLocalizationMode false; } } // Check reset { unique_lockmutex lock(mMutexReset); if(mbReset) { mpTracker-Reset(); mbReset false; } } cv::Mat Tcw mpTracker-GrabImageRGBD(im,depthmap,timestamp); unique_lockmutex lock2(mMutexState); mTrackingState mpTracker-mState; mTrackedMapPoints mpTracker-mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn mpTracker-mCurrentFrame.mvKeysUn; return Tcw; } //运行系统设定为单目 cv::Mat System::TrackMonocular(const cv::Mat im, const double timestamp) { if(mSensor!MONOCULAR) { cerr ERROR: you called TrackMonocular but input sensor was not set to Monocular. endl; exit(-1); } // Check mode change { unique_lockmutex lock(mMutexMode); if(mbActivateLocalizationMode) { mpLocalMapper-RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper-isStopped()) { usleep(1000); } mpTracker-InformOnlyTracking(true); mbActivateLocalizationMode false; } if(mbDeactivateLocalizationMode) { mpTracker-InformOnlyTracking(false); mpLocalMapper-Release(); mbDeactivateLocalizationMode false; } } // Check reset { unique_lockmutex lock(mMutexReset); if(mbReset) { mpTracker-Reset(); mbReset false; } } cv::Mat Tcw mpTracker-GrabImageMonocular(im,timestamp); unique_lockmutex lock2(mMutexState); mTrackingState mpTracker-mState; mTrackedMapPoints mpTracker-mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn mpTracker-mCurrentFrame.mvKeysUn; return Tcw; } //激活定位模块 void System::ActivateLocalizationMode() { unique_lockmutex lock(mMutexMode); mbActivateLocalizationMode true; } //失活定位模块 void System::DeactivateLocalizationMode() { unique_lockmutex lock(mMutexMode); mbDeactivateLocalizationMode true; } //地图是否进行修改 bool System::MapChanged() { static int n0; int curn mpMap-GetLastBigChangeIdx(); if(ncurn) { ncurn; return true; } else return false; } //重置系统 void System::Reset() { unique_lockmutex lock(mMutexReset); mbReset true; } //关闭整个系统 void System::Shutdown() { mpLocalMapper-RequestFinish(); mpLoopCloser-RequestFinish(); if(mpViewer) { mpViewer-RequestFinish(); while(!mpViewer-isFinished()) usleep(5000); } // Wait until all thread have effectively stopped while(!mpLocalMapper-isFinished() || !mpLoopCloser-isFinished() || mpLoopCloser-isRunningGBA()) { usleep(5000); } if(mpViewer) pangolin::BindToContext(ORB-SLAM2: Map Viewer); } //如果运行的是TUM数据集保存其轨迹 void System::SaveTrajectoryTUM(const string filename) { cout endl Saving camera trajectory to filename ... endl; if(mSensorMONOCULAR) { cerr ERROR: SaveTrajectoryTUM cannot be used for monocular. endl; return; } vectorKeyFrame* vpKFs mpMap-GetAllKeyFrames(); sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); // 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. cv::Mat 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. // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag // which is true when tracking failed (lbL). listORB_SLAM2::KeyFrame*::iterator lRit mpTracker-mlpReferences.begin(); listdouble::iterator lT mpTracker-mlFrameTimes.begin(); listbool::iterator lbL mpTracker-mlbLost.begin(); for(listcv::Mat::iterator litmpTracker-mlRelativeFramePoses.begin(), lendmpTracker-mlRelativeFramePoses.end();lit!lend;lit, lRit, lT, lbL) { if(*lbL) continue; KeyFrame* pKF *lRit; cv::Mat Trw cv::Mat::eye(4,4,CV_32F); // 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; cv::Mat Tcw (*lit)*Trw; cv::Mat Rwc Tcw.rowRange(0,3).colRange(0,3).t(); cv::Mat twc -Rwc*Tcw.rowRange(0,3).col(3); vectorfloat q Converter::toQuaternion(Rwc); f setprecision(6) *lT setprecision(9) twc.atfloat(0) twc.atfloat(1) twc.atfloat(2) q[0] q[1] q[2] q[3] endl; } f.close(); cout endl trajectory saved! endl; } //如果运行的是TUM数据集保存其关键帧轨迹 void System::SaveKeyFrameTrajectoryTUM(const string filename) { cout endl Saving keyframe trajectory to filename ... endl; vectorKeyFrame* vpKFs mpMap-GetAllKeyFrames(); sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); // 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. //cv::Mat Two vpKFs[0]-GetPoseInverse(); ofstream f; f.open(filename.c_str()); f fixed; for(size_t i0; ivpKFs.size(); i) { KeyFrame* pKF vpKFs[i]; // pKF-SetPose(pKF-GetPose()*Two); if(pKF-isBad()) continue; cv::Mat R pKF-GetRotation().t(); vectorfloat q Converter::toQuaternion(R); cv::Mat t pKF-GetCameraCenter(); f setprecision(6) pKF-mTimeStamp setprecision(7) t.atfloat(0) t.atfloat(1) t.atfloat(2) q[0] q[1] q[2] q[3] endl; } f.close(); cout endl trajectory saved! endl; } //如果运行的是KITTI数据集保存其轨迹 void System::SaveTrajectoryKITTI(const string filename) { cout endl Saving camera trajectory to filename ... endl; if(mSensorMONOCULAR) { cerr ERROR: SaveTrajectoryKITTI cannot be used for monocular. endl; return; } vectorKeyFrame* vpKFs mpMap-GetAllKeyFrames(); sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); // 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. cv::Mat 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. // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag // which is true when tracking failed (lbL). listORB_SLAM2::KeyFrame*::iterator lRit mpTracker-mlpReferences.begin(); listdouble::iterator lT mpTracker-mlFrameTimes.begin(); for(listcv::Mat::iterator litmpTracker-mlRelativeFramePoses.begin(), lendmpTracker-mlRelativeFramePoses.end();lit!lend;lit, lRit, lT) { ORB_SLAM2::KeyFrame* pKF *lRit; cv::Mat Trw cv::Mat::eye(4,4,CV_32F); while(pKF-isBad()) { // cout bad parent endl; Trw Trw*pKF-mTcp; pKF pKF-GetParent(); } Trw Trw*pKF-GetPose()*Two; cv::Mat Tcw (*lit)*Trw; cv::Mat Rwc Tcw.rowRange(0,3).colRange(0,3).t(); cv::Mat twc -Rwc*Tcw.rowRange(0,3).col(3); f setprecision(9) Rwc.atfloat(0,0) Rwc.atfloat(0,1) Rwc.atfloat(0,2) twc.atfloat(0) Rwc.atfloat(1,0) Rwc.atfloat(1,1) Rwc.atfloat(1,2) twc.atfloat(1) Rwc.atfloat(2,0) Rwc.atfloat(2,1) Rwc.atfloat(2,2) twc.atfloat(2) endl; } f.close(); cout endl trajectory saved! endl; } int System::GetTrackingState() { unique_lockmutex lock(mMutexState); return mTrackingState; } vectorMapPoint* System::GetTrackedMapPoints() { unique_lockmutex lock(mMutexState); return mTrackedMapPoints; } vectorcv::KeyPoint System::GetTrackedKeyPointsUn() { unique_lockmutex lock(mMutexState); return mTrackedKeyPointsUn; } } //namespace ORB_SLAM从上面的代码可以看出其实针对不同的模型使用的接口还是有差异的。从代码中可以看出1主线程Tracking线程就是在主线程上这一部分主要工作是从图像中提取ORB特征根据上一帧进行姿态估计或者进行通过全局重定位初始化位姿然后跟踪已经重建的局部地图优化位姿再根据一些规则确定新的关键帧。2Local mappng线程这一部分主要完成局部地图构建。包括对关键帧的插入验证最近生成的地图点并进行筛选然后生成新的地图点使用局部捆集调整Local BA最后再对插入的关键帧进行筛选去除多余的关键帧。3Loop closing线程这一部分主要分为两个过程分别是闭环探测和闭环校正。闭环检测先使用WOB进行探测然后通过Sim3算法计算相似变换。闭环校正主要是闭环融合和Essential Graph的图优化。4Viewer线程对估计的位姿和特征点进行可视化显示参考链接http://blog.csdn.net/u010128736/article/details/53157605

【SLAM】ORB-SLAM2的源码阅读（一）：系统的整体构建

相关推荐

【Tensorflow实战系列】Tensorflow: CUDA_ERROR_OUT_OF_MEMORY

【点云】python: 关于点云的一些基本处理

【点云】Pointet的读后感(CVPR2016):1.相关工作的介绍

更多资讯

Gitea 版本控制服务端【Red Hat 离线安装】

make_tuple 一文讲透：CANN 昇腾算子开发里最省心的元组创建方式

lxmusic--听歌自由,感谢开源

读懂PIIP-LLaVA的trainer_state与training_args：5197步微调训练全复盘

如何可视化TIPSv1-So400m/14的1024个空间Patch特征：PCA可视化实战

无名杀完全上手指南：在浏览器里跑一套可自由改造的三国杀引擎

今日更新

电商智能运营体系从0到1：架构设计、落地步骤与避坑指南

压缩空气储能与热网热惯性联合优化调度：热电联产系统建模与求解

若依框架AI代码生成不稳定？用Agent Skill三层结构+五步法搞定

本周精选

金融AI落地指南：从调研报告到技术选型与工程实践

大模型上下文成本与注意力优化技术深度解析——为何O(n²)平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测

基于GMM的语音识别系统原理与MATLAB实现详解

本月热门

我发现了一个新思路：用 Remotion + Claude Code 像写代码一样自动化生成短视频

Windows下 Codex 中 Chrome 和 Computer Use 插件不可用问题排查及解决参考方式：TaoToken 统一 Key 配置与验证

2026 大模型集体涨价：用 Python 做企业 Token 成本测算与选型避坑（附配置）

读完文章，想聊聊您的网站？