ORBSLAM2总结之-Tracking.cpp

/**
 * @file Tracking.cc
 * @author guoqing (1337841346@qq.com)
 * @brief 追踪线程
 * @version 0.1
 * @date 2019-02-21
 * 
 * @copyright Copyright (c) 2019
 * 
 */

/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/


#include "Tracking.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "ORBmatcher.h"
#include "FrameDrawer.h"
#include "Converter.h"
#include "Map.h"
#include "Initializer.h"

#include "Optimizer.h"
#include "PnPsolver.h"

#include <iostream>
#include <cmath>
#include <mutex>


using namespace std;

// 程序中变量名的第一个字母如果为"m"则表示为类中的成员变量，member
// 第一个、第二个字母:
// "p"表示指针数据类型
// "n"表示int类型
// "b"表示bool类型
// "s"表示set类型
// "v"表示vector数据类型
// 'l'表示list数据类型
// "KF"表示KeyFrame数据类型   

namespace ORB_SLAM2
{
   

///构造函数
/**
 * Step 1 从配置文件中加载相机参数-----相机内参、畸变参数、mbf--双目摄像头相机的基线长度 * 相机的焦距、  、Max/Min Frames to insert keyframes and to check relocalisation、以及相机颜色通道顺序是RGB还是BGR
 * Step 2 加载ORB特征点有关的参数,并新建特征点提取器 比如每一帧提取的特征点数 1000、图像建立金字塔时的变化尺度 1.2、尺度金字塔的层数 8、 提取fast特征点的默认阈值 20 fast20、如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
 *        tracking过程都会用到mpORBextractorLeft作为特征点提取器、如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
 *        在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器  mpIniORBextractor要提取2000个特征点(单目初始化的时候，第一个图片要提取2000个特征点)
 *        如果是双目或者RGBD  mThDepth判断一个3D点远/近的阈值 mbf * 35 / fx、ThDepth其实就是表示基线长度的多少倍
 *        如果是RGBD mDepthMapFactor--深度相机disparity转化为depth时的因子
 **/
Tracking::Tracking(
    System *pSys,                       //系统实例
    ORBVocabulary* pVoc,                //BOW字典
    FrameDrawer *pFrameDrawer,          //帧绘制器
    MapDrawer *pMapDrawer,              //地图点绘制器
    Map *pMap,                          //地图句柄
    KeyFrameDatabase* pKFDB,            //关键帧产生的词袋数据库
    const string &strSettingPath,       //配置文件路径
    const int sensor):                  //传感器类型
        mState(NO_IMAGES_YET),                              //当前系统还没有准备好
        mSensor(sensor),                                
        mbOnlyTracking(false),                              //处于SLAM模式
        mbVO(false),                                        //当处于纯跟踪模式的时候，这个变量表示了当前跟踪状态的好坏
        mpORBVocabulary(pVoc),          
        mpKeyFrameDB(pKFDB), 
        mpInitializer(static_cast<Initializer*>(NULL)),     //暂时给地图初始化器设置为空指针
        mpSystem(pSys), 
        mpViewer(NULL),                                     //注意可视化的查看器是可选的，因为ORB-SLAM2最后是被编译成为一个库，所以对方人拿过来用的时候也应该有权力说我不要可视化界面（何况可视化界面也要占用不少的CPU资源）
        mpFrameDrawer(pFrameDrawer),
        mpMapDrawer(pMapDrawer), 
        mpMap(pMap), 
        mnLastRelocFrameId(0)                               //恢复为0,没有进行这个过程的时候的默认值
{
   
    // Load camera parameters from settings file
    // Step 1 从配置文件中加载相机参数
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
    float fx = fSettings["Camera.fx"];
    float fy = fSettings["Camera.fy"];
    float cx = fSettings["Camera.cx"];
    float cy = fSettings["Camera.cy"];

    //     |fx  0   cx|
    // K = |0   fy  cy|
    //     |0   0   1 |
    //构造相机内参矩阵
    cv::Mat K = cv::Mat::eye(3,3,CV_32F);
    K.at<float>(0,0) = fx;
    K.at<float>(1,1) = fy;
    K.at<float>(0,2) = cx;
    K.at<float>(1,2) = cy;
    K.copyTo(mK);

    // 图像矫正系数
    // [k1 k2 p1 p2 k3]
    cv::Mat DistCoef(4,1,CV_32F);
    DistCoef.at<float>(0) = fSettings["Camera.k1"];
    DistCoef.at<float>(1) = fSettings["Camera.k2"];
    DistCoef.at<float>(2) = fSettings["Camera.p1"];
    DistCoef.at<float>(3) = fSettings["Camera.p2"];
    const float k3 = fSettings["Camera.k3"];
    //有些相机的畸变系数中会没有k3项
    if(k3!=0)
    {
   
        DistCoef.resize(5);
        DistCoef.at<float>(4) = k3;
    }
    DistCoef.copyTo(mDistCoef);

    // 双目摄像头baseline * fx 50
    mbf = fSettings["Camera.bf"];//相机的基线长度 * 相机的焦距

    float fps = fSettings["Camera.fps"];
    if(fps==0)
        fps=30;

    // Max/Min Frames to insert keyframes and to check relocalisation
    mMinFrames = 0;
    mMaxFrames = fps;

    //输出
    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;

    // 1:RGB 0:BGR
    int nRGB = fSettings["Camera.RGB"];
    mbRGB = nRGB;

    if(mbRGB)
        cout << "- color order: RGB (ignored if grayscale)" << endl;
    else
        cout << "- color order: BGR (ignored if grayscale)" << endl;

    // Load ORB parameters

    // Step 2 加载ORB特征点有关的参数,并新建特征点提取器

    // 每一帧提取的特征点数 1000
    int nFeatures = fSettings["ORBextractor.nFeatures"];
    // 图像建立金字塔时的变化尺度 1.2
    float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
    // 尺度金字塔的层数 8
    int nLevels = fSettings["ORBextractor.nLevels"];
    // 提取fast特征点的默认阈值 20 fast20
    int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
    // 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
    int fMinThFAST = fSettings["ORBextractor.minThFAST"];

    // tracking过程都会用到mpORBextractorLeft作为特征点提取器
    mpORBextractorLeft = new ORBextractor(
        nFeatures,      //参数的含义还是看上面的注释吧
        fScaleFactor,
        nLevels,
        fIniThFAST,
        fMinThFAST);

    // 如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
    if(sensor==System::STEREO)
        mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

    // 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器  mpIniORBextractor要提取2000个特征点(单目初始化的时候，第一个图片要提取2000个特征点)
    if(sensor==System::MONOCULAR)
        mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

    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;

    //如果传感器是双目或者RGBD
    if(sensor==System::STEREO || sensor==System::RGBD)
    {
   
        // mThDepth判断一个3D点远/近的阈值 mbf * 35 / fx
        //ThDepth其实就是表示基线长度的多少倍
        mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
        cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
    }

    if(sensor==System::RGBD)
    {
   
        //将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
        // mDepthMapFactor--深度相机disparity转化为depth时的因子
        mDepthMapFactor = fSettings["DepthMapFactor"];
        if(fabs(mDepthMapFactor)<1e-5)
            mDepthMapFactor=1;
        else
            mDepthMapFactor = 1.0f/mDepthMapFactor;
    }

}

//创建基本的类
//设置局部建图器
void Tracking::SetLocalMapper(LocalMapping *pLocalMapper)
{
   
    mpLocalMapper=pLocalMapper;
}

//设置回环检测器
void Tracking::SetLoopClosing(LoopClosing *pLoopClosing)
{
   
    mpLoopClosing=pLoopClosing;
}

//设置可视化查看器
void Tracking::SetViewer(Viewer *pViewer)
{
   
    mpViewer=pViewer;
}

/**
 * 这里是双目的获取图片的函数、同时利用获得的灰度图构建当前帧，进行tracking过程，得到世界到相机的位姿变换
 **/
// 输入左右目图像，可以为RGB、BGR、RGBA、GRAY
// 1、将图像转为mImGray和imGrayRight并初始化mCurrentFrame
// 2、进行tracking过程
// 输出世界坐标系到该帧相机坐标系的变换矩阵
cv::Mat Tracking::GrabImageStereo(
    const cv::Mat &imRectLeft,      //左侧图像
    const cv::Mat &imRectRight,     //右侧图像
    const double &timestamp)        //时间戳
{
   
    mImGray = imRectLeft;
    cv::Mat imGrayRight = imRectRight;

    // step 1 ：将RGB或RGBA图像转为灰度图像
    if(mImGray.channels()==3)
    {
   
        if(mbRGB)
        {
   
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
        }
        else
        {
   
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
        }
    }
    // 这里考虑得十分周全,甚至连四通道的图像都考虑到了  个人映像里四通道多了个alpha透明度吧
    else if(mImGray.channels()==4)
    {
   
        if(mbRGB)
        {
   
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
        }
        else
        {
   
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
            cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
        }
    }

    // Step 2 ：构造Frame
    mCurrentFrame = Frame(
        mImGray,                //左目图像
        imGrayRight,            //右目图像
        timestamp,              //时间戳
        mpORBextractorLeft,     //左目特征提取器
        mpORBextractorRight,    //右目特征提取器
        mpORBVocabulary,        //字典
        mK,                     //内参矩阵
        mDistCoef,              //去畸变参数
        mbf,                    //基线长度
        mThDepth);              //远点,近点的区分阈值

    // Step 3 ：跟踪
    Track();//追踪就应该是计算位姿

    //返回位姿
    return mCurrentFrame.mTcw.clone();
}

/**
 *  这里是RGBD的获取图片的函数、同时利用获得的灰度图构建当前帧，进行tracking过程，得到世界到相机的位姿变换
 **/
// 输入左目RGB或RGBA图像和深度图
// 1、将图像转为mImGray和imDepth并初始化mCurrentFrame
// 2、进行tracking过程
// 输出世界坐标系到该帧相机坐标系的变换矩阵
cv::Mat Tracking::GrabImageRGBD(
    const cv::Mat &imRGB,           //彩色图像
    const cv::Mat &imD,             //深度图像
    const double &timestamp)        //时间戳
{
   
    mImGray = imRGB;
    cv::Mat imDepth = imD;

    // step 1：将RGB或RGBA图像转为灰度图像
    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);
    }

    // step 2 ：将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
    //这里的判断条件感觉有些尴尬
    //前者和后者满足一个就可以了
    //mDepthMapFactor--深度相机disparity转化为depth时的因子
    //满足前者意味着,mDepthMapFactor 相对1来讲要足够大
    //满足后者意味着,如果深度图像不是浮点型? 才会执行
    //意思就是说,如果读取到的深度图像是浮点型,就不执行这个尺度的变换操作了呗? TODO 
    if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F) //深度图得是CV_32F
        imDepth.convertTo(  //将图像转换成为另外一种数据类型,具有可选的数据大小缩放系数
            imDepth,            //输出图像
            CV_32F,             //输出图像的数据类型
            mDepthMapFactor);   //缩放系数

    // 步骤3：构造Frame
    mCurrentFrame = Frame(
        mImGray,                //灰度图像
        imDepth,                //深度图像
        timestamp,              //时间戳
        mpORBextractorLeft,     //ORB特征提取器
        mpORBVocabulary,        //词典
        mK,                     //相机内参矩阵
        mDistCoef,              //相机的去畸变参数
        mbf,                    //相机基线*相机焦距
        mThDepth);              //内外点区分深度阈值

    // 步骤4：跟踪
    Track();

    //返回当前帧的位姿
    return mCurrentFrame.mTcw.clone();
}

/**
 * @brief 
 * 输入左目RGB或RGBA图像，输出世界坐标系到该帧相机坐标系的变换矩阵
 * 
 * @param[in] im 单目图像
 * @param[in] timestamp 时间戳
 * @return cv::Mat 
 * 
 * Step 1 ：将彩色图像转为灰度图像
 * Step 2 ：构造Frame
 * Step 3 ：跟踪
 */
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im,const double &timestamp)
{
   
    mImGray = im;

    // Step 1 ：将彩色图像转为灰度图像
    //若图片是3、4通道的，还需要转化成灰度图
    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);
    }

    // Step 2 ：构造Frame
    //判断该帧是不是初始化
    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(
            mImGray,
            timestamp,
            mpIniORBextractor,      //初始化ORB特征点提取器会提取2倍的指定特征点数目
            mpORBVocabulary,
            mK,
            mDistCoef,
            mbf,
            mThDepth);
    else
        mCurrentFrame = Frame(
            mImGray,
            timestamp,
            mpORBextractorLeft,     //正常运行的时的ORB特征点提取器，提取指定数目特征点
            mpORBVocabulary,
            mK,
            mDistCoef,
            mbf,
            mThDepth);

    // Step 3 ：跟踪
    Track();

    //返回当前帧的位姿
    return mCurrentFrame.mTcw.clone();
}




/*
 * @brief Main tracking function. It is independent of the input sensor.
 *
 * track包含两部分：估计运动、跟踪局部地图
 * 
 * Step 1：初始化
 * Step 2：跟踪
 * Step 3：记录位姿信息，用于轨迹复现
 */
/**
 * NO_IMAGES_YET等同于NOT_INITIALIZED
 * 首先判断状态，如果状态是NOT_INITIALIZED 那么要进行初始化
 * step1 初始化 根据相机类型选择初始化函数 ----  StereoInitialization()-双目/RGBD  MonocularInitialization()-单目;
 *
 * step2 系统初始化过后，跟踪帧---总之就是计算当前帧的位姿
 *       跟踪分为两种
 *       1、跟踪进入正常SLAM模式，有地图更新  不仅追踪  同时局部地图被激活
 *           1.1 正常初始化成功
 *                运动模型是空的或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪( 第一个条件,如果运动模型为空,说明是刚初始化开始，或者已经跟丢了
                                                                      第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们将重定位帧来恢复位姿)
 *                如果恒速模型跟踪失败那也要跟踪参考关键帧
 *           1.2 如果跟踪状态不成功,那么就只能重定位了---BOW搜索，EPnP求解位姿----重定位的函数挺复杂的
 *       2、只进行跟踪tracking，局部地图不工作
 *           2.1 如果状态是跟踪丢失 只能重定位了---BOW搜索，EPnP求解位姿----重定位的函数挺复杂的
 *           2.2 如果状态没有跟踪丢失(因为此时已经初始化了，状态没有跟踪丢失，就是跟踪正常)
 *                mbVO是mbOnlyTracking为true时的才有的一个变量
 *                mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常 (注意有点反直觉)
 *                mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏
 *                2.2.1 mbvO为false时，利用恒速模型或者参考帧计算当前帧位姿
 *                2.2.2 mbvO为true时，这时候点不够多，恒速模型，重定位都用上，更相信重定位，通过这个得到位姿 ，重定位和恒速模型有一个成功就行
 *
 * Step 3 在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
 *        前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
 *        3.1 跟踪进入正常SLAM模式，有地图更新 调用TrackLocalMap()函数，进行局部地图跟踪，得到优化的位姿
 *        3.2 只进行跟踪tracking，局部地图不工作 这里是恒速模型，重定位都用到
 *step3中两种情况都要追踪局部地图 最终 返回局部追踪后的状态 如果局部追踪成功，状态就是OK 否则就是 LOST
 *
 * Step 4 更新显示线程中的图像、特征点、地图点等信息----mpFrameDrawer->Update(this);
 *
 * 只有在成功追踪时才考虑生成关键帧的问题
 *
 * Step 5 跟踪成功，更新恒速运动模型----肯定要更新哦，因为得到当前帧的位姿，之前的恒速模型是上上帧到上帧的模型，追踪成功后，恒速模型肯定变成从上一帧到当前帧了
 *
 * 更新显示中的位姿----当前帧的位姿已经通过追踪得到啦!
 *
 * Step 6 清除观测不到的地图点----个人理解是，当前帧上的有些特征点在空间中没有与之对应的路标点(3d)，就让这些点存空指针，
 *
 * Step 7  清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）---添加临时的地图点为了提高追踪精度吧
 *         步骤6中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
 *         临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
 *         不仅仅是清除临时添加的MapPoints-mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint--？？不太懂
 *
 * Step 8 检测并插入关键帧，对于双目或RGB-D会产生新的地图点
 *
 * Step 9 删除那些在bundle adjustment中检测为outlier的地图点
 *         作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点
 *         但是估计下一帧位姿的时候我们不想用这些外点，所以删掉

 * Step 10 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset
 *
 * Step 11 记录位姿信息，用于最后保存所有的轨迹
 *         11.1 如果当前帧跟踪好了，保存跟踪成功的位姿信息
 *              计算相对位姿-Tcr = Tcw * Twr, Twr = Trw^-1
 *         11.2 如果跟踪失败，则相对位姿使用上一次值
 *
 */
void Tracking::Track()
{
   
    // track包含两部分：估计运动、跟踪局部地图
    
    // mState为tracking的状态，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
    // 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
    if(mState==NO_IMAGES_YET)
    {
   
        mState = NOT_INITIALIZED;//如果没有图片也算是没有初始化
    }

    // mLastProcessedState 存储了Tracking最新的状态，用于FrameDrawer中的绘制
    mLastProcessedState=mState;

    // Get Map Mutex -> Map cannot be changed
    // 上锁。保证地图不会发生变化
    // 疑问:这样子会不会影响地图的更新?
    // 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

    // Step 1：初始化
    if(mState==NOT_INITIALIZED)
    {
   
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            //双目RGBD相机的初始化共用一个函数
            StereoInitialization();
        else
            //单目初始化
            MonocularInitialization();

        //更新帧绘制器中存储的最新状态
        mpFrameDrawer->Update(this);

        //这个状态量在上面的初始化函数中被更新
        if(mState!=OK)
            return;
    }
    else
    {
   
        //系统已经初始化 ，跟踪帧
        // System is initialized. Track Frame.
        // bOK为临时变量，用于表示每个函数是否执行成功
        bool bOK;

        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        // mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式
        // tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking
        if(!mbOnlyTracking)// Step 2：跟踪进入正常SLAM模式，有地图更新  不仅追踪  同时局部地图被激活
        {
   
            // Local Mapping is activated. This is the normal behaviour, unless
            // you explicitly activate the "only tracking" mode.

            // Step 2：跟踪进入正常SLAM模式，有地图更新
            // 正常初始化成功
            if(mState==OK)
            {
   
                // Local Mapping might have changed some MapPoints tracked in last frame
                // Step 2.1 检查并更新上一帧被替换的MapPoints
                // 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查
                CheckReplacedInLastFrame();

                // Step 2.2 运动模型是空的或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪
                // 第一个条件,如果运动模型为空,说明是刚初始化开始，或者已经跟丢了
                // 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们将重定位帧来恢复位姿
                // mnLastRelocFrameId 上一次重定位的那一帧
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
   
                    // 用最近的关键帧来跟踪当前的普通帧
                    // 通过BoW的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点都对应3D点重投影误差即可得到位姿
                    bOK = TrackReferenceKeyFrame();//跟踪参考帧
                }
                else
                {