认真的虎ORBSLAM2源码解读(六)：Tracking

1. 简述

系统将图片交给Tracking::GrabImageMonocular()后，先将图片转化为灰度图，然后使用图片构建了一个Frame。
注意系统在初始化的时候使用了不同的ORBextractor来构建Frame，是应为在初始化阶段的帧需要跟多的特征点

然后将进入了Track()，Tracking的大部分功能将在这里实现

Tracking执行了4个任务：

1. 单目初始化；
2. 通过上一帧获得初始位姿估计或者重定位。也就是求出当前帧在世界坐标系下的位姿$T_2$；
3. 跟踪局部地图（TrackLocalMap()） 求得位姿估计$T_3$。这个步骤回答当前帧的特征点和map中的哪些mappoint匹配。
4. 判断是否需要给localmapping插入关键帧；

2. 头文件

class Tracking
{
     

public:
    Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
             KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);

    // Preprocess the input and call Track(). Extract features and performs stereo matching.
    cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double &timestamp);
    cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp);
    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);

    void SetLocalMapper(LocalMapping* pLocalMapper);
    void SetLoopClosing(LoopClosing* pLoopClosing);
    void SetViewer(Viewer* pViewer);

    // Load new settings
    // The focal lenght should be similar or scale prediction will fail when projecting points
    // TODO: Modify MapPoint::PredictScale to take into account focal lenght
    void ChangeCalibration(const string &strSettingPath);

    // Use this function if you have deactivated local mapping and you only want to localize the camera.
    void InformOnlyTracking(const bool &flag);


public:

    // Tracking states
    enum eTrackingState{
   
        SYSTEM_NOT_READY=-1,
        NO_IMAGES_YET=0,
        NOT_INITIALIZED=1,
        OK=2,
        LOST=3
    };

    eTrackingState mState;
    eTrackingState mLastProcessedState;

    // Input sensor
    int mSensor;

    // Current Frame
    Frame mCurrentFrame;
    cv::Mat mImGray;

    // Initialization Variables (Monocular)
    //这个没有用到
    std::vector<int> mvIniLastMatches;
    //初始化时得到的特征点匹配，大小是mInitialFrame的特征点数量，其值是当前帧特征点序号
    std::vector<int> mvIniMatches;
    //mInitialFrame中待匹配的特征点的像素位置
    std::vector<cv::Point2f> mvbPrevMatched;
    //初始化时三角化投影成功的匹配点对应的3d点
    std::vector<cv::Point3f> mvIniP3D;
    //初始化的第一帧，初始化需要两帧,世界坐标系就是这帧的坐标系
    Frame mInitialFrame;

    // Lists used to recover the full camera trajectory at the end of the execution.
    // Basically we store the reference keyframe for each frame and its relative transformation
    list<cv::Mat> mlRelativeFramePoses;
    list<KeyFrame*> mlpReferences;
    list<double> mlFrameTimes;
    list<bool> mlbLost;

    // True if local mapping is deactivated and we are performing only localization
    bool mbOnlyTracking;

    void Reset();

protected:

    // Main tracking function. It is independent of the input sensor.
    //跟踪
    void Track();

    // Map initialization for stereo and RGB-D
    void StereoInitialization();

    // Map initialization for monocular
    /**
    * @brief 单目的地图初始化
    *
    * 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
    * 得到初始两帧的匹配、相对运动、初始MapPoints
    */
    void MonocularInitialization();
    
    //单目模式下初始化后，开始建图
    //将mInitialFrame和mCurrentFrame都设置为关键帧
    //新建mappoint
    //更新共视关系
    void CreateInitialMapMonocular();

    void CheckReplacedInLastFrame();
    
    /**
    * 将将上一帧的位姿作为当前帧mCurrentFrame的初始位姿；
    * 匹配参考帧关键帧中有对应mappoint的特征点与当前帧特征点，通过dbow加速匹配；
    * 以上一帧的位姿态为初始值，优化3D点重投影误差，得到更精确的位姿以及剔除错误的特征点匹配；
    * @return 如果匹配数大于10，返回true
    */
    bool TrackReferenceKeyFrame();
    /**
     * 更新mLastFrame
     * 更新mlpTemporalPoints
     */
    void UpdateLastFrame();
    
    /**
      * @brief 根据匀速度模型对上一帧mLastFrame的MapPoints与当前帧mCurrentFrame进行特征点跟踪匹配
      * 
      * 1. 非单目情况，需要对上一帧产生一些新的MapPoints（临时）
      * 2. 将上一帧的MapPoints投影到当前帧的图像平面上，在投影的位置进行区域匹配
      * 3. 根据匹配优化当前帧的姿态
      * 4. 根据姿态剔除误匹配
      * @return 如果匹配数大于10，返回true
      */
    bool TrackWithMotionModel();

    //BOW搜索候选关键帧，PnP求解位姿
    bool Relocalization();

    //更新局部地图，即更新局部地图关键帧，局部地图mappoint
    void UpdateLocalMap();
    //将mvpLocalKeyFrames中的mappoint，添加到局部地图关键点mvpLocalMapPoints中
    void UpdateLocalPoints();
    
    /**
     * 更新mpReferenceKF，mCurrentFrame.mpReferenceKF
     * 更新局部地图关键帧mvpLocalKeyFrames
     */
    void UpdateLocalKeyFrames();

    /**
    * @brief 对mvpLocalKeyFrames，mvpLocalMapPoints进行跟踪
    * 
    * 1. 更新局部地图，包括局部关键帧和关键点
    * 2. 以局部地图的mappoint为范围和当前帧进行特征匹配
    * 3. 根据匹配对通过BA估计当前帧的姿态
    * 4. 更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
    * @return 根据跟踪局部地图的效果判断当前帧的跟踪成功与否，返回其判断结果
    * @see V-D track Local Map
    */
    bool TrackLocalMap();
    //在局部地图的mappoint中查找在当前帧视野范围内的点，将视野范围内的点和当前帧的特征点进行投影匹配
    void SearchLocalPoints();

    //判断是否需要添加新的keyframe
    bool NeedNewKeyFrame();
    /**
    * @brief 创建新的关键帧
    *
    * 对于非单目的情况，同时创建新的MapPoints
    */
    void CreateNewKeyFrame();

    // In case of performing only localization, this flag is true when there are no matches to
    // points in the map. Still tracking will continue if there are enough matches with temporal points.
    // In that case we are doing visual odometry. The system will try to do relocalization to recover
    // "zero-drift" localization to the map.
    //在OnlyTracking模式中使用
    //true表明在上一帧中匹配到了足够多的mappoint
    bool mbVO;

    //Other Thread Pointers
    LocalMapping* mpLocalMapper;
    LoopClosing* mpLoopClosing;

    //ORB
    ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
    ORBextractor* mpIniORBextractor;

    //BoW
    ORBVocabulary* mpORBVocabulary;
    KeyFrameDatabase* mpKeyFrameDB;

    // Initalization (only for monocular)
    Initializer* mpInitializer;

    //Local Map
    //参考关键帧
    //在CreateNewKeyFrame()中，为当前帧
    //在UpdateLocalKeyFrames()中，为当前帧共视程度最高的关键帧
    KeyFrame* mpReferenceKF;
    std::vector<KeyFrame*> mvpLocalKeyFrames;
    //mvpLocalKeyFrames的所有关键帧的所有匹配的mappoint集合
    std::vector<MapPoint*> mvpLocalMapPoints;
    
    // System
    System* mpSystem;
    
    //Drawers
    Viewer* mpViewer;
    FrameDrawer* mpFrameDrawer;
    MapDrawer* mpMapDrawer;

    //Map
    Map* mpMap;

    //Calibration matrix
    cv::Mat mK;
    cv::Mat mDistCoef;
    float mbf;

    //New KeyFrame rules (according to fps)
    int mMinFrames;
    //
    int mMaxFrames;

    // Threshold close/far points
    // Points seen as close by the stereo/RGBD sensor are considered reliable
    // and inserted from just one frame. Far points requiere a match in two keyframes.
    float mThDepth;

    // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
    float mDepthMapFactor;

    //Current matches in frame
    //当前的匹配
    int mnMatchesInliers;

    //Last Frame, KeyFrame and Relocalisation Info
    //最近新插入的keyframe
    KeyFrame* mpLastKeyFrame;
    // 记录最近的一帧
    Frame mLastFrame;
    //tracking上一次插入mpLastKeyFrame的Frame的ID
    unsigned int mnLastKeyFrameId;
    //上一次Relocalization()使用的Frame ID，最近一次重定位帧的ID
    unsigned int mnLastRelocFrameId;

    //Motion Model
    cv::Mat mVelocity;

    //Color order (true RGB, false BGR, ignored if grayscale)
    bool mbRGB;

    //在UpdateLastFrame()更新
    list<MapPoint*> mlpTemporalPoints;
};

3.源文件

3.1.Track()

void Tracking:: Track()
{
   
    // 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
    if(mState==NO_IMAGES_YET)
    {
   
        mState = NOT_INITIALIZED;
    }

    mLastProcessedState=mState;

    // Get Map Mutex -> Map cannot be changed
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

    //如果tracking没有初始化，则初始化
    if(mState==NOT_INITIALIZED)
    {
   
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();

        mpFrameDrawer->Update(this);

        if(mState!=OK)
            return;
    }
    else
    //tracking初始化成功后
    {
   
        // System is initialized. Track Frame.
	// bOK为临时变量，bOK==true现在tracking状态正常,能够及时的反应现在tracking的状态
        bool bOK;

        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        // 用户可以通过在viewer中的开关menuLocalizationMode，控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking是否为true
        // mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
        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
		//lastframe中可以看到的mappoint替换为lastframe储存的备胎mappoint点mpReplaced，也就是更新mappoint
                CheckReplacedInLastFrame();

		// 运动模型是空的或刚完成重定位
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
   
		    /**
		    * 将参考帧关键帧的位姿作为当前帧的初始位姿进行跟踪；
		    * 匹配参考帧关键帧中有对应mappoint的特征点与当前帧特征点，通过dbow加速匹配；
		    * 优化3D点重投影误差，得到更精确的位姿以及剔除错误的特征点匹配；
		    * @return 如果匹配数大于10，返回true
		    */
                    bOK = TrackReferenceKeyFrame();
                }
                else
                {
   
                    bOK = TrackWithMotionModel();
                    if(!bOK)
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else
            {
   
		//BOW搜索候选关键帧，PnP求解位姿
                bOK = Relocalization();
            }
        }
        // 只进行跟踪tracking，局部地图不工作
        else
        {
   
            // Localization Mode: Local Mapping is deactivated

	    // tracking跟丢了
            if(mState==LOST)
            {
   
                bOK = Relocalization();
            }
            else
            {
   
		// mbVO是mbOnlyTracking为true时的才有的一个变量
                // mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常，
                // mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏
                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<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;

			//确认mbVO==1
                        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();
        }

        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())
            {
   
		//更新恒速运动模型TrackWithMotionModel中的mVelocity
                cv::Mat LastTwc = cv::