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 该线程工作结束
6、代码中位姿变量理解,Tcw、Twc等
https://blog.csdn.net/cnn_block/article/details/88790909
https://zhuanlan.zhihu.com/p/51848304
7、SLAM精度评定
https://www.zhihu.com/question/485606304
https://blog.csdn.net/weixin_59058976/article/details/121710474?utm_medium=distribute.pc_relevant.none-task-blog-2defaultbaidujs_title~default-0.pc_relevant_paycolumn_v2&spm=1001.2101.3001.4242.1&utm_relevant_index=3