ORB-SLAM2源码逐行解析系列（六）：ORB-SLAM2中的单目Track

1. ORB-SLAM2中的单目追踪

  ORB-SLAM2中跟踪线程主要分为两个阶段，第一个阶段包括3种跟踪方式：参考关键帧跟踪、恒速模型跟踪和重定位跟踪，它们的目的是保证能够跟得上，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，它将当前帧的局部关键帧对应的局部地图点投影到该帧中，得到更多的特征点匹配关系，对第一阶段的位姿再次进行优化，得到相对准确的位姿（这几种跟踪方式的具体实现后面会详细介绍）。

2. 单目追踪流程

   通过上述流程图可知，第一阶段中大部分时间均为恒速模型跟踪，只有在速度未空或者刚重定位时使用参考关键帧跟踪，另外在跟踪失败后使用重定位跟踪。当第一阶段跟踪成功后，进入第二阶段使用局部地图跟踪，获取相对准确的位姿。

3. 代码实现

/*
 * @brief Main tracking function. It is independent of the input sensor.
 *
 * track包含两部分：估计运动、跟踪局部地图
 * 
 * Step 1：初始化
 * Step 2：跟踪
 * Step 3：记录位姿信息，用于轨迹复现
 */
void Tracking::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;

    // 地图更新时加锁。保证地图不会发生变化
    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  // 初始化成功后，执行以下部分
    {
        // bOK为临时变量，用于表示每个函数是否执行成功
        bool bOK;
/*
    mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式
    tracking 类构造时默认为false，即默认是SLAM模式。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTracking
*/
        if(!mbOnlyTracking)  // Step 2: 进入SLAM模式
        {
            if(mState==OK)  // 若可正常跟踪
            {
                // Step 2.1 检查并更新上一帧被替换的MapPoints
                // 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查(局部地图更新)
                CheckReplacedInLastFrame();
/*
   Step 2.2 运动模型是空的或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪
            第一个条件,如果运动模型为空,说明是刚初始化开始，或者已经跟丢了
            第二个条件,如果当前帧紧紧地跟在重定位帧的后面，则采用重定位帧来恢复位姿
            mnLastRelocFrameId: 表示上一次重定位的那一帧
*/
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
                    bOK = TrackReferenceKeyFrame();  // 参考关键帧跟踪
                }
                else
                {   
                    // 不满足上述条件时，使用恒速模型跟踪
                    bOK = TrackWithMotionModel();
                    if(!bOK)
                        // 若恒速模型跟踪失败了，则只能根据参考关键帧来跟踪
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else  // Step 2.3: 若跟踪状态不成功,则只能重定位了
            {
                // BOW搜索，EPnP求解位姿
                bOK = Relocalization();
            }
        }
        else  // Step 3: 只能进行定位
        {
            if(mState==LOST)  // Step 3.1: 如果跟丢了，则只能重定位
            {
                bOK = Relocalization();
            }
            else
            {
                // mbVO为false表示当前帧匹配了很多MapPoints的情况
                // mbVO为true表示当前帧匹配的MapPoints较少(<10个)的情况
                if(!mbVO)  // Step 3.2: 在可正常跟踪的情况下的跟踪
                {
                    if(!mVelocity.empty())  // 若运动模型不为空
                    {
                        bOK = TrackWithMotionModel();  // 则使用恒速模型进行跟踪
                    }
                    else
                    {
                        bOK = TrackReferenceKeyFrame();  // 否则使用参考关键帧进行跟踪
                    }
                }
               // Step 3.3: 当前帧匹配的MapPoints较少，跟踪完之后需要增加可视MapPoints的被观测次数
                else  
                {
                    // 定义相关变量
                    bool bOKMM = false;         // bOKMM表示恒速模型跟踪结果
                    bool bOKReloc = false;      // bOKReloc表示重定位跟踪结果
                    vector<MapPoint*> vpMPsMM;  // vpMPsMM表示恒速模型跟踪中构造的地图点
                    vector<bool> vbOutMM;       // vbOutMM表示恒速模型跟踪中发现的外点
                    cv::Mat TcwMM;              // TcwMM表示恒速模型跟踪获得的位姿
                    // Step 3.3.1: 恒速模型跟踪
                    if(!mVelocity.empty())
                    {
                        bOKMM = TrackWithMotionModel();
                        vpMPsMM = mCurrentFrame.mvpMapPoints;
                        vbOutMM = mCurrentFrame.mvbOutlier;
                        TcwMM = mCurrentFrame.mTcw.clone();
                    }
                    // Step 3.3.2: 重定位跟踪
                    bOKReloc = Relocalization();
                    // Step 3.3.3: 根据前面的恒速模型、重定位结果来更新状态
                    if(bOKMM && !bOKReloc)  
                    {
                        // 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果来更新当前帧状态
                        mCurrentFrame.SetPose(TcwMM);
                        mCurrentFrame.mvpMapPoints = vpMPsMM;
                        mCurrentFrame.mvbOutlier = vbOutMM;
                        
                        // 如果当前帧匹配的3D点(MapPoints)很少，增加当前可视地图点的被观测次数
                        if(mbVO)  // 个人认为这里的if判断可去掉，因为在else中mbVO必为true
                        {
                            // 更新当前帧的地图点被观测次数
                            for(int i =0; i<mCurrentFrame.N; i++)
                            {
                                // 如果这个特征点形成了地图点,并且也不是外点的时候
                                if(mCurrentFrame.mvpMapPoints[i] && 
                                   !mCurrentFrame.mvbOutlier[i])
                                {
                                    // 增加能观测到该地图点的帧数
                                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)
                    {
              // Step 3.3.4: 若重定位跟踪成功，则表示整个跟踪过程正常进行(重定位与跟踪，更相信重定位)
              // 匹配的3D点自然就足够多，即mbVO = false
                        mbVO = false;
                    }
/*
     重定位跟踪与恒速模型跟踪中只要有一个成功，就认为执行成功了
     bOKReloc放在前面，间接表明只要重定位成功就执行成功了，不用看恒速模型结果，即更相信重定位
*/
                    bOK = bOKReloc || bOKMM;
                }
            }
        }
/*
   Step 4: 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
*/
        // 将最新的关键帧作为当前帧的参考关键帧 ???
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
        if(!mbOnlyTracking)  // SLAM(定位+建图)模式下，进行局部地图跟踪
        {
            if(bOK)
                bOK = TrackLocalMap();
        }
        else  // 定位模型下，若匹配的3D点足够多，则进行局部地图跟踪
        {
            if(bOK && !mbVO)
                bOK = TrackLocalMap();
        }
        // 根据上面的操作来判断是否追踪成功
        if(bOK)
            mState = OK;
        else
            mState=LOST;
        
        // Step 5：更新显示线程中的图像、特征点、地图点等信息
        mpFrameDrawer->Update(this);
        // Step 6: 只有在成功追踪时才考虑生成关键帧的问题
        if(bOK)
        {
            // Step 6.1：跟踪成功，更新恒速运动模型(疑问: 需要更新的内容后面具体分析???)
            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 = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwc
                mVelocity = mCurrentFrame.mTcw*LastTwc; 
            }
            else
                // 否则速度为空
                mVelocity = cv::Mat();
            // Step 6.2：更新显示中的位姿
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
            
            // Step 6.3：清除观测不到的地图点   
            for(int i=0; i<mCurrentFrame.N; i++)
            {
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(pMP)
                    if(pMP->Observations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                    }
            }
/*
   Step 6.4：清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）
   步骤6.3中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
   临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
*/
            for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), 
                lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
            // 在将mlpTemporalPoints中每个元素delete掉之后，还需要将mlpTemporalPoints销毁掉
            mlpTemporalPoints.clear();
            
            // Step 6.5: 检测并插入关键帧，对于双目或RGB-D会产生新的地图点
            if(NeedNewKeyFrame())
                CreateNewKeyFrame();
            
            // Step 6.6: 删除那些在bundle adjustment中检测为outlier的地图点
            for(int i=0; i<mCurrentFrame.N;i++)
            {
                // 若当前帧的第i个MapPoint存在，但被认为是Outlier，则将该MapPoint置为空
                if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
                    mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
            }
        }
        // Step 7: 如果初始化后不久就跟踪失败，并且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);
    }
    
    // Step 8: 记录位姿信息，用于最后保存所有的轨迹
    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
    {
        // 如果跟踪失败，则相对位姿使用上一次值
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState==LOST);
    }
}