ORB_SLAM2原理及代码解析:Tracking 线程——Tracking::Track() - 实践

目录

1 作用

2 参数及含义

3 运行逻辑

4 代码

5 解析

5.1 初始化检查

5.2 获取地图互斥锁

5.3 未初始化则初始化

5.3.1 StereoInitialization() 函数

5.3.2 MonocularInitialization() 函数

5.3.3 mState含义

5.4 已初始化则位姿跟踪

5.4.1 运动模型+关键帧跟踪

5.4.1.1 mbOnlyTracking含义

5.4.1.2 CheckReplacedInLastFrame() 函数

5.4.1.3 TrackReferenceKeyFrame() 函数

5.4.1.4 TrackWithMotionModel() 函数

5.4.1.5 Relocalization() 函数

5.4.2 仅定位

5.5 局部地图跟踪

5.5.1 TrackLocalMap() 函数

5.6 状态更新与关键帧管理

5.6.1 更新运动模型

5.6.2 清理 VO 匹配点和临时 MapPoints

5.6.3 判断是否插入关键帧

5.6.3.1 NeedNewKeyFrame() 函数

5.6.3.2 CreateNewKeyFrame() 函数

5.6.4 删除当前帧中异常MapPoints

5.7 跟踪丢失

5.8 记录帧位置信息

1 作用

（1）初始化检查与单目/双目/RGB-D初始化；

（2）利用运动模型或参考关键帧进行相机位姿估计；

（3）重定位处理（Tracking 丢失时）；

（4）局部地图跟踪；

（5）关键帧插入判断与更新。

2 参数及含义

3 运行逻辑

4 代码

void Tracking::Track()
{
    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }
    mLastProcessedState=mState;
    // Get Map Mutex -> Map cannot be changed
    unique_lock lock(mpMap->mMutexMapUpdate);
    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();
        mpFrameDrawer->Update(this);
        if(mState!=OK)
            return;
    }
    else
    {
        // System is initialized. Track Frame.
        bool bOK;
        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        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
                CheckReplacedInLastFrame();
                if(mVelocity.empty() || mCurrentFrame.mnId vpMPsMM;
                    vector 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)
                        {
                            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.
        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())
            {
                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
            for(int i=0; iObservations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
                    }
            }
            // Delete temporal MapPoints
            for(list::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
            mlpTemporalPoints.clear();
            // Check if we need to insert a new keyframe
            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.
            for(int i=0; i(NULL);
            }
        }
        // Reset if the camera get lost soon after initialization
        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.
    if(!mCurrentFrame.mTcw.empty())
    {
        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);
    }
}

5 解析

5.1 初始化检查

    if(mState==NO_IMAGES_YET)
    {
        mState = NOT_INITIALIZED;
    }
    mLastProcessedState=mState;

5.2 获取地图互斥锁

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

运行到这一步时地图点不可更新，等这一程序块运行结束自动解锁。

5.3 未初始化则初始化

    if(mState==NOT_INITIALIZED)
    {
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();
        mpFrameDrawer->Update(this);
        //退出当前帧
        if(mState!=OK)
            return;
    }

5.3.1 StereoInitialization() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152281400?spm=1011.2415.3001.5331

5.3.2 MonocularInitialization() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152448072?spm=1011.2415.3001.5331

5.3.3 mState含义

5.4 已初始化则位姿跟踪

    else
    {
        // System is initialized. Track Frame.
        bool bOK;
        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        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
                CheckReplacedInLastFrame();
                if(mVelocity.empty() || mCurrentFrame.mnId vpMPsMM;
                    vector 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)
                        {
                            for(int i =0; iIncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)
                    {
                        mbVO = false;
                    }
                    bOK = bOKReloc || bOKMM;
                }
            }
        }

5.4.1 运动模型+关键帧跟踪

       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
                CheckReplacedInLastFrame();
                //如果运动模型为空或当前帧的ID小于重定位帧+2帧
                if(mVelocity.empty() || mCurrentFrame.mnId

PS.正常SLAM模式(mState==OK)优先运动模型追踪，失败用关键帧匹配，彻底丢失就重定位。

5.4.1.1 mbOnlyTracking含义

5.4.1.2 CheckReplacedInLastFrame() 函数

（1）作用

         Tracking线程每次处理新帧时，会先检查“上一帧”中使用的地图点是否被替换掉。

原因：

a.在LocalMapping（局部建图）线程中，可能进行了地图点融合，即两个相同位置的点被合并成一个新的MapPoint；

b.那么上一帧中引用的旧点就“过期”了；

c.Tracking需要更新引用，否则下一帧追踪就会错乱。

PS.地图点融合在局部建图线程中完成，此处不做过多介绍。

（2）声明：Tracking.h

    void CheckReplacedInLastFrame();

（3）定义：Tracking.cc

void Tracking::CheckReplacedInLastFrame()
{
    for(int i =0; iGetReplaced();
            if(pRep)
            {
                //更新为新的地图指帧
                mLastFrame.mvpMapPoints[i] = pRep;
            }
        }
    }
}

5.4.1.3 TrackReferenceKeyFrame() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152559357?sharetype=blogdetail&sharerId=152559357&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.4.1.4 TrackWithMotionModel() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152604661?spm=1011.2415.3001.5331

5.4.1.5 Relocalization() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152609824?sharetype=blogdetail&sharerId=152609824&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.4.2 仅定位

        else
        {
            // Localization Mode: Local Mapping is deactivated
            if(mState==LOST)
            {
                bOK = Relocalization();
            }
            else
            {
                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;
                    //是否成功通过重定位（PnP + BoW）追踪
                    bool bOKReloc = false;
                    //当前帧通过运动模型匹配到的 MapPoints
                    vector vpMPsMM;
                    //每个 MapPoint 是否被判定为外点
                    vector 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)
                        {
                            for(int i =0; iIncreaseFound();
                                }
                            }
                        }
                    }
                    else if(bOKReloc)
                    {
                        mbVO = false;
                    }
                    bOK = bOKReloc || bOKMM;
                }
            }
        }

仅定位分三种情况：

（1）跟踪丢失(mState==LOST) → 尝试重定位；

（2）还在跟踪，且不是纯VO模式 →运动模型追踪，不行就用关键帧追踪；

（3）VO 模式（mbVO == true）。

5.5 局部地图跟踪

       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();
        }

5.5.1 TrackLocalMap() 函数

详见：

https://blog.csdn.net/weixin_45728280/article/details/152666047?sharetype=blogdetail&sharerId=152666047&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.6 状态更新与关键帧管理

        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())
            {
                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
            for(int i=0; iObservations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
                    }
            }
            // Delete temporal MapPoints
            for(list::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
            mlpTemporalPoints.clear();
            // Check if we need to insert a new keyframe
            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.
            for(int i=0; i(NULL);
            }
        }

5.6.1 更新运动模型

           // Update motion model
            if(!mLastFrame.mTcw.empty())
            {
                cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
//上一帧旋转矩阵的逆Rwc放入LastTwc                   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();

5.6.2 清理 VO 匹配点和临时 MapPoints

            mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
            // Clean VO matches
            for(int i=0; iObservations()<1)
                    {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i]=static_cast(NULL);
                    }
            }
            // Delete temporal MapPoints
            for(list::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++)
            {
                MapPoint* pMP = *lit;
                delete pMP;
            }
            mlpTemporalPoints.clear();

5.6.3 判断是否插入关键帧

           // Check if we need to insert a new keyframe
            if(NeedNewKeyFrame())
                CreateNewKeyFrame();

5.6.3.1 NeedNewKeyFrame() 函数

详见1：

https://blog.csdn.net/weixin_45728280/article/details/152323101?spm=1011.2415.3001.5331

5.6.3.2 CreateNewKeyFrame() 函数

详见2：

https://blog.csdn.net/weixin_45728280/article/details/152323101?sharetype=blogdetail&sharerId=152323101&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.6.4 删除当前帧中异常MapPoints

            // 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.
            for(int i=0; i(NULL);
            }

清除外点。

5.7 跟踪丢失

        // Reset if the camera get lost soon after initialization
        if(mState==LOST)
        {
            //如果全局地图中关键帧<=5
            if(mpMap->KeyFramesInMap()<=5)
            {
                cout << "Track lost soon after initialisation, reseting..." << endl;
                //重启SLAM，清空地图、清空关键帧数据库、重置跟踪器状态
                mpSystem->Reset();
                return;
            }
        }
        if(!mCurrentFrame.mpReferenceKF)
            mCurrentFrame.mpReferenceKF = mpReferenceKF;
        mLastFrame = Frame(mCurrentFrame);

5.8 记录帧位置信息

    // Store frame pose information to retrieve the complete camera trajectory afterwards.
    if(!mCurrentFrame.mTcw.empty())
    {
        //当前帧相对于参考关键帧的相对位姿 = 当前帧的世界位姿(世界 → 当前帧)*参考关键帧的逆变换
        cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
        //当前帧相对参考关键帧的位姿矩阵
        mlRelativeFramePoses.push_back(Tcr);
        //当前帧对应的参考关键帧指针
        mlpReferences.push_back(mpReferenceKF);
        //当前帧的时间戳
        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
        //当前帧是否 tracking 丢失
        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);
    }