ORB SLAM2学习笔记之mono_kitti（五）

Track函数架构

Track函数流程图如下所示，忽略跟丢的情况的话，主要分为初始化、跟踪参考帧解算位姿，运动模型解算位姿等。
先用两帧初始化，接下来再读取一帧进行位姿计算，由于这是恒速模型还没有建立，所以使用函数 TrackReferenceKeyFrame进行两帧间的跟踪，然后进行 TrackLocalMap ，目的是使用对 local map 进行跟踪得到更多的匹配，并优化当前位姿，然后更新恒速模型 mVelocity，接下来再读取一帧使用 TrackWithMotionModel进行两帧间的跟踪，再像之前一样进行 TrackLocalMap，再更新 mVelocity，然后循环往复…

代码细节过多，不能记录的太详细，挑一些关键性的记录一下：

TrackReferenceKeyFrame：跟踪上一关键帧

第一步：将当前帧的描述子转化为BoW向量

mCurrentFrame.ComputeBoW();

第二步：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配（特征点的匹配关系由MapPoints进行维护）

ORBmatcher matcher(0.7,true);
vector vpMapPointMatches;
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

第三步：对mCurrentFrame能看到的mappoints进行赋值（赋的值为参考帧中对应的mappoints），将上一帧的位姿态作为当前帧位姿的初始值然后进行3D-2D的重投影优化位姿

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
Optimizer::PoseOptimization(&mCurrentFrame);

第四步：最后，剔除优化后的outlier匹配点（MapPoints），在n个点中，首先判断mappoints中有没有这个点，然后判断这个点是不是外点

TrackWithMotionModel：恒速跟踪模型

第一步：根据恒速模型设置一个初始pose（这只是个初始值，不准，后面还要优化）

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

第二步：对上一帧的MapPoints进行跟踪，根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围（根据第一步计算的pose先投影下来）

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

第三步：g2o优化位姿

Optimizer::PoseOptimization(&mCurrentFrame);

第四步：优化位姿后剔除outlier的mvpMapPoints（注意，这里如果当前帧中观测到的mappoints少于一定数量的时候那么这时跟踪失败，会使用TrackReferenceKeyFrame跟踪上一关键帧计算pose）

---

～ 接下来，重中之重：

我之所以认为他很重要的原因是因为，这个部分是一个纠错的过程，高博十四讲上有写，如果只是两帧两帧之间的跟踪很容易一错再错，最后累计很大误差，但如果加入个局部地图，用地图来维护mappoints的话，即使跟踪前一帧时有误差，还是可以通过这个部分校正好位姿的（因为这个地图包括了更前面的信息，优化位姿时能利用前面正确的信息进行优化）

---

TrackLocalMap：维护局部地图

第一步：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

UpdateLocalMap();

第二步：在局部地图中查找与当前帧匹配的mappoints

SearchLocalPoints();

第三步：更新局部所有MapPoints后对位姿再次优化位姿（之前 TrackReferenceKeyFrame 已经优化过一次）

Optimizer::PoseOptimization(&mCurrentFrame);

第四步：更新当前帧的mappoints被观测程度，并统计跟踪局部地图的效果

步骤较粗略，现在来解释一下前两步：

UpdateLocalMap

字面意思，更新局部地图。分为两步：

①更新局部关键帧和局部mappoints

UpdateLocalKeyFrames();
UpdateLocalPoints();

先来看UpdateLocalKeyFrames：

• 一、遍历mappoints，先统计能看到当前帧mappoints的各个关键帧

    map<KeyFrame*,int> keyframeCounter;
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
            if(!pMP->isBad())
            {
                // 能观测到当前帧MapPoints的关键帧
                const map<KeyFrame*,size_t> observations = pMP->GetObservations();
                for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
                    keyframeCounter[it->first]++;
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }

    if(keyframeCounter.empty())
        return;

keyframeCounter这个变量统计的就是哪个关键帧能看到几个当前帧的mappoints。

• 二、更新局部关键帧，更新的变量是mvpLocalKeyFrames，其中有两个方法增加关键帧，不细看了…

再来看 UpdateLocalPoints：

（遍历局部关键帧mvpLocalKeyFrames）

• 一`、通过 GetMapPointMatches 函数提取帧里的mappoints
• 二、 将局部关键帧的mappoints添加到mvpLocalMapPoints

    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        KeyFrame* pKF = *itKF;
        const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();

        
        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
        {
            MapPoint* pMP = *itMP;
            if(!pMP)
                continue;
            // mnTrackReferenceForFrame防止重复添加局部MapPoint
            if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
                continue;
            if(!pMP->isBad())
            {
                mvpLocalMapPoints.push_back(pMP);
                pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
            }
        }
    }

②设置参考mappoints

mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

将我们提取出的mvpLocalMapPoints赋值给mvpReferenceMapPoints。

SearchLocalPoints

第一步：遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索（当前的mvpMapPoints一定在当前帧的视野中）

    for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
        if(pMP)
        {
            if(pMP->isBad())
            {
                *vit = static_cast<MapPoint*>(NULL);
            }
            else
            {
                // 更新能观测到该点的帧数加1
                pMP->IncreaseVisible();
                // 标记该点被当前帧观测到
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                // 标记该点将来不被投影，因为已经匹配过
                pMP->mbTrackInView = false;
            }
        }
    }

第二步：将所有局部mappoints投影到当前帧，判断是否在视野范围内，然后进行投影匹配。

    for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;

        // 已经被当前帧观测到MapPoint不再判断是否能被当前帧观测到
        if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
            continue;
        if(pMP->isBad())
            continue;
        
        // Project (this fills MapPoint variables for matching)
        //判断LocalMapPoints中的点是否在在视野内
        if(mCurrentFrame.isInFrustum(pMP,0.5))
        {
        	// 观测到该点的帧数加1，该MapPoint在某些帧的视野范围内
            pMP->IncreaseVisible();
            // 只有在视野范围内的MapPoints才参与之后的投影匹配
            nToMatch++;
        }
    }

    if(nToMatch>0)
    {
        ORBmatcher matcher(0.8);
        int th = 1;
        if(mSensor==System::RGBD)
            th=3;

        // If the camera has been relocalised recently, perform a coarser search
        // 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大
        if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
            th=5;

        // 对视野范围内的MapPoints通过投影进行特征点匹配
        matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
    }

• 两点要说：
  (1) 这里面有个函数 isInFrustum ，蛮重要的，内部进行了重投影坐标的计算，略记录一下：①先得到mappoints的世界坐标，②然后根据pose计算像相机坐标系下的坐标，③根据内参计算像素坐标然后判断是否在边界之内，④计算mappoints到相机光心的距离然后判断是否在尺度变化的距离内，⑤计算当前视角和平均视角夹角的余弦值，⑥根据mappoints的深度预测其 尺度（金字塔第几层），函数请见 PredictScale。⑦以上条件满足，记录该mappoint将来要被投影。
  (2) SearchByProjection 函数对视野范围内的mappoints通过投影进行特征点匹配，其通过定义一个半径和之前预测的尺度（level）来进行搜索寻找兴趣点，找到后把相应的特征点索引和mappoints配对。

---

TrackLocalMap 过程进行完会更新mState，更新mpFrameDrawer，将最新的关键帧作为参考帧（reference frame）等等收尾工作，可以参考本篇博客最前面说的。
现在有个疑问，就是一直在维护局部地图，一直在更新当前帧中的mappoints，但从没有增加过新的mappoints，那么岂不是再剔除外点以后越来越少呢？原来添加新的mappoints的过程在 LocalMapping 线程里，那么先忽略重定位的过程，接下来进入 LocalMapping ！