DynaSLAM代码详解(5) — Tracking.cc跟踪线程

目录

5.1 DynaSLAM中Tracking线程简介

5 .2 RGBD模式下跟踪流程

5.3 DynaSLAM的低成本跟踪

(1) Tracking::LightTrack() 低成本跟踪函数

(2) Tracking::LightTrackWithMotionModel() 低成本的恒速模型跟踪流程

5.4 DynaSLAM的正常跟踪

---

文章着重将与ORB-SLAM2不同的地方，关于Tracking中具体跟踪过程参考ORB-SLAM2方面的博文

5.1 DynaSLAM中Tracking线程简介

        DynaSLAM的跟踪线程与ORB-SLAM2的跟踪线程类似，负责估计运动信息、跟踪局部地图。
总体来说，ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

与ORB-SLAM不同：DynaSLAM会有一个Low-Cost Tracking。

为什么会有低成本的跟踪模式：在RGB-D情况下，在获得了动态物体分割的结果后，有必要了解相机位姿，为此，我们实现了一个低成本的跟踪模块，用于在已创建的场景地图中定位相机。

5 .2 RGBD模式下跟踪流程

跟踪线程的流程以RGBD模式为例，输入RGB或RGBA图像、深度图、动态物体分割Mask；首先将图像转为mImGray和imDepth；然后获取深度相机的真实Depth；初始化mCurrentFrame并进行低成本tracking过程；初始化mCurrentFrame进行正常tracking过程；最后输出世界坐标系到该帧相机坐标系的变换矩阵。

与ORB-SLAM不同：DynaSLAM会有一个低成本的跟踪LightTrack()

// 输入RGB或RGBA图像、深度图、动态物体分割Mask
// 1、将图像转为mImGray和imDepth
// 2、获取深度相机的真实Depth
// 3、初始化mCurrentFrame后进行低成本tracking过程
// 4、初始化mCurrentFrame后进行正常tracking过程
// 输出世界坐标系到该帧相机坐标系的变换矩阵
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, cv::Mat &mask,
                                const double ×tamp, cv::Mat &imRGBOut,
                                cv::Mat &imDOut, cv::Mat &maskOut)
{
    mImGray = imRGB;
    cv::Mat imDepth = imD;
    cv::Mat imMask = mask;
    cv::Mat _imRGB = imRGB;

    // step1：将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);
    }
    // step2：将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
    if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
        imDepth.convertTo(imDepth,      //输出图像
                          CV_32F,       //输出图像的数据类型
                          mDepthMapFactor);     //缩放系数

     // step3：构造Frame用于低成本跟踪
    mCurrentFrame = Frame(
        mImGray,            //灰度图像
        imDepth,            //深度图像
        imMask,             //动态物体检测Mask
        _imRGB,             //RGB图像
        timestamp,          //时间戳
        mpORBextractorLeft, //ORB特征提取器
        mpORBVocabulary,    //词典
        mK,                 //相机内参矩阵
        mDistCoef,          //相机的去畸变参数
        mbf,                //相机基线*相机焦距
        mThDepth);          //内外点区分深度阈值

    // 低成本跟踪
    LightTrack();
    //将低成本跟踪的结果赋值给imRGBOut
    imRGBOut = _imRGB;

    //！ 如果低成本跟踪求解的位姿不可用，则利用多视图几何的方法
    if (!mCurrentFrame.mTcw.empty())
    {
        mGeometry.GeometricModelCorrection(mCurrentFrame,imDepth,imMask);
    }

    // step4：构造Frame用于正常跟踪
    mCurrentFrame = Frame(mImGray,imDepth,imMask,imRGBOut,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    // 正常跟踪
    Track();

    if (!mCurrentFrame.mTcw.empty())
    {
        mGeometry.InpaintFrames(mCurrentFrame, mImGray, imDepth, imRGBOut, imMask);
    }

    mGeometry.GeometricModelUpdateDB(mCurrentFrame);

    // step5：输出跟踪结果
    imDOut = imDepth;
    imDepth.convertTo(imDOut,CV_16U,1./mDepthMapFactor);
    maskOut = imMask;

    return mCurrentFrame.mTcw.clone();
}

5.3 DynaSLAM的低成本跟踪

(1) Tracking::LightTrack() 低成本跟踪函数

低成本跟踪的整体流程：

与正常跟踪的对比：低成本跟踪相当于是简化版的跟踪流程，没有仅定位模式跟踪和局部地图跟踪的流程。

void Tracking::LightTrack()
{
    // Get Map Mutex -> Map cannot be changed
    unique_lock lock(mpMap->mMutexMapUpdate);

    // 是否使用恒速模型跟踪
    bool useMotionModel = true; //set true

    // 如果跟踪线程没有被初始化则退出
    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
    {
        cout << "Light Tracking not working because Tracking is not initialized..." << endl;
        return;
    }
    // 系统已初始化，开始进行跟踪状态的判断
    else
    {
        // System is initialized. Track Frame.
        bool bOK;
        {
            // Localization Mode:
            // 如果当前状态为LOST，则进行重定位跟踪
            if(mState==LOST)
            {
                bOK = Relocalization(1);
            }
            else
            {
                // 如果上一帧在地图中跟踪到足够多的地图点
                if(!mbVO)
                {
                    // In last frame we tracked enough MapPoints in the map
                    // 且mVelocity不为空且useMotionModel为true,进行低成本的跟踪
                    if(!mVelocity.empty() && useMotionModel)
                    {
                        bool _bOK = false;
                        bOK = LightTrackWithMotionModel(_bOK);// TODO: check out!!!
                    }
                    // 否则进行参考关键帧跟踪
                    else
                    {
                        bOK = TrackReferenceKeyFrame();
                    }
                }
                // mbVO为true，表明此帧匹配了很少的地图点，既做跟踪又做重定位
                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;
                    // 低成本的跟踪的结果
                    bool lightTracking = false;
                    bool bVO = false;

                    // 且mVelocity不为空且useMotionModel为true,进行低成本的跟踪
                    if(!mVelocity.empty() && useMotionModel)
                    {
                        lightTracking = true;
                        //进行低成本的恒速跟踪
                        bOKMM = LightTrackWithMotionModel(bVO); // TODO: check out!!
                        // 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量
                        vpMPsMM = mCurrentFrame.mvpMapPoints;
                        vbOutMM = mCurrentFrame.mvbOutlier;
                        TcwMM = mCurrentFrame.mTcw.clone();
                    }
                    // 使用重定位的方法来得到当前帧的位姿
                    bOKReloc = Relocalization(1);
                    //根据前面的恒速模型、重定位结果来更新状态
                    if(bOKMM && !bOKReloc)
                    {
                         恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果
                        mCurrentFrame.SetPose(TcwMM);
                        mCurrentFrame.mvpMapPoints = vpMPsMM;
                        mCurrentFrame.mvbOutlier = vbOutMM;

                        if((lightTracking && bVO) || (!lightTracking && mbVO))
                        {
                            // 更新当前帧的地图点被观测次数
                            for(int i =0; iIncreaseFound();
                                }
                            }
                        }
                    }
                    //有一个成功我们就认为执行成功了
                    bOK = bOKReloc || bOKMM;
                }
            }
        }

        // 将当前帧的参考关键帧指针设置为mpReferenceKF
        mCurrentFrame.mpReferenceKF = mpReferenceKF;

        if(!bOK)
        {
            //地图中的关键帧数量小于等于5，则低成本跟踪失败
            if(mpMap->KeyFramesInMap()<=5)
            {
                cout << "Light Tracking not working..." << endl;
                return;
            }
        }

        // 如果当前帧的参考关键帧指不存在，重新设置
            mCurrentFrame.mpReferenceKF = mpReferenceKF;

    }
}

(2) Tracking::LightTrackWithMotionModel() 低成本的恒速模型跟踪流程

Tracking::LightTrackWithMotionModel()函数的作用是用上一帧地图点来对当前帧进行跟踪。

1. 更新上一帧的位姿，目的是为了得到更好的地图点位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点。
2. 根据之前估计的速度，用恒速模型得到当前帧的初始位姿，恒速模型利用上一帧的位姿作为当前帧的位姿的初始值。
3. 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
4. 利用3D-2D投影关系，优化当前帧位姿 ，上一帧的位姿只是初始值，优化后更加精确
5. 剔除地图点中外点
6. 最后判断，如果匹配数大于20，认为跟踪成功，返回true；在正常跟踪下匹配数设置为10。

与正常恒速模型跟踪的对比：低成本的恒速模型跟踪和正常恒速模型跟踪基本相同，代码存在冗余；低成本的恒速模型跟踪比正常恒速模型跟踪会多一个仅定位模式的判断。

/**
 * @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
 * Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
 * Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿
 * Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
 * Step 4：利用3D-2D投影关系，优化当前帧位姿 
 * Step 5：剔除地图点中外点
 * @return 如果匹配数大于20，认为跟踪成功，返回true
 */
bool Tracking::LightTrackWithMotionModel(bool &bVO)
{
    // 最小距离 < 0.9 * 次小距离 匹配成功，检查旋转
    ORBmatcher matcher(0.9,true);

    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points if in Localization Mode
    Frame lastFrameBU = mLastFrame;
    list lpTemporalPointsBU = mlpTemporalPoints;

    // Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点
    UpdateLastFrame(); //TODO: check out!

    // Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿
    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

    // 清空当前帧的地图点
    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL)); //TODO:Checkout

    // Project points seen in previous frame
    // 设置特征匹配过程中的搜索半径
    int th;
    if(mSensor!=System::STEREO)
        th=15;    //单目
    else
        th=7;     //双目
    // Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);//TODO:Checkout

    // If few matches, uses a wider window search
    // 如果匹配点太少，则扩大搜索半径再来一次
    if(nmatches<20)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));//TODO:Checkout
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);//TODO:Checkout
    }

    // 如果还是不能够获得足够的匹配点,那么就认为跟踪失败
    if(nmatches<20)
        return false;

    // Optimize frame pose with all matches
    // Step 4：利用3D-2D投影关系，优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);

    // Discard outliers
    // Step 5：剔除地图点中外点
    int nmatchesMap = 0;
    for(int i =0; i(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                // 累加成功匹配到的地图点数目
                nmatchesMap++;
        }
    }
    mLastFrame = lastFrameBU;
    mlpTemporalPoints = lpTemporalPointsBU;

    // 如果成功追踪的地图点非常少, 那么这里的bVO标志就会置位
    bVO = nmatchesMap<10;

    // Step 6：匹配超过20个点就认为跟踪成功
    return nmatches>20;

}

5.4 DynaSLAM的正常跟踪

        DynaSLAM的正常跟踪 Tracking::Track() 与ORB-SLAM2的跟踪函数相同，跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

        关于参考关键帧来跟踪、恒速模型跟踪、重定位跟踪、局部地图跟踪可以参考其他博主的解析。

/*
 * @brief Main tracking function. It is independent of the input sensor.
 *
 * track包含两部分：估计运动、跟踪局部地图
 * 
 * Step 1：初始化
 * Step 2：跟踪
 * Step 3：记录位姿信息，用于轨迹复现
 */
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 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)
        {
            // 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 vpMPsMM;
                    //在追踪运动模型后发现的外点
                    vector vbOutMM;
                    //运动模型得到的位姿
                    cv::Mat TcwMM;

                    // Step 2.3 当运动模型有效的时候,根据运动模型计算位姿
                    if(!mVelocity.empty())
                    {
                        bOKMM = TrackWithMotionModel();

                        // 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量
                        vpMPsMM = mCurrentFrame.mvpMapPoints;
                        vbOutMM = mCurrentFrame.mvbOutlier;
                        TcwMM = mCurrentFrame.mTcw.clone();
                    }

                    // Step 2.4 使用重定位的方法来得到当前帧的位姿
                    bOKReloc = Relocalization();

                    // Step 2.5 根据前面的恒速模型、重定位结果来更新状态
                    if(bOKMM && !bOKReloc)
                    {
                        // 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果
                        mCurrentFrame.SetPose(TcwMM);
                        mCurrentFrame.mvpMapPoints = vpMPsMM;
                        mCurrentFrame.mvbOutlier = vbOutMM;

                        //? 疑似bug！这段代码是不是重复增加了观测次数？后面 TrackLocalMap 函数中会有这些操作
                        // 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数
                        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.
        // Step 3：在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
        // 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
        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();
        }

        //根据上面的操作来判断是否追踪成功
        if(bOK)
            mState = OK;
        else
            mState=LOST;

        // Step 4：更新显示线程中的图像、特征点、地图点等信息
        mpFrameDrawer->Update(this);

        // If tracking were good, check if we insert a keyframe
        //只有在成功追踪时才考虑生成关键帧的问题
        if(bOK)
        {
            // Update motion model
            // Step 5：跟踪成功，更新恒速运动模型
            if(!mLastFrame.mTcw.empty())
            {
                // 更新恒速运动模型 TrackWithMotionModel 中的mVelocity
                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 = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc
                mVelocity = mCurrentFrame.mTcw*LastTwc; 
            }
            else
                //否则速度为空
                mVelocity = cv::Mat();

            //更新显示中的位姿
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

            // Clean VO matches
            // Step 6：清除观测不到的地图点   
            for(int i=0; iObservations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
                    }
            }

            // Delete temporal MapPoints
            // Step 7：清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
            // 步骤6中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
            // 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
            for(list::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }

            // 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
            // 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露
            mlpTemporalPoints.clear();

            // Check if we need to insert a new keyframe
            // Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点
            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.
            // 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点
            // 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉

            //  Step 9 删除那些在bundle adjustment中检测为outlier的地图点
            for(int i=0; i(NULL);
            }
        }

        // Reset if the camera get lost soon after initialization
        // Step 10 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset
        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.
    // Step 11：记录位姿信息，用于最后保存所有的轨迹
    if(!mCurrentFrame.mTcw.empty())
    {
        // 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1
        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