ORB_SLAM3相关博客整理

原创已于 2022-02-26 21:34:32 修改 · 766 阅读

2 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #c++

于 2021-09-23 17:13:40 首次发布

ORB_SLAM3 专栏收录该内容

8 篇文章

订阅专栏

本文详细介绍了ORB_SLAM2的实现原理与流程，包括多线程间的协作方式、前端跟踪方法如初始化、位姿恢复及重定位，后端的局部地图优化与回环检测过程。此外还探讨了代码中位姿变量的理解以及SLAM系统的精度评估。

ORB_SLAM博客整理

1、线程调度thread、unique_lock
2、（前端）Track主线程三种跟踪方法
3、（后端）Localclosing回环检测
4、（后端）LocalMapping关键帧的位姿优化，利用其他全局的关键帧的位姿进行优化
5、三线程之间通信
6、代码中位姿变量理解，Tcw、Twc等
7、SLAM精度评定
8、ORB-SLAM改进

System()构造函数中重点函数:

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer, const int initFr, const string &strSequence, const string &strLoadingFile):
    mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false), mbResetActiveMap(false),
    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false){
    mpMap = new Map();//创建全局地图，3个线程共用此地图
    //创建LocalMapping线程，并开始运行
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
	//创建LoopClosing线程，并开始运行
    //Initialize the Loop Closing thread and launch
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
    }

System()构造函数中Tracking()构造函数:

mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, strSequence);

Tracking states中5种状态

// Tracking states
    enum eTrackingState{
        SYSTEM_NOT_READY=-1,
        NO_IMAGES_YET=0,
        NOT_INITIALIZED=1,
        OK=2,
        RECENTLY_LOST=3,
        LOST=4,
        OK_KLT=5
    };

定位建图重启：

mbReset，初始值是false，跟踪或者建图失败时候mbReset设置为True
mbResetActiveMap，初始值是false，与IMU数据失败有关，跟踪或者建图失败时候mbResetActiveMap设置为True

模式变换：

mbActivateLocalizationMode 初始值为false，设为True时，仅仅开启相机追踪模式
mbDeactivateLocalizationMode 初始值为false，设为True时，开启slam建图与定位模式

1、线程调度thread、unique_lock

【ORB-SLAM2代码】（一）线程调度
 ORB-SLAM2多线程用法总结
 c++11多线程的创建和unique_lock的使用

2、（前端）Track主线程三种跟踪方法

2.1 MonocularInitialization()初始化

ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

两线程计算H、F矩阵，进行单目初始化，得到两帧间相对运动、初始MapPoints

mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)

删除无法三角化的点
第一帧和第二帧初始化没有整体平差？？？

2.2 恢复位姿

一、 TrackWithMotionModel() 按照恒速模型跟踪上一帧

根据恒速模型设定当前帧的初始位姿
通过投影的方式在参考帧中找当前帧特征点的匹配点

matcher.SearchByProjection(mCurrentFrame,mLastFrame,...)

优化3D-2D的重投影误差来获得位姿(这是局部地图的3D点还是全局地图的3D点？)，剔除外点

Optimizer::PoseOptimization(&mCurrentFrame)

二、TrackReferenceKeyFrame()跟踪上一关键帧

通过特征点的BoW加快当前帧与参考帧之间的特征点匹配

matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

通过优化3D-2D的重投影误差来获得位姿，剔除外点

Optimizer::PoseOptimization(&mCurrentFrame)

三、Relocalization()重定位

找到与当前帧相似的候选关键帧

DetectRelocalizationCandidates()

Ransac+EPnP算法估计姿态
优化位姿，剔除外点

Optimizer::PoseOptimization()

如果内点较少，则通过SearchByProjection投影的方式对之前未匹配的点进行匹配再进行优化求解

2.3 TrackLocalMap()跟踪局部稀疏点云

如果跟踪成功

TrackLocalMap()//跟踪局部地图

更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
在局部地图中查找与当前帧匹配的MapPoints
更新局部所有MapPoints后对位姿再次优化

  Optimizer::PoseOptimization(&mCurrentFrame);

3、（后端）Localclosing回环检测

通过BOW词袋模型计算当前关键帧和其余关键帧的BOW得分，获得候选关键帧。

4、（后端）LocalMapping关键帧的位姿优化，利用其他全局的关键帧的位姿进行优化

Tracking线程新生成一帧关键帧，关键帧传入Local Mapping线程，处理完成前不接受Tracking线程传入关键帧。Local Mapping线程处理完成后传入Local Closing线程。
LocalMapping线程主要对冗余的关键帧和全局地图点进行剔除，建立全局的共视图和Essential Graph本质图
新关键帧和共视程度较高的关键帧之间三角化更多的点

5、三线程之间通信

ORB-SLAM2代码】（一）线程调度
LoopClosing线程：

mpLocalMapper->RequestStop()。1、mbAbortBA = true;2、mbStopRequested = true;在回环线程里，在运行全局优化时，以及检测后，调整Loop时，localMapping需要暂停。
mpLocalMapper->Release(); 此函数在全局优化以及调整Loop后使用，目的是使mbStopped，mbStopRequested都置为false。且清空 mlNewKeyFrames。
mbFinished表示回环检测和纠正结束。
系统重置时：
if(mbResetRequested) { mlpLoopKeyFrameQueue.clear(); mLastLoopKFid=0; mbResetRequested=false; }

Tracking线程：

mpLocalMapper->SetNotStop(false/true)
mbAbortBA。tracking线程-》CreateNewKeyFrame()函数将mbAbortBA设置为True，LocalMapping线程中止平差优化，插入关键帧优先。

LocalMapping线程：

if(mbStopRequested && !mbNotStop)时候mbStopped = true;
mbStopped等于True的时候，LoaclMapping线程已经停止工作
系统重置时：
if(mbResetRequested) { mlNewKeyFrames.clear(); mlpRecentAddedMapPoints.clear(); mbResetRequested=false; }
mbFinished =True 该线程工作结束