ORB_SLAM2源码阅读（一）概要

今天斥资十块钱买了一个可爱的单目摄像头，那么做一个伟大的项目还远吗？——沃镃基硕德

在算法领域遨游的这些年，我们的能力、见识都在飞速增长，代码一行行看，博客知乎一篇篇刷，书本一本本买，从最最开始的《C++ primer》和《21天精通Python》（黑历史）到后面的《概率机器人》《机器学习》《视觉slam十四讲》，以及《Unix环境高级编程》《UNIX网络编程》这种越挖越深的大部头，都对我们弱小的心灵造成巨大的冲击与震撼，还有一些只存在于博客、论文、公开课上的只言片语缺少编纂的零碎而前沿的知识。走进算法领域，原本高大上、不可方物的术语，以及炫酷而神秘的视觉效果，都被一层层揭开面纱，如同庖丁解牛一般展现在面前，原来它们都像是华美壮丽的房子由一块块砖头累积而成一样富有层次地描绘着数学原始的魅力。当然，编程不仅是堆砌砖头的工程，更是一种美的追求，一个好的工程就像工艺品一样供他人传阅品读也依旧不失好评，就像我们今天研读的工程一样。

言归正传，既然有一个摄像头，当然要用它做一些力所能及的事情，例如简单的人脸识别、车牌识别、车位识别以及物体跟踪等等，在slam领域，只用一个单目相机就能完整描绘出世界的模样是业界追求的梦想（毕竟便宜），而至今还不能拿出完全使人满意买单的结果，这更突显出它无尽的吸引力。对于我们这样的菜狗中的老手，一味追求新潮或许不太利于夯实基础，从经典创作中，可能更能体会到slam过程的原理与编程之美。

ORB_SLAM系列就是经典创作中的代表，它作为视觉SLAM的demo已经推出好几年，被各路高手所使用并验证它的优越性。作为稀疏建图的代表，它的结构清晰，也几乎覆盖了十四讲中的经典方法。我们将分别从前端位姿估计、建图、后端优化三方面着手进行学习。

一、纵览

基于ORB特征的SLAM系统包含视觉里程计、建图、后端、回环检测部分：

1.视觉里程计包括对极约束的初始化、创建尺度；基于pnp的位姿估计；局部bundle adjustment的位姿优化；全局ba的位姿优化。

2.建图包括基于三角测量的地图点云构建。

3.后端包括利用g2o的关键帧位姿存储，基于词袋模型的回环与位姿图更新。

对于视觉SLAM来说，这三部分以极高的频率交替进行。

二、切入

ORB_SLAM2提供了单目、双目和rgbd的模式接口，我们直接进入单目模式，它提供三种数据集，如果从mono_kitti.cc进入，它的main函数初始化了整个SLAM系统：

// Create SLAM system. It initializes all system threads and gets ready to process frames.ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

如果追求实时的SLAM过程，则从ros_mono.cc进入，它们都由system创建了整个系统。和segmap很像，它新建了三个线程，mptLocalMapping、mptLoopClosing、mptViewer，分别是局部建图、回环检测以及发布可视化的线程。

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)
{// Output welcome messagecout << 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(mSensor==MONOCULAR)cout << "Monocular" << endl;else if(mSensor==STEREO)cout << "Stereo" << endl;else if(mSensor==RGBD)cout << "RGB-D" << endl;//Check settings filecv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);if(!fsSettings.isOpened()){cerr << "Failed to open settings file at: " << strSettingsFile << endl;exit(-1);}//Load ORB Vocabularycout << 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 DatabasempKeyFrameDatabase = 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)//主线程，它负责主体的初始化、定位、重定位的功能mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);//Initialize the Local Mapping thread and launch//局部建图的线程，它负责将传来的关键帧进行地图点绘制mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);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 launchif(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);
}

根据论文的流程图，我们可以清楚看到，在进行建图的过程中这三个线程是互相交替运行的。tracking负责“跟踪”，跟踪即利用已有的“局部”地图点以及不断传来的帧，跟踪到相机位姿，并间断地提供关键帧；local mapping负责"局部“建图，也就是拿到tracking的新关键帧，在相邻的几个关键帧中建立联系，利用三角测量更新地图点；loop closure负责更新位姿图以及全局优化。

三、前期的准备工作

从概要部分我们可以发现，首先初始化的关键部分是Tracking这个类，它包含了众多的前期准备工作:

1.相机自身的参数：

cout << endl << "Camera Parameters: " << endl;cout << "- fx: " << fx << endl;cout << "- fy: " << fy << endl;cout << "- cx: " << cx << endl;cout << "- cy: " << cy << endl;//世界坐标向相机像素投影的参数cout << "- k1: " << DistCoef.at<float>(0) << endl;cout << "- k2: " << DistCoef.at<float>(1) << endl;if(DistCoef.rows==5)cout << "- k3: " << DistCoef.at<float>(4) << endl;//径向畸变参数cout << "- p1: " << DistCoef.at<float>(2) << endl;cout << "- p2: " << DistCoef.at<float>(3) << endl;//枕形畸变参数cout << "- fps: " << fps << endl;

2.ORB特征提取部分的参数：

cout << endl  << "ORB Extractor Parameters: " << endl;cout << "- Number of Features: " << nFeatures << endl;//某个像素向外搜索的四周像素数量cout << "- Scale Levels: " << nLevels << endl;//图像金字塔的层数cout << "- Scale Factor: " << fScaleFactor << endl;cout << "- Initial Fast Threshold: " << fIniThFAST << endl;//cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;

四、tracking线程

在每一帧图像到来后，都会优先调用Tracking::Track函数，这是主线程的主题表现，它进行相机位姿的确定并且产生关键帧。

void Tracking::Track()
{if(mState==NO_IMAGES_YET){mState = NOT_INITIALIZED;}mLastProcessedState=mState;// Get Map Mutex -> Map cannot be changed//当获得该锁时，将停止全局ba与位姿图的更新unique_lock<mutex> lock(mpMap->mMutexMapUpdate);if(mState==NOT_INITIALIZED){if(mSensor==System::STEREO || mSensor==System::RGBD)StereoInitialization();elseMonocularInitialization();//没有初始化的时候，将进行初始化，这是一种在线初始化的方式mpFrameDrawer->Update(this);if(mState!=OK)return;}else{// System is initialized. Track Frame.bool bOK;// Initial camera pose estimation using motion model or relocalization (if tracking is lost)//开始定位相机位姿if(!mbOnlyTracking){//这种情况是局部建图也在同时进行// Local Mapping is activated. This is the normal behaviour, unless// you explicitly activate the "only tracking" mode.if(mState==OK){// Local Mapping might have changed some MapPoints tracked in last frame   //在局部建图中,最后一帧的关键点会有所变化，因此更新之CheckReplacedInLastFrame();if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2){bOK = TrackReferenceKeyFrame();//跟上一帧关键帧进行匹配与pnp位姿求解}else{bOK = TrackWithMotionModel();//用之前的相机速度模拟本帧的位姿，再进行pnp求解if(!bOK)bOK = TrackReferenceKeyFrame();}}else{bOK = Relocalization();//出现跟丢的现象，则进行重定位，与所有关键帧匹配}}else{// Localization Mode: Local Mapping is deactivatedif(mState==LOST){bOK = Relocalization();}else{if(!mbVO){// In last frame we tracked enough MapPoints in the mapif(!mVelocity.empty()){bOK = TrackWithMotionModel();}else{bOK = TrackReferenceKeyFrame();}}else{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.bool bOKMM = false;bool bOKReloc = false;vector<MapPoint*> vpMPsMM;vector<bool> vbOutMM;cv::Mat TcwMM;if(!mVelocity.empty()){bOKMM = TrackWithMotionModel();vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}bOKReloc = Relocalization();if(bOKMM && !bOKReloc){mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;if(mbVO){for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}else if(bOKReloc){mbVO = false;}bOK = bOKReloc || bOKMM;}}}mCurrentFrame.mpReferenceKF = mpReferenceKF;// If we have an initial estimation of the camera pose and matching. Track the local map.if(!mbOnlyTracking){if(bOK)bOK = TrackLocalMap();}else{// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes// the camera we will use the local map again.if(bOK && !mbVO)bOK = TrackLocalMap();//pnp方法求解相机位姿}if(bOK)mState = OK;elsemState=LOST;// Update drawermpFrameDrawer->Update(this);//// If tracking were good, check if we insert a keyframeif(bOK){// Update motion modelif(!mLastFrame.mTcw.empty()){cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));mVelocity = mCurrentFrame.mTcw*LastTwc;//获取的是上一帧的变换矩阵的逆与本帧的变换矩阵，相乘后是两帧之间的变换矩阵，即是所谓的速度}elsemVelocity = cv::Mat();mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// Clean VO matchesfor(int i=0; i<mCurrentFrame.N; i++){MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(pMP)if(pMP->Observations()<1)//去除那些共视数量少的匹配点{mCurrentFrame.mvbOutlier[i] = false;mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Delete temporal MapPointsfor(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;delete pMP;}mlpTemporalPoints.clear();// Check if we need to insert a new keyframeif(NeedNewKeyFrame())CreateNewKeyFrame();//通知局部建图线程插入关键帧// We allow points with high innovation (considererd outliers by the Huber Function)// pass to the new keyframe, so that bundle adjustment will finally decide// if they are outliers or not. We don't want next frame to estimate its position// with those points so we discard them in the frame.for(int i=0; i<mCurrentFrame.N;i++){if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Reset if the camera get lost soon after initializationif(mState==LOST){if(mpMap->KeyFramesInMap()<=5){cout << "Track lost soon after initialisation, reseting..." << endl;mpSystem->Reset();return;}}if(!mCurrentFrame.mpReferenceKF)mCurrentFrame.mpReferenceKF = mpReferenceKF;mLastFrame = Frame(mCurrentFrame);}// Store frame pose information to retrieve the complete camera trajectory afterwards.if(!mCurrentFrame.mTcw.empty()){cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();mlRelativeFramePoses.push_back(Tcr);mlpReferences.push_back(mpReferenceKF);mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);mlbLost.push_back(mState==LOST);}else{// This can happen if tracking is lostmlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState==LOST);}}

五、局部建图线程

在创造关键帧的同时，局部建图线程也在等待关键帧，一旦获得新的关键帧便开始估计这一段的新增地图点。

void LocalMapping::Run()
{mbFinished = false;while(1){// Tracking will see that Local Mapping is busySetAcceptKeyFrames(false);//在该循环中不再接受新关键帧// Check if there are keyframes in the queueif(CheckNewKeyFrames()){// BoW conversion and insertion in Map//每个关键帧都需要持有自身的词袋向量，为了在回环以及重定位时进行比对ProcessNewKeyFrame();// Check recent MapPoints//剔除少于某特定数量的关键帧所能观测到的地图点MapPointCulling();// Triangulate new MapPoints//根据三角测量更新地图点CreateNewMapPoints();//一再判断新关键帧队列是否为空，在ProcessNewKeyFrame时取出了最老的一个，如果队列为空，则找寻附近关键帧中可以观测到本关键帧的特征点的帧，加进来一并进行光束平差优化关键帧位姿与地图点if(!CheckNewKeyFrames()){// Find more matches in neighbor keyframes and fuse point duplicationsSearchInNeighbors();}mbAbortBA = false;//将队列中的全取出为止，再进行局部的光束平差优化位姿if(!CheckNewKeyFrames() && !stopRequested()){// Local BAif(mpMap->KeyFramesInMap()>2)Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);//它在这里将mbAbortBA作为指针送入，为的是在外部有若干API可以直接停止优化// Check redundant local KeyframesKeyFrameCulling();//根据共视情况，去除过密的关键帧}//为后端保存该关键帧mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);}else if(Stop()){// Safe area to stopwhile(isStopped() && !CheckFinish()){usleep(3000);}if(CheckFinish())break;}ResetIfRequested();// Tracking will see that Local Mapping is busySetAcceptKeyFrames(true);//放开if(CheckFinish())break;usleep(3000);}SetFinish();
}

六、回环检测线程

依据关键帧的流通方向，它在一个周期中是跟在局部建图后。

void LoopClosing::Run()
{mbFinished =false;while(1){// Check if there are keyframes in the queue//依然是从队列中取帧，不过这里的关键帧是在局部建图中调整后的，并不是tracking直接传来的if(CheckNewKeyFrames()){// Detect loop candidates and check covisibility consistencyif(DetectLoop())//检测回环，这里主要是基于相隔的关键帧数量，并且，回环帧的评分不能低于最小的共视的相邻关键帧的回环评分{// Compute similarity transformation [sR|t]// In the stereo/RGBD case s=1if(ComputeSim3())//依据评分确定本关键帧位姿{// Perform loop fusion and pose graph optimizationCorrectLoop();//开启新的一轮全局ba优化}}}       ResetIfRequested();if(CheckFinish())break;usleep(5000);}SetFinish();
}

避免一次写太长，下一篇从跟踪定位，也就是视觉里程计开始详细学习。

ORB_SLAM2源码阅读（一）概要相关推荐

ORB_SLAM2源码阅读（三）相机定位
我们继续上一节的tracking部分,tracking部分结合了多视图几何的许多计算算法,这就不得不翻开<多视图几何>这本大部头才能看懂作者的源码.上一节的初始化过程后,Tracking需 ...
ORB-SLAM2源码阅读(1)
ORB-SLAM2源码阅读(1) ORB-SLAM2 的代码以结构清晰,注释完整,易于理解著称,最好先阅读一下论文再来读代码,网上有很多大神已经翻译好了,个人推荐这位的ORB-SLAM2全文翻译系统 ...
应用监控CAT之cat-client源码阅读（一）
CAT 由大众点评开发的,基于 Java 的实时应用监控平台,包括实时应用监控,业务监控.对于及时发现线上问题非常有用.(不知道大家有没有在用) 应用自然是最初级的,用完之后,还想了解下其背后的原理, ...
centos下将vim配置为强大的源码阅读器
每日杂事缠身,让自己在不断得烦扰之后终于有了自己的清静时光来熟悉一下我的工具,每次熟悉源码都需要先在windows端改好,拖到linux端,再编译.出现问题,还得重新回到windows端,这个过程太耗 ...
源码阅读：AFNetworking（十六）——UIWebView+AFNetworking
该文章阅读的AFNetworking的版本为3.2.0. 这个分类提供了对请求周期进行控制的方法,包括进度监控.成功和失败的回调. 1.接口文件 1.1.属性 /**网络会话管理者对象*/ @prop ...
源码阅读：SDWebImage（六）——SDWebImageCoderHelper
该文章阅读的SDWebImage的版本为4.3.3. 这个类提供了四个方法,这四个方法可分为两类,一类是动图处理,一类是图像方向处理. 1.私有函数先来看一下这个类里的两个函数 /**这个函数是计算 ...
mybatis源码阅读
说下mybatis执行一个sql语句的流程执行语句,事务等SqlSession都交给了excutor,excutor又委托给statementHandler SimpleExecutor:每执行一次 ...
24 UsageEnvironment使用环境抽象基类——Live555源码阅读(三)UsageEnvironment
24 UsageEnvironment使用环境抽象基类--Live555源码阅读(三)UsageEnvironment 24 UsageEnvironment使用环境抽象基类--Live555源码阅读 ...
Transformers包tokenizer.encode()方法源码阅读笔记
Transformers包tokenizer.encode()方法源码阅读笔记_天才小呵呵的博客-CSDN博客_tokenizer.encode
源码阅读笔记 BiLSTM+CRF做NER任务流程图
源码阅读笔记 BiLSTM+CRF做NER任务(二) 源码地址:https://github.com/ZhixiuYe/NER-pytorch 本篇正式进入源码的阅读,按照流程顺序,一一解剖. 一.流 ...

ORB_SLAM2源码阅读（一）概要

ORB_SLAM2源码阅读（一）概要相关推荐

最新文章

热门文章