ORB-SLAM2初步（Tracking.cpp）

　　今天主要是分析一下Tracking.cpp这个文件，它是实现跟踪过程的主要文件，这里主要针对单目，并且只是截取了部分代码片段。

一、跟踪过程分析

首先构造函数中使用初始化列表对跟踪状态mState(NO_IMAGES_YET), 传感器类型mSensor(sensor), 是否只进行定位mbOnlyTracking(false)等变量进行了初始化（注意：一些const关键字或者指针类的变量只能使用初始化列表进行初始化），同时在构造函数的函数体内通过OpenCV的FileStorage从文件中读取了Camera标定参数，ORB特征提取的相关参数等，并用其对相关变量进行了初始化。（由于这一段比较简单，很容易看懂，就不再赘述。）

Tracking.cpp文件的调用接口函数是：

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

　　它完成了对一帧的初始化，并转入Track过程：

if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)

        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    else

        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    Track();

Track（）实现了跟踪的主要逻辑过程：（1）第一步首先就是判断是否进行了初始化，关于初始化先暂且不表，只知道它通过调用一个初始化函数进行了初始化：

if(mState==NOT_INITIALIZED)

    {

        //根据双目或RGBD或单目分别进行初始化，调用不同的函数；

        if(mSensor==System::STEREO || mSensor==System::RGBD)

            StereoInitialization();

        else

            MonocularInitialization();

        mpFrameDrawer->Update(this);

        //如何初始化没有完成，需要返回重新进入线程进行初始化，成功了继续执行；

        if(mState!=OK)

            return;

    }

（2）接下来也是判断，这里是同时跟踪和建图的跟踪过程，其实只有跟踪的时候也很有意思：

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

                //判定速度是否为空，是则根据参考帧进行跟踪，否则根据运动模型进行跟踪，或者距离上一次重定位过去少于2帧；

                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+)

                {

                    bOK = TrackReferenceKeyFrame();

                }

                else

                {

                    bOK = TrackWithMotionModel();

                    //如果运动模型跟踪失败，使用参考帧模型进行跟踪；

                    if(!bOK)

                        bOK = TrackReferenceKeyFrame();

                }

            }

            //如果跟踪状态为丢失，则使用重定位找回当前相机的位姿；

            else

            {

                bOK = Relocalization();

            }

        }

（3）到这里三种跟踪模型都已经出现了，它们分别是：运动模型、参考帧模型、重定位，这里先暂时跳过，后面单独分析这三种模型

TrackWithMotionModel();

TrackReferenceKeyFrame();

Relocalization()

从上面的过程可以看出，如果初始化成功或上一帧的状态为成功(mState==OK)，同时速度矩阵非空（mVelocity.empty()），则优先使用运动模型进行跟踪（从后面的分析可以看出这种跟踪方式速度最快），否则使用参考帧模型进行跟踪，如果上一帧跟踪状态为失败，就需要直接进行重定位找回相机位姿。

假设不管使用哪种方法，跟踪状态显示成功了，同时返回了一个初始的相机位姿，下面就是要进行局部地图的跟踪过程，可以理解为三种模型获取一个初始相机位姿，然后使用跟踪局部地图的方式对位姿进行优化：
```
bOK = TrackLocalMap();
```
我认为只要前面三种模型跟踪成功了，对局部地图的跟踪就会成功，所以这里bOK的状态不会改变（没有考虑其它特殊情况），其结果相当于获得了一个相对精确的相机位姿。

下面就是一些扫尾工作，如果不知道运动模型是什么一种跟踪模型，那对前面的速度矩阵肯定很感兴趣了：

if(!mLastFrame.mTcw.empty())

            {

                cv::Mat LastTwc = cv::Mat::eye(,,CV_32F);

                mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(,).colRange(,));

                mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(,).col());

                //这里的速度矩阵存储的具体内容是当前帧的位姿乘以上一帧的位姿；

                mVelocity = mCurrentFrame.mTcw*LastTwc;

            }

            else

                //把速度矩阵设置为空。

                mVelocity = cv::Mat();

结合后面的运动模型可以得知，它假设的是相机运动速度不变（就是假设下一帧的位姿矩阵和这一帧一样，但是相机的位置是不一样的）：

//更新当前帧的位姿：速度乘以上一帧的位姿；

    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

其它的工作包括判断是否插入关键帧，删除一些外点，把当前帧置位上一帧等，如果为跟丢，且关键帧总数小于5（初始化不久就丢了），则需要进行重置。

二、子函数分析

首先是运动模型：

//设置匹配器；
ORBmatcher matcher(0.9,true);
//更新上一帧信息，对单目只更新了相机位姿；

UpdateLastFrame();

//更新当前帧的位姿：速度乘以上一帧的位姿；

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));

// Project points seen in previous frame

int th;

if(mSensor!=System::STEREO)

    th=;

else

    th=;

int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);


//如果匹配数小于20，则扩大搜索范围；
if(nmatches<)

{

    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));

    nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,*th,mSensor==System::MONOCULAR);

}

//如果还是匹配数小于20，则判定运动模型跟踪失败；

if(nmatches<)

    return false;

//如果匹配数大于20了，就优化相机位姿；

// Optimize frame pose with all matches

Optimizer::PoseOptimization(&mCurrentFrame);

后面就是根据不同情况对跟踪结果进行返回，还有当前帧特征中的地图点的判定等。

参考帧模型：

//计算当前帧的Bow向量

mCurrentFrame.ComputeBoW();

//设定匹配器；

ORBmatcher matcher(0.7,true);

vector<MapPoint*> vpMapPointMatches;

//统计当前帧和参考关键帧之间匹配点数，使用BoW加速匹配过程；

int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
//这里将上一帧的位姿赋给了当前帧
mCurrentFrame.SetPose(mLastFrame.mTcw);

//优化当前位姿；
Optimizer::PoseOptimization(&mCurrentFrame);

后面的过程跟运动模型类似。但是这里直接将上一帧位姿作为初值进行优化，并没有使用PnP求解，保留疑问。

重定位：

//计算BoW向量；

    mCurrentFrame.ComputeBoW();

    //在关键帧数据库中搜索当前帧的候选关键帧；

    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

    if(vpCandidateKFs.empty())

        return false;

    const int nKFs = vpCandidateKFs.size();

    //设置匹配器；

    ORBmatcher matcher(0.75,true);

    vector<PnPsolver*> vpPnPsolvers;

    vpPnPsolvers.resize(nKFs);

    vector<vector<MapPoint*> > vvpMapPointMatches;

    vvpMapPointMatches.resize(nKFs);

    vector<bool> vbDiscarded;

    vbDiscarded.resize(nKFs);

    int nCandidates=;

    //对每一个关键帧进行PnP求解；

    //首先进行BoW匹配，匹配达到15个点的就进行进一步求解，并作为候选关键帧；

    for(int i=; i<nKFs; i++)

    {

        KeyFrame* pKF = vpCandidateKFs[i];

        if(pKF->isBad())

            vbDiscarded[i] = true;

        else

        {

            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);

            if(nmatches<)

            {

                vbDiscarded[i] = true;

                continue;

            }

            else

            {

                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);

                pSolver->SetRansacParameters(0.99,,,,0.5,5.991);

                vpPnPsolvers[i] = pSolver;

                nCandidates++;

            }

        }

    }

    //再后面一个大循环是上面的匹配结果进行优化求精，进行RANSAC迭代，如果有足够多的内点则跳出循环并返回

重定位的过程至少在逻辑上是很好理解的，就是在已经生成的关键帧数据库中搜索看看当前帧和谁最相近，而方法就是先用BoW匹配，然后进行PnP求解，最后使用RANSAC迭代。

还有一个重要的函数，就是局部地图跟踪：

//更新局部地图，包括关键帧和地图点的更新；

    //它的主要作用就是通过关键帧和地图点的共视关系更新这两个变量：mvpLocalKeyFrames，mvpLocalMapPoints，它们中存储着局部地图中的关键帧和地图点；

    UpdateLocalMap();

    //这个函数就是搜索一下mvpLocalMapPoints中的点是否符合跟踪要求，并匹配当前帧和局部地图点；

    SearchLocalPoints();

    //然后就是优化位姿，需要注意的是，无论是匹配过程还是优化过程都会对地图点做一些修改

    Optimizer::PoseOptimization(&mCurrentFrame);

    //后面就是根据匹配和优化结果更新地图点的状态，并判断匹配的内点数量，最后返回

实际上局部地图匹配的过程要比上面几行代码复杂的多，基本思想就是通过共视关系找出局部地图的关键帧和局部地图点，并用当前帧与之进行匹配优化。

还有一些子函数，如判断是否插入关键帧，跟踪的重置，初始化过程，以及只进行跟踪不建图的跟踪过程等，以后有机会再说吧。

三、总结

　　通过梳理跟踪过程的代码，对ORB-SLAM的跟踪过程的算法和代码都有了更深入的理解，也体会到了写一个类似工程的知识量和工作量。

从前面的分析知道跟踪过程的时间消耗主要在局部地图的跟踪过程，提高效率的方法之一应该就是相应的减小局部地图的大小，具体来说就是mvpLocalKeyFrames，mvpLocalMapPoints这两个变量的大小，现在还没遇到这个问题，所以没有做相应实验。

巴特西

ORB-SLAM2初步（Tracking.cpp）

最新文章

热门文章