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(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(mSensor==MONOCULAR)
        cout << "Monocular" << endl;
    else if(mSensor==STEREO)
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl;

    //Check settings file
    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
    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)
    //主线程，它负责主体的初始化、定位、重定位的功能
    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 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);
}

 根据论文的流程图，我们可以清楚看到，在进行建图的过程中这三个线程是互相交替运行的。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(0) << endl;
    cout << "- k2: " << DistCoef.at(1) << endl;
    if(DistCoef.rows==5)
        cout << "- k3: " << DistCoef.at(4) << endl;//径向畸变参数
    cout << "- p1: " << DistCoef.at(2) << endl;
    cout << "- p2: " << DistCoef.at(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 lock(mpMap->mMutexMapUpdate);

    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();//没有初始化的时候，将进行初始化，这是一种在线初始化的方式

        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 vpMPsMM;
                    vector 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; iIncreaseFound();
                                }
                            }
                        }
                    }
                    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;
        else
            mState=LOST;

        // Update drawer
        mpFrameDrawer->Update(this);//

        // If tracking were good, check if we insert a keyframe
        if(bOK)
        {
            // Update motion model
            if(!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;//获取的是上一帧的变换矩阵的逆与本帧的变换矩阵，相乘后是两帧之间的变换矩阵，即是所谓的速度
            }
            else
                mVelocity = cv::Mat();

            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

            // Clean VO matches
            for(int i=0; iObservations()<1)//去除那些共视数量少的匹配点
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
                    }
            }

            // Delete temporal MapPoints
            for(list::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 keyframe
            if(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(NULL);
            }
        }

        // Reset if the camera get lost soon after initialization
        if(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 lost
        mlRelativeFramePoses.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 busy
        SetAcceptKeyFrames(false);//在该循环中不再接受新关键帧

        // Check if there are keyframes in the queue
        if(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 duplications
                SearchInNeighbors();
            }

            mbAbortBA = false;
            //将队列中的全取出为止，再进行局部的光束平差优化位姿
            if(!CheckNewKeyFrames() && !stopRequested())
            {
                // Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);//它在这里将mbAbortBA作为指针送入，为的是在外部有若干API可以直接停止优化

                // Check redundant local Keyframes
                KeyFrameCulling();//根据共视情况，去除过密的关键帧
            }
            //为后端保存该关键帧
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        else if(Stop())
        {
            // Safe area to stop
            while(isStopped() && !CheckFinish())
            {
                usleep(3000);
            }
            if(CheckFinish())
                break;
        }

        ResetIfRequested();

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(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 consistency
            if(DetectLoop())//检测回环，这里主要是基于相隔的关键帧数量，并且，回环帧的评分不能低于最小的共视的相邻关键帧的回环评分
            {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())//依据评分确定本关键帧位姿
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();//开启新的一轮全局ba优化
               }
            }
        }       

        ResetIfRequested();

        if(CheckFinish())
            break;

        usleep(5000);
    }

    SetFinish();
}

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