ORB-SLAM2从理论到代码实现（六）：Tracking.cc程序详解（上）

终于写到Tracking.cc了，想想还有点小激动呢。

本人邮箱jinbo666888@qq.com，欢迎交流。

本系列代码注释，是在吴博注释的基础上进一步注释。

1.Tracking 框架

Tracking线程流程框图：

 各流程对应的主要函数（来自吴博@泡泡机器人）：

 

Tracking整体流程图（ 来自https://blog.csdn.net/chishuideyu/article/details/75314896）

上面这张图把Tracking.cc讲的特别明白。

tracking线程在获取图像数据后，会传给函数GrabImageStereo、GrabImageRGBD或GrabImageMonocular进行预处理，这里我以GrabImageMonocular为例。

• GrabImageMonocular(const cv::Mat &im, const double &timestamp)

函数功能	1、将图像转为mImGray并初始化mCurrentFrame，2、进行tracking过程，输出世界坐标系到该帧相机坐标系的变换矩阵
im	输入图像
timestamp	时间戳

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)

{
    mImGray = im;//读取图像
    // 步骤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);
    }
    // 步骤2：构造Frame
    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    // 步骤3：跟踪
    Track();
    return mCurrentFrame.mTcw.clone();
}

数据，流到Track（），由于代码超长，我会分几段粘贴注释。 

void Tracking::Track()

步骤

1. 判断tracking状态：如果是未初始化（NOT_INITIALIZED），则对单目和非单目分别执行MonocularInitialization()、StereoInitialization()进行初始化，并更新地图视图。 2.对于初始化成功的，接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking判断） （1）只进行跟踪不建图 （2）同时跟踪和建图： 初始化之后ORB-SLAM有三种跟踪模型可供选择 a.TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿——根据匀速运动模型对上一帧的地图点进行跟踪——优化位姿。 b.TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点——将上一帧的位姿作为当前帧的初始值——通过优化3D-2D的重投影误差来获得位姿。 c.Relocalization()；重定位模型：计算当前帧的BoW——检测满足重定位条件的候选帧——通过BoW搜索当前帧与候选帧的匹配点——大于15个点就进行PnP位姿估计——优化。 这三个模型的选择方法： 首先假设相机恒速（即Rt和上一帧相同），然后计算匹配点数（如果匹配足够多则认为跟踪成功），如果匹配点数目较少，说明恒速模型失效，则选择参考帧模型（即特征匹配，PnP求解），如果参考帧模型同样不能进行跟踪，说明两帧键没有相关性，这时需要进行重定位，即和已经产生的关键帧中进行匹配（看看是否到了之前已经到过的地方）确定相机位姿，如果重定位仍然不能成功，则说明跟踪彻底丢失，要么等待相机回转，要不进行重置。

无参数

 A.初始化部分

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);
    // 步骤1：初始化
    if(mState==NOT_INITIALIZED)//判断是否初始化
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)//双目或深度相机
            StereoInitialization();//双目初始化
        else
            MonocularInitialization();//单目初始化
        mpFrameDrawer->Update(this);
        if(mState!=OK)
            return;
    }

//。。。。。。。

B.跟踪步骤1.跟踪上一帧或者参考帧或者重定位

else// 步骤2：跟踪
    {
        // System is initialized. Track Frame.系统完成初始化，跟踪帧
        // bOK为临时变量，用于表示每个函数是否执行成功
        bool bOK;
        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)运用运动模型或重定位初始化相机位姿估计
        // 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
        // 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
                // 检查并更新上一帧被替换的MapPoints
                // 更新Fuse函数和SearchAndFuse函数替换的MapPoints
                CheckReplacedInLastFrame();
                // 步骤2.1：跟踪上一帧或者参考帧或者重定位
                // 运动模型是空的或刚完成重定位
                // mCurrentFrame.mnId<mnLastRelocFrameId+2表示刚重定位少于两帧
                // 应该只要mVelocity不为空，就优先选择TrackWithMotionModel
                // mnLastRelocFrameId上一次重定位的那一帧
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
                {
                    // 将上一帧的位姿作为当前帧的初始位姿
                    // 通过BoW的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点都对应3D点重投影误差即可得到位姿
                    bOK = TrackReferenceKeyFrame();//跟踪参考帧
                }
                else
                {
                    // 根据恒速模型设定当前帧的初始位姿
                    // 通过投影的方式在参考帧中找当前帧特征点的匹配点
                    // 优化每个特征点所对应3D点的投影误差即可得到位姿
                    bOK = TrackWithMotionModel();//根据固定运动速度模型预测当前帧的位姿
                    if(!bOK)
                        // TrackReferenceKeyFrame是跟踪参考帧，不能根据固定运动速度模型预测当前帧的位姿态，通过bow加速匹配（SearchByBow）
                        // 最后通过优化得到优化后的位姿
                        bOK = TrackReferenceKeyFrame();
                }
            }
            else
            {
                // BOW搜索，PnP求解位姿
                bOK = Relocalization()//重定位成功与否
            }
        }
        else
        {
            // Localization Mode: Local Mapping is deactivated
            // 只进行跟踪tracking，局部地图不工作
            // 步骤2.1：跟踪上一帧或者参考帧或者重定位
            // 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
                    // mbVO为0则表明此帧匹配了很多的3D 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.
                    // mbVO为1，则表明此帧匹配了很少的3D map点，少于10个，要跪的节奏，既做跟踪又做定位
                    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;
                        if(mbVO)
                        {
                            // 更新当前帧的MapPoints被观测程度
                            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;
               }
            }
        }

跟踪步骤2.在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿，local map:当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系，在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化。

步骤3：更新恒速运动模型TrackWithMotionModel中的mVelocity
 

  // 将最新的关键帧作为reference frame，接上面代码
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
 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())
            {
                // 步骤2.3：更新恒速运动模型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; // 其实就是Tcl
            } 
            else
                mVelocity = cv::Mat();
            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);

 步骤4.清除UpdateLastFrame中为当前帧临时添加的MapPoints

5.清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
6.检测并插入关键帧，对于双目会产生新的MapPoints

 // 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
            for(int i=0; i<mCurrentFrame.N; i++)//遍历当前帧所有MapPoint
            {
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(pMP)
                    // 排除UpdateLastFrame函数中为了跟踪增加的MapPoints
                    if(pMP->Observations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                    }
            }

            // Delete temporal MapPoints
            // 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
            // 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
            // 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
            for(list<MapPoint*>::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
           // 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
            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.
            // 删除那些在bundle adjustment中检测为outlier的3D map点
            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
        // 跟踪失败，并且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);
    }

C.记录位姿信息，用于轨迹复现

if(!mCurrentFrame.mTcw.empty())
    {
        // 计算相对姿态T_currentFrame_referenceKeyFrame
        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);
    }
}

未完，待续。。。