【ORBSLAM2源码笔记（3）】Tracking线程跟踪定位流程及Track()源码解析

【ORBSLAM2源码笔记（3）】Tracking线程跟踪定位流程及Track()源码解析

有道云笔记思维导图整理链接：

Track()源码流程解析: https://note.youdao.com/s/O9lzEplm

Track()源码注释及解析：

在这里一部分引用了计算机视觉life的教程内容，加上自己对代码部分的理解

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;

    // 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)
        {
            // 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点重投影误差即可得到位姿(BA)
                    bOK = TrackReferenceKeyFrame();
                }
                else
                {
                    // 用最近的普通帧来跟踪当前的普通帧
                    // 根据恒速模型设定当前帧的初始位姿
                    // 通过投影的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点所对应3D点的投影误差即可得到位姿(BA)
                    bOK = TrackWithMotionModel();
                    if(!bOK)
                        //若根据恒速模型计算初始位姿失败了，只能根据参考关键帧来跟踪
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else
            {
                // 如果跟踪状态不成功,那么就只能重定位了
                // BOW搜索，EPnP求解位姿
                bOK = Relocalization();
            }
        }
        else
        {
            // Localization Mode: Local Mapping is deactivated
            // Step 2：只进行跟踪tracking，局部地图不工作
            if(mState == LOST)
            {
                // Step 2.1 如果跟丢了，只能重定位
                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
                    // Step 2.2 如果跟踪正常，运动模型非空，使用恒速模型进行跟踪定位
                    if(!mVelocity.empty())
                    {
                        bOK = TrackWithMotionModel();
                    }
                    // Step 2.2 如果跟踪不正常，运动模型为空，使用参考关键帧进行跟踪定位
                    else
                    {
                        bOK = TrackReferenceKeyFrame();
                    }
                }

                /// mbVO为true，表明此帧匹配了很少（小于10）的地图点
                /// MapPoints < 10
                /// 跟踪异常，需要同时进行跟踪和重定位
                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;                             /// MM，“Motion Model”的缩写,表示通过运动模型进行跟踪的结果
                    bool bOKReloc = false;                          /// 通过重定位进行跟踪的结果
                    vector<MapPoint*> vpMPsMM;                      /// 运动模型中构造的地图点
                    vector<bool> 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;

                        // 如果当前帧匹配的地图点较少，增加当前可视地图点的观测次数
                        if(mbVO)
                        {
                            // mCurrentFrame.N: 当前帧的特征点个数
                            // 更新当前帧的每一个特征点被观测到的地图点个数
                            for(int i =0; i<mCurrentFrame.N; i++)
                            {
                                // mvpMapPoints: 每个特征点对应的MapPoint
                                // mvbOutlier: 每个特征点对应的外点
                                // 如果当前帧的该特征点观测到对应的MapPoint，并且不存在属于外点的观测
                                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                                {
                                    // IncreaseFound(): 能找到该特征点的地图点个数+1
                                    mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                                }
                            }
                        }
                    }

                    // 只要重定位成功整个跟踪过程正常进行（重定位与运动模型，更相信重定位）
                    else if(bOKReloc)
                    {
                        mbVO = false;
                    }

                    // 只要其中一个成功，系统就认为仅跟踪tracking初始姿态的过程执行成功
                    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后进行位姿优化
        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
        // 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 = mCurrentFrame.mTcw*LastTwc;
            }
            else
                //否则速度为空
                mVelocity = cv::Mat();

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

            // Clean VO matches
            // Step 6：清除观测不到的地图点
            for(int i=0; i<mCurrentFrame.N; i++)
            {
                /// mvpMapPoints:每个特征点对应的MapPoint.如果特征点没有对应的地图点,那么将存储一个空指针
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                // 判断该特征点有无对应的地图点
                if(pMP)
                    // Observations(): 返回地图点被观测到的相机数目，单目+1，双目或RGB-D则+2
                    // < 1表示该地图点不能被观测到
                    if(pMP->Observations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
                    }
            }

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

            // 不能够直接执行这个是因为其中存储的都是指针,前面循环中的操作都是为了避免内存泄露
            mlpTemporalPoints.clear();

            // Check if we need to insert a new keyframe
            // Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点
            if(NeedNewKeyFrame())
                CreateNewKeyFrame();
            idk ++;

            // 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来审判他们是不是真正的外点
            // 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉
            for(int i = 0; i < mCurrentFrame.N; i++)
            {
                if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
            }
        }

        // Reset if the camera get lost soon after initialization
        // Step 10 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Reset
        if(mState == LOST)
        {
            // 如果地图中的关键帧信息过少的话,直接重新进行初始化了
            // KeyFramesInMap(): 返回地图中的关键帧数目
            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
        // 如果跟踪失败，则相对位姿使用上一次值 (.back())
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState == LOST);
    }

}