ORB-SLAM （四）tracking跟踪解析

　　初始化完成后，对于相机获取当前图像mCurrentFrame，通过跟踪匹配上一帧mLastFrame特征点的方式，可以获取一个相机位姿的初始值；为了兼顾计算量和跟踪鲁棒性，处理了三种模型：

　　1. TrackWithMotionModel

　　2. TrackReferenceKeyFrame

　　3. Relocalization

　　这三种跟踪模型都是为了获取相机位姿一个粗略的初值，后面会通过跟踪局部地图TrackLocalMap对位姿进行BundleAdjustment（捆集调整），进一步优化位姿。

　　优先选择通过恒速运动模型，从LastFrame直接预测出（乘以一个固定的位姿变换矩阵）当前帧的姿态；如果是静止状态或者运动模型匹配失效（运用恒速模型后反投影发现LastFrame的地图点和CurrentFrame的特征点匹配很少），通过增大参考帧的地图点反投影匹配范围，获取较多匹配后，计算当前位姿；若这两者均失败，即代表tracking失败，mState!=OK，则在KeyFrameDataBase中用Bow搜索CurrentFrame的特征点匹配，进行全局重定位GlobalRelocalization，在RANSAC框架下使用EPnP求解当前位姿。这三种跟踪模式仅仅跟踪了一帧图像中的特征，没有全局的信息，因此获取的位姿误差较大。

　　

　　一旦我们通过上面三种模型获取了初始的相机位姿和初始的特征匹配，就可以将完整的地图投影到当前帧中去搜索更多的匹配。但是投影完整的地图，在large scale的场景中是很耗计算而且也没有必要的，因此，这里使用了局部地图LocalMap来进行投影匹配。

　　LocalMap包含：与当前帧相连的关键帧K1，以及与K1相连的关键帧K2（一级二级相连关键帧）；K1、K2对应的地图点；参考关键帧Kf。

　　匹配过程如下：

　　1. 抛弃投影范围超出相机画面的；

　　2. 抛弃观测视角和地图点平均观测方向相差60^o以上的；

　　3. 抛弃特征点的尺度和地图点的尺度（通过高斯金字塔层数表示）不匹配的；

　　4. 计算当前帧中特征点的尺度；

　　5. 将地图点的描述子和当前帧ORB特征的描述子匹配，需要根据地图点尺度在初始位姿获取的粗略x投影位置附近搜索；

　　6. 根据所有匹配点进行PoseOptimization优化。

　　

　　在处理完mCurrentFrame的跟踪定位后，需要判定当前帧是否可以加入关键帧list：

　　以下条件必须同时满足，才可以加入关键帧，但是其实ORB-SLAM中关键帧的加入是比较密集的，这样确保了定位的精度，同时在LocalMapping线程最后会进行关键帧的剔除，又确保了关键帧的数量不会无限增加，不会对large scale的场景造成计算负担。但是，ORB的作者又提到了，Tracking中除了提取特征点，TrackLocalMap也挺耗时，可以通过减少关键帧的数量来降低Local Map的规模，提高Tracking速度（但是精确度可能降低）。

　　1. 距离上一次重定位距离至少20帧；

　　2. 局部地图线程空闲，或者距离上一次加入关键帧过去了20帧；如果需要关键帧插入（过了20帧）而LocalMapping线程忙，则发送信号给LocalMapping线程，停止局部地图优化，使得新的关键帧可以被及时处理（20帧，大概过去了不到1s）。

　　3. 当前帧跟踪至少50个点；确保了跟踪定位的精确度

　　4. 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%；确保关键帧之间有明显的视觉变化。

　　注意这里只是判断当前帧是否需要加入关键帧，并没有真的加入地图，因为Tracking线程的主要功能是局部定位，而处理地图中的关键帧，地图点，包括如何加入，如何删除的工作是在LocalMapping线程完成的，这里也可以看出作者的思路是比较清楚的，Tracking负责localization，LocalMapping负责Mapping，就构建了粗略的完整SLAM框架，然后加入初始化和闭环检测以及一些可视化模块，形成完整的SLAM。

　　PTAM中关键帧的插入仅仅依靠关键帧之间的距离，明显逊于ORB-SLAM的适者生存方法（survival of the fittest）。

---

对于RGBD和双目，非关键帧均可以恢复出图像点的深度,，可以创建一些临时地图点，用于普通帧之间跟踪，主要用于恒速运动模型，这些临时地图点仅仅用于MotionModel跟踪，不会加入地图或者关键帧中。运动过程中不断会跟丢地图点，因此需要不断补充。

void Tracking::UpdateLastFrame();

然后根据恒速模型估计当前帧位姿

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

使用匀速模型估计的位姿，将LastFrame中临时地图点投影到当前姿态，在投影点附近根据描述子距离进行匹配（需要>20对匹配，否则匀速模型跟踪失败，运动变化太大时会出现这种情况），然后以运动模型预测的位姿为初值，优化当前位姿，优化完成后再剔除外点，若剩余的匹配依然>=10对，则跟踪成功，否则跟踪失败，需要Relocalization：

bool Tracking::TrackWithMotionModel()
{
    ORBmatcher matcher(0.9,true);

    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points if in Localization Mode
    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);

    // If few matches, uses a wider window search
    if(nmatches<)
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,*th,mSensor==System::MONOCULAR);
    }

    if(nmatches<)
        return false;

    // Optimize frame pose with all matches
    Optimizer::PoseOptimization(&mCurrentFrame);

    // Discard outliers
    int nmatchesMap = ;
    for(int i =; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            if(mCurrentFrame.mvbOutlier[i])
            {
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];

                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>)
                nmatchesMap++;
        }
    }    

    if(mbOnlyTracking)
    {
        mbVO = nmatchesMap<;
        return nmatches>;
    }

    return nmatchesMap>=;
}

以上两种仅仅完成了视觉里程计中的帧间跟踪，还需要进行局部地图的跟踪，提高精度：（这其实是Local Mapping线程中干的事情）

bool Tracking::TrackLocalMap()

其中会完成局部地图的更新，以及局部地图点的更新：

UpdateLocalMap();

SearchLocalPoints();

BA优化后，统计符合局部先验信息的mnMatchesInliers内点数，判断是否成功，如果最近刚刚做过Relocalization，则要求的内点数多一些。

重定位Relocalization的过程大概是这样的：

1. 计算当前帧的BoW映射；

2. 在关键帧数据库中找到相似的候选关键帧；

3. 通过BoW匹配当前帧和每一个候选关键帧，如果匹配数足够，进行EPnP求解；

4. 对求解结果使用BA优化，如果内点较少，则反投影当前帧的地图点到候选关键帧获取额外的匹配点；若这样依然不够，放弃该候选关键帧，若足够，则将通过反投影获取的额外地图点加入，再进行优化。

5. 如果内点满足要求(>50)则成功重定位，将最新重定位的id更新：mnLastRelocFrameId = mCurrentFrame.mnId;　　否则返回false。

这三种模式可以一定程度上保证短期的VO不会跟丢；下面还需要对局部地图进行跟踪，以及选择插入关键帧过程。

局部地图跟踪TrackLocalMap()中需要首先对局部地图进行更新(UpdateLocalMap)，并且搜索局部地图点(SearchLocalPoint)。局部地图的更新又分为局部地图点(UpdateLocalPoints)和局部关键帧(UpdateLocalKeyFrames)的更新：

bool Tracking::TrackLocalMap()
{
    // We have an estimation of the camera pose and some map points tracked in the frame.
    // We retrieve the local map and try to find matches to points in the local map.

    UpdateLocalMap();

    SearchLocalPoints();

    // Optimize Pose
    Optimizer::PoseOptimization(&mCurrentFrame);
    mnMatchesInliers = ;

    // Update MapPoints Statistics
    for(int i=; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            if(!mCurrentFrame.mvbOutlier[i])
            {
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                if(!mbOnlyTracking)
                {
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>)
                        mnMatchesInliers++;
                }
                else
                    mnMatchesInliers++;
            }
            else if(mSensor==System::STEREO)
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);

        }
    }

    // Decide if the tracking was succesful
    // More restrictive if there was a relocalization recently
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<)
        return false;

    if(mnMatchesInliers<)
        return false;
    else
        return true;
}

局部地图点的更新比较容易，完全根据局部关键帧来，所有局部关键帧的地图点就构成局部地图点；因此，关键在于如何去选择当前帧对应的局部关键帧。

第一部分是所有能观察到当前帧对应地图点的的关键帧；

当关键帧数量较少时(<=80)，考虑加入第二部分关键帧，是与第一部分关键帧联系紧密的关键帧，并且始终限制关键数量不超过80。联系紧密体现在三类：1. 共视化程度高的关键帧；2. 子关键帧；3. 父关键帧。

还有一个关键的问题是：如何判断该帧是否关键帧，以及如何将该帧转换成关键帧？调用NeedNewKeyFrame()和CreateNewKeyFrame()两个函数来完成。

再推荐一个Tracking写的比较精炼的博客