ORB_SLAM2 Monocular 代码流程解析

ORB_SLAM2 Monocular 代码流程解析

有人问我：“ ORB代码看过么？ LSD，DTAM看过么？” 我说没看过别人的代码，只看过视频结果和论文。结果就被别人鄙视了。“这么优秀的SLAM代码都没看过，没法相信你懂SLAM。” 额，这个逻辑不理解，不看别人框架，就不能写SLAM了？好吧，我看还不成，但是看完发现大家的框架和思想基本都那样，没什么差别。为了梳理一下之前自己的“土框架”，也为了和别人“正统的教材SLAM”做个比较，做此文留个笔记。大体上是针对非视觉算法工程师梳理整个代码流程，其中夹杂极其概括的SLAM理论介绍，需要另行查阅的部分，文内会尽量给出通用的KeyWord。

ORB_SLAM2 Monocular KITTI 模式

1. 主框架： 包括 System.cc 和 Tracking.cc ; 以及Example中Mono对应的Main函数。

2. 核心模块： ORBExtractor.cc和ORBMatcher.cc 个人认为是ORB_SLAM的最大贡献，提供了一个带有描述子的快速FAST和Harris特征点选取办法，其余的感觉没什么差别。Frame，Initializer，KeyFrame, Map, MapPoint, PnPSolver等是其各种计算和数据模块。

3. 后端优化： G2o，Bow 组成的 Optimizer, LoopClosing。

4. 剩下一些没提到的不重要的.cc们

上面这些模块会围绕在Tracking过程中通过判断当前状态来回调取和填充数据，所以整个详解流程也会跟着程序代码顺序来回跳跃。由于对象是要给非视觉工程师看，所以相比分模块整理说明，这个方法来得更直接，所有人更通用，而且分模块分析XX_SLAM的文章网上一堆一堆的。至于每个模块的算法和核心思想，像为什么要用Multi-view算姿态，为什么要算F，H矩阵等等这些，请遵循 “内事不决问百度，外事不决问谷歌” 的态度仔细查找，否则内容太多了。而我们的目的只有一个，就是非视觉算法人员也能看起来。

下面从mono_kitti.cc的main()函数开始，再里面一层是System.cc, 再里面一层是Tracing.cc。剩下的就是各种模块来回调取和更新了。所有单个cc代码文件会有编号在结尾，TO-DO并可在开头目录下点击链接快速跳跃。

mono_kitti.cc我是Main() 1号

void LoadImages(const string &strSequence, vector &vstrImageFilenames,vector &vTimestamps); 

 图片数据读取函数没什么好说的，复制存起来今后用，^_^。

下面就是Class System里面的constructor：

 System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,false);

进到Main函数里，第一个重要的调取，（可以看到函数最后一个参数源代码默认是true，是控制显示UI界面的bool变量，我这里直接设false了，因为要把没必要的部分都去掉，后面有些地方的函数和变量跟默认的ORBSLAM2代码不一致也是因为去掉了Pangolin（胖格林？这名字不错）的显示部分。所以各种Viewer的部分文章里就没有了。下面直接跳到 system.h和system.cc。一会儿看完system还会跳回mono_kitti再往下走，后面也会用这种形式。

system.cc 我是2号

mono_kitti中首次调取Class System对整个系统的线程等进行了初始化，也因为是初始化，所以在System.h中可以看到

System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = false); 

包括ORBVOC词包，Config Setting文件地址(里面有摄像头内参Intrinsic等)，传感器类型（我们这里就是Mono），以及是否图形化显示结果。再跳到System.cc 看这个函数的实现，new了一堆自定义的数据变量，和3个线程: thread 1: localMapping, 2: loop closing, 3: viewer。 之后所有的和viewer相关的我们全部省略。至此这个constructor的任务就结束了，细节我们不再讨论，只求看懂整个框架。于是跳回mono_kitti.cc 今后只说代码编号，这次是代码1号。

mono_kitti.cc 我是1号

初始化完毕，就进入了Main Loop。注意一点由于ORB用的是image sequences 所以为了模拟实际情况需要读取序列帧的时间戳，如果是实时camera则可以通过camera帧率和记录的时间计算整个系统所花费的时间，目的是为了避免由于多线程等因素带来的计算对象与图片不匹配的问题。于是下面就是整个代码最外层的核心:

SLAM.TrackMonocular(im,tframe); 我们这里是mono，所以就调用了Class system的TrackMonocular函数。传入了两个参数，一个是当前图片，一个是当前图片的时间戳。此函数前后均有记录系统时间的代码，可以稍微关注一下。跳到代码system.cc 2号。

system.cc 我是2号

函数 cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)

代码片段2-1：

    // Check mode change
    {
        unique_lock 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;
        }
    }

整段都是检查当前追踪模式和线程操作，我们跳过。

注意看后面的：

cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

可以看到 mpTracker是一个在System.h中声明的Class Tracking类型，而Class tracking的声明是在Tracking.h中，一会儿我们跳过去看。先说这里的返回的是一个Mat Tcw，其实就是最终算出来的姿态矩阵。现在我们跳到mpTracker的类型Class tracking 的声明去寻找GrabImage(im, timestamp)函数; 跳到tracking.cc 3号。

tracking.cc 我是3号

直接寻找函数

cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp);

代码片段3-1：

    mImGray = im;

    if(mImGray.channels()==3)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
    }

检查图片格式的代码，跳过

代码片段3-2：

    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)        

mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else

mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

呵呵函数包函数，Class包Class跳的就是这么的销魂。这个if...else...的区别只在于初始化时的参数与通用的略有不同。我们还没有初始化，则我们接下来跑if这段，跳到Frame.cc 4号，查看Class Frame。

Frame.cc 我是4号

对照参数可以知道是monocular的哪一组constructor 就不一一解释了，需要点名的是两个，mpIniORBextractor 和 mpORBVocabulary 第一个是ORB特征点的extractor Class 变量，并且是Initialization版本的，后面的是DBoW2的词包参数。那mpIniORBextractor 就是提取的特征点啦，我们直接看Class Frame constructor的实现部分， 注意是mono的那个。

代码片段4-1

    // Frame ID
    mnId=nNextId++;

    // Scale Level Info
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();


    // ORB extraction
    ExtractORB(0,imGray);

这里之前我们说的那个mpIniORBextractor 对应的就是ORBextractorLeft啦，然后各种对ORBextractor的参数初始化后，直接执行了ExtractORB(0,imGray);函数。0代表左摄像头，ORB这里就是Mono的情况下只调取左摄像头。看ExtractORB(0,imGray)的实现：

代码片段4-2

void Frame::ExtractORB(int flag, const cv::Mat &im)
{
    if(flag==0)
        (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
    else
        (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);是我们mono需要执行的部分， 输入参数：图片， Mask， Key特征点， Descriptor描述子。自此我们需要跳到ORBextractor.cc 5号。

ORBextractor.cc 我是5号

其实不太想说这一块的，因为作者自己写了这个特征点选取办法，所以建议大家看ORB的论文，为了保持我们销魂的跳跃式，我们来这逛一圈嘿嘿

    void operator()( cv::InputArray image, cv::InputArray mask,
      std::vector& keypoints,
      cv::OutputArray descriptors);

就是这个函数了执行完后我们所需要的特征点坐标，特征点的描述子就都存进了keypoints和descriptors里了，于是乎我们认为我们执行完了这一步。现在我们需要跳回4号的ExtractORB(0,imGray);这个函数。

Frame.cc 我是4号

执行完ExtractORB(0,imGray);函数我们再跳回到4号的代码片段4-1后面

代码片段4-3：

    N = mvKeys.size();


    if(mvKeys.empty())
        return;


    UndistortKeyPoints();


    // Set no stereo information
    mvuRight = vector(N,-1);
    mvDepth = vector(N,-1);


    mvpMapPoints = vector(N,static_cast(NULL));
    mvbOutlier = vector(N,false);


    // This is done only for the first Frame (or after a change in the calibration)
    if(mbInitialComputations)
    {
        ComputeImageBounds(imGray);


        mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/static_cast(mnMaxX-mnMinX);
        mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/static_cast(mnMaxY-mnMinY);


        fx = K.at(0,0);
        fy = K.at(1,1);
        cx = K.at(0,2);
        cy = K.at(1,2);
        invfx = 1.0f/fx;
        invfy = 1.0f/fy;


        mbInitialComputations=false;
    }


    mb = mbf/fx;


    AssignFeaturesToGrid();

这里对得到的特征点进行了反畸变等转换，为什么这么做这里不做解释了，我们只分析代码流程，内事不决问百度，外事不决问谷歌！则整个Class Frame的初始化和特征点选取也结束了回到Tracking.cc 3号的代码片段3-2之后。

Tracking.cc 我是3号

在代码片段3-2之后是一个Track();函数。需要提一下的是之前的这一帧的特征点等数据都在mCurrentFrame数据中。我们直接看Tracking.cc中的Track()函数。

代码片段3-3

    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }

则我们此时的State就是NOT_INITIALIZED。因为我们之前只跑了第一帧图像，那还缺一帧才能做Multi-view三角定位，后面肯定还得再跑一帧图像，然后大致用到的是上面流程中的特征点提取办法，再来一个mLastFrame这样的变量储存之前的mCurrentFrame，而新的第二帧图片就是新的mCurrentFrame啦，先提前说明一下，我们接着往下看。

mLastProcessedState=mState;

这个变量就用了这一次，不懂。。。

代码片段3-4

    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();

        //mpFrameDrawer->Update(this);

        if(mState!=OK)
            return;
    }

我们现在的状态就是NOT_INITIALIZED，忘记说明一下了，注意这里的这个初始化指的是SLAM中第一对儿图片的初始化，是SLAM的一个重要流程，而不是指的系统的初始化函数的初始化CLASS的初始化哦。然后我们又是Mono，所以会执行 MonocularInitialization()这个函数; 

代码片段3-5

void Tracking::MonocularInitialization()

{

    if(!mpInitializer)
    {
        // Set Reference Frame
        if(mCurrentFrame.mvKeys.size()>100)
        {
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i                 mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;


            if(mpInitializer)
                delete mpInitializer;


            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);


            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);


            return;
        }
    }

上来就用mpInitializer来判断初始化状态，我们还没初始化，mpInitializer是一个Class Initializer 在这里第一次使用，tracking.h里声明的。由于这个东西后面还要用所以我们后面再说他，我们先当它是false（一般默认上来都是false么）。则当我们之前算的那一帧mCurrentFrame的keypoints大于100的时候，将其设为reference frame。 然后 ^_^

mInitialFrame = Frame(mCurrentFrame); //reference frame
mLastFrame = Frame(mCurrentFrame); //为了通用性，除了Initialization的时候需要用上一帧，之后的Tracking也需要。

嘿嘿， 我说什么来着。

再guajiguaji开新的空间，mpInitializer =  new Initializer(mCurrentFrame,1.0,200);创建了新的Initializer。

接着再开空间，然后返回。回到Tracking.cc 的Track()函数，通过判断mState状态，又会return，其实就直接等下一帧的到来了，中间判断过程省略。。。 等下一帧再回到MonocularInitialization()函数的代码片段3-5, 这时mpInitializer为true了判断不过，直接走到else

代码片段3-6:

    else
    {
        // Try to initialize
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }


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


        // Check if there are enough correspondences
        if(nmatches<100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(NULL);
            return;
        }


        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector vbTriangulated; // Triangulated Correspondences (mvIniMatches)


        if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
        {
            for(size_t i=0, iend=mvIniMatches.size(); i             {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }


            // Set Frame Poses
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);

            CreateInitialMapMonocular();
        }
    }

}

此时的mCurrentFrame就是新的第二帧的啦，之前的那帧存在了mInitialFrame中。这里对新的一帧做了特征点数量的判断，<100个则delete mpInitializer，再来一次循环回到片段3-5将这次的mCurrentFrame填入mInitialFrame。(Tip: 我后来改为了之前的reference frame不变，再选新的一帧与其做Initialization，如果个数超过设置值，则看后面匹配个数和三角化能出来多少个3D点，如果失败则像ORB这样重来，或者次数超过了设置值才重选reference frame) 我们这里假设 >100 个的判断通过了，然后调取函数：

ORBmatcher matcher(0.9,true); //特征点匹配的constructor初始化，两张图需要寻找一致点（correspondences points）才能模拟人的双眼观察世界的效果，才能计算三角化，得到深度和摄像头姿态，大家可以在堵车跟车的时候挡住一只眼试试，会追尾的，所以请在副驾做实验。其余详细部分问百度或者谷歌。ORBmatcher.cc就像ORBextractor.cc一样省略了。单独说一下函数

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

只需注意匹配方式为Initialization模式，ORBmatcher.cc中还有其他模式，分别在不同的tracking()模式和Loop Closing等其他情况下用。这里先不解释，会在后面的Tracking()模式中简单说一下。

接着后面 if(nmatches<100)判断匹配个数，不足则再重复前面过程，提取新的referenceFrame。我们这次认为个数充足够用。则：

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

回到我们之前跳过的mpInitializer，需要跳到新的Initializer.cc 6号。

Initializer.cc 我是6号

之前我们没有详细介绍mpInitializer的那次其实mpInitializer已经 mpInitializer =  new Initializer(mCurrentFrame,1.0,200); 初始化过了，下面我们直接看

Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))；

mCurrentFrame是当前帧， mvIniMatches是之前reference frame（InitialFrame）和当前帧的Initialization匹配结果，Rcw旋转矩阵，tcw位移矩阵。mvIniP3D顾名思义3D点，vbTriangulated一个vector变量记录哪些3D点是Inlier的。 此函数基本用的都是之前的各种建好的数据类型，函数目的就是将第一对儿Frames进行三角定位+运动恢复，包括初始化，不是指的代码系统数据的初始化哦，是单目SLAM的其中一个环节叫初始化，注意区分。这里简单提两句特别的，ORB的初始化选择了同时分解H和F矩阵，至于用哪个矩阵来计算3D点和Rt姿态矩阵是通过

代码片段 6-1

    float RH = SH/(SH+SF);
    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

来计算的，上面的H，F矩阵，Rt姿态，以及Initialzer函数里面算出来的KeyPoints为什么要归一化，为什么要去畸变什么的，请百度和Google看详解。这里的权重大家可以更改一下参数试一下。走完这一遭，Rt矩阵出来了，3D点也出来了，整个初始化流程也就基本结束了，不懂的可以自己慢慢理解这个.cc，基本没有跳跃到其他.cc中，所有函数都在Initializer.cc中实现了。我们这里就不详细解释了，回到代码Tracking.cc 3号的 代码片段3-6

Tracking.cc 我是3号

代码片段3-6 （continue）：

    // Set Frame Poses
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);

            CreateInitialMapMonocular();

上面几行代码没啥好说的，把算出来的姿态矩阵放入到一个更规整的4×4矩阵里，方便矩阵运算调用，并将初始化的姿态存入mCurrentFrame的mTcw中，这个就是摄像头姿态，并在SetPose函数中将旋转啊，位移啊等元素单独放入了事先申请好的变量中，方便后面调用。

最后这个CreateInitialMapMonocular()函数的实现也是在Tracking.cc中也是紧接着上面的3-6部分的，所以很好查阅。内容是一些往Map，KeyFrame等声明好的Class数据结构中填入和规整相应的数据方便后面调用，优化，包括算出来的3D点云，KeyFrame特征点，描述子等，这里我们就不详细查看了，都是基础的数据结构调用和填充。单独提一下里面有2个比较重要的部分：

一个是在插入KeyFrame的时候计算了BoW，其他与Frame相关的代码中也有computerBoW() 这个函数被调用过，

另一个重要的是Class Optimizer。用的是G2o的BA，这个CSDN里有不少介绍，大家可以去查阅，高博士（半闲居士）的就很清楚。

这些东西牵扯到太多关于SLAM的优化原理和优化方法等知识，这里就跳过了，否则篇幅太长了。跳出这个函数前我们看到最后有mState = OK的状态改变，带着这个状态现在我们跳回代码片段3-4。

代码片段3-4 (continue)

函数MonocularInitialization();执行完后面就是

if(mState!=OK)
            return;

如果初始化不成功，则会重复上面的流程直到OK为止，我们现在认为流程都执行过并且符合要求成功了，我们继续往后看。根据程序的执行顺序，不管else，直接跳到后面：

代码片段3-5：

    // 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);
    }

如果Tracking()成功，则mCurrentFrame.mTcw中保存着初始化后算出的摄像头姿态，并更新数据，然后下一帧。否则会保存最后一次成功的数据然后进入下一轮的循环等待新的序列帧传入再继续判断状态。我们现在设定为所有都为成功的状态，进入下一轮循环，回到3-4之前跳过的else部分，标注为： 代码片段3-4-1

紧接之前的代码片段3-4 这里是全部工程另一个非常重要的部分，所以单独标注3-4-1

函数MonocularInitialization();上面已经执行完毕了，函数结尾return后等待下一帧到来。SLAM的初始化已经完毕了。所以直接跳到else部分，现在第三帧新的图像传入，就要开始做真正意义的Tracking了。

代码片段3-4-1:

    else
    {
        // System is initialized. Track Frame.
        bool bOK;

        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                 {
                    bOK = TrackReferenceKeyFrame();
                }
                else
                {
                    bOK = TrackWithMotionModel();
                    if(!bOK)
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else
            {
                bOK = Relocalization();
            }
        }

if(!mbOnlyTracking)的else部分作为3-4-2先不说。

mbOnlyTracking默认False，只要不改，都会执行if部分，为正常的SLAM模式，否则是读取已经存好的空间点云做定位的Localization模式。

先说函数 CheckReplacedInLastFrame(); 

因为是多线程，每次还做了点云的优化，所以在进行新的Tracking之前进行了一次数据的整理和更新，感兴趣的可以去深究一下，没什么难度。

现在进入下一层的if...else...。 mVelocity初始为empty，至此之前还没有使用过，在后面执行完后会对这个变量赋值，所以先执行if语句。这里我们可以看到有三种追踪状态：

bOK = TrackReferenceKeyFrame();     //用最近的一个KeyFrame做为referenceFrame做追踪

bOK = TrackWithMotionModel();      //用之前的结果继续做追踪

bOK = Relocalization();     //重定位模式 一般是追踪丢失了或者是用已经建立好的地图做重定位

这里简单提一下，不想看理论的直接跳过这一段。（SLAM，特别是单目的SLAM会因为每一帧的计算误差形成累积误差，所以KeyFrame based SLAM大家会用取KeyFrame的方法来计算关键帧之间的运动和点云，然后再优化关键帧与关键帧之间的误差，从而降低每帧都计算的误差。算的次数少了，累积误差当然就降低了，速度也提高了，所以就产生了前两个模式。第三个模式很容易理解，在前面都失败了的情况下，Tracking丢失了，只能去通过回到之前看到过的KeyFrame场景下，从这个KeyFrame开始重新定位追踪。详细的大家可以去稍微看一下SLAM原理，很多讲义上的前几页就会提到。）

我们继续代码分析，接之前提到过但没解释的，不同的模式里面也会指向不同的Matching模式，每个模式的初始化参数略有不同（SearchForInitializaiton，前面有讲到）。这次我们结合Tracking()简单说一下其他几种匹配模式：

1. bOK = TrackReferenceKeyFrame();  这个函数里面我们主要讲三个地方

第一个mCurrentFrame.ComputeBoW(); 计算这一帧的词包并保存，其实就是保存的这一帧的描述子并给一个index，为将来matching时的searching做准备。

第二个int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches); 这次变成了SearchByBow的匹配了，填入函数的参数为mpReferenceKF，由于我们假设程序是第一次走到这里，其实这次就是前面初始化时顺带储存的第一个ReferenceKeyFrame，mCurrentFrame是当前帧，vpMapPointMatches是这两帧匹配后的特征点。这个过程就是典型的KeyFrame与后续帧的Tracking()。这个函数里面当然肯定也会有BoW的匹配操作。

第三个Optimizer::PoseOptimization(&mCurrentFrame); 对于新的帧的优化，ORB_SLAM主要就是G2O的优化。

函数后面还有一些数据整理的部分，比如把Outlier的点去掉啊等，请大家自行查看，这里就不说了。

下面我们看第二个模式

2. bOK = TrackWithMotionModel(); 

首先还会看到ORBmatcher() 其中的参数会略有不同，大家可以手动修改一下看看效果。接着后面的代码就是跟之前的类似，更新一下LastFrame里的特征点，优化一下当前帧等等，大体都是类似的，但：

 int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

可以看到这里的matching方式就变为了SearchByProjection()，主要看一下matching的对象就不是referenceKeyFrame了，而是上一帧，然后又对结果不满足Threshold怎么办啊等等进行了情况划分。总体就是只对相邻两帧图像之间进行运动回复和点云的构建然后排除Outlier点。并且如果返回结果是false，则再进行1模式的TrackReferenceKeyFrame(),希望能够追踪到KeyFrame。

3. bOK = Relocalization();

int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); 这里的matching()方式就是通过词包的匹配去匹配储存好的KeyFrame，需要先对整个空间场景进行扫描，然后再走匹配和Tracking流程。特别的，在这个函数中包含：

PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);

这一步。如果匹配到KeyFrame，则直接计算 PnPProblem （这个大家可以查Google，Perspective  N Points)。

if(!mbOnlyTracking)的else部分作为3-4-2先不说。直接看这组if...else...的下面。我们认为此时

bOK = TrackReferenceKeyFrame()是返回的True状态。记录当前mCurrentFrame的referenceKeyFrame;

接着后面便是TrackLocalMap（）很好理解，是对当下的局部地图进行匹配和整理，关心的可以去看一下。

        if(bOK)
            mState = OK;
        else
            mState = LOST;

将mState状态改为OK。再往后后面的操作就是根据我们前面的Tracking状态等对数据进行归档，更新，删除等，比如填充mVelocity，创建新的KeyFrame。这些都是为下面新的一帧的到来做准备，没有什么特别的地方需要讲了。

代码片段3-4-2

       else
        {
            // Localization Mode: Local Mapping is deactivated


            if(mState==LOST)
            {
                bOK = Relocalization();
            }
            else
            {
                if(!mbVO)
                {
                    // In last frame we tracked enough MapPoints in the map


                    if(!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 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; i                             {
                                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                                {
                                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)
                    {
                        mbVO = false;
                    }


                    bOK = bOKReloc || bOKMM;
                }
            }
        }

这里就是不同模式下的逻辑过程相对有些差异，但函数功能等都是相同的，所以我们上面基本讲完了这些不同函数的功能，这里就不再描述了，大家可以根据代码走到的位置，分别跳跃查看整个ORB_SLAM的流程。

总结一下

回到mono_kitti.cc（差点把它忘了），再往后的部分就是通过计算整个Track()的时间，再与时间戳比较，就可以知道每循环一次需要等待多长时间，直到下一帧读入，

由于我只是要给其他的工程师们一个流程性的讲解，最终的使用平台也不是通用的软件编程语言，所以其他的关于算法和计算的部分这里不做过多解释，如果不是专门需要更改算法细节，不去关心他们也没有什么太大的关系。需要注意的是ORB_SLAM中对于KeyFrame的更新，Points的更新时会来回对之前的结果进行数据操作，和数据优化，所以一旦有些地方顺序和个数写错了不太容易发现，要多加小心。

整个流程的文章整理和编写也拖了很长时间，后面部分可能会比较概(Tou)括(Lan)，可以结合一部分的Computer Vision里的Multi-View的理论再从新去读一遍Matching，LocalMatching，PnPProblem的部分，会有一个更清楚的了解，其次就是BoW，和G2o的地方也需要再单独查看一下。大部分的SLAM也都是类似的框架，读懂这个，其他的也不难。我也还没有完全掌握SLAM中的全部模块。这次先到这里吧。