ORB-SLAM2源码逐行解析系列（十七）：ORB-SLAM2中的跟踪方法

1. 参考关键帧跟踪

（1）原理

​  参考关键帧跟踪是将当前普通帧（位姿未知）和它对应的参考关键帧（位姿已知）进行特征匹配及优化，从而估计当前普通帧的位姿。其中，可通过基于词袋的SearchByBoW来加速两帧之间的特征匹配。

（2）应用场景

• 地图刚刚初始化之后，此时恒速模型中的速度为空，只能使用参考关键帧，即使用初始化后的第1、2帧对当前帧进行跟踪
• 恒速模型跟踪失败后，尝试使用最近的参考关键帧跟踪当前普通帧。

（3）大致流程

• 通过基于词袋的方法，对当前普通帧和参考关键帧进行特征匹配，获取匹配上的地图点（此时，也属于当前普通帧的地图点）
• 通过优化3D-2D的重投影误差，获取当前普通帧的准确位姿，同时标记匹配点中的外点。其中，3D点通过匹配得到的，2D点来自于当前普通帧，优化时需要的当前普通帧位姿来自于上一帧位姿。
• 统计经过优化并剔除外点后的成功匹配点数，若超过10，则认为跟踪成功。

（4）代码实现

/*
 * @brief 用参考关键帧的地图点来对当前普通帧进行跟踪
 * 
 * Step 1：将当前普通帧的描述子转化为BoW向量，通过计算出的词袋将当前帧的特征点分配到特定层的nodes上
 * Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配: 对属于同一node的描述子进行匹配
 *         根据匹配特征点对来估计当前帧的位姿
 * Step 3: 将上一帧的位姿作为当前普通帧位姿的初始值
 * Step 4: 通过优化3D-2D的重投影误差来获得位姿
 * Step 5：根据位姿剔除优化后的匹配点中的外点
 * @return 如果匹配数超过10，返回true
 * 
 */
bool Tracking::TrackReferenceKeyFrame()
{
   
    // Step 1：将当前帧的描述子转化为BoW向量
    mCurrentFrame.ComputeBoW();
    ORBmatcher matcher(0.7,true);
    vector<MapPoint*> vpMapPointMatches;
    
    // Step 2：通过词袋BoW加速当前帧与参考帧之间的特征点匹配
    int nmatches = matcher.SearchByBoW(
        mpReferenceKF,          // 参考关键帧
        mCurrentFrame,          // 当前帧
        vpMapPointMatches);     // 存储的是匹配上的当前帧与参考帧的共同MapPoint
    // 若匹配地图点数目小于15，则认为跟踪失败
    if(nmatches<15)
        return false;
    
    // Step 3:将上一帧的位姿作为当前普通帧位姿的初始值，可以加速BA收敛
    mCurrentFrame.mvpMapPoints = vpMapPointMatches;  // 从上面的匹配中获取当前帧的地图点
    mCurrentFrame.SetPose(mLastFrame.mTcw); 
    
    // Step 4:通过优化3D-2D的重投影误差来获得位姿  仅对当前普通帧位姿进行优化，没有对3D点进行优化
    Optimizer::PoseOptimization(&mCurrentFrame);
    
    // Step 5：剔除优化后的匹配点中的外点，在优化过程中对这些外点做了标记
    int nmatchesMap = 0;  // 成功匹配的3D点数目(仅含内点)
    for(int i =0; 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()>0)
                // 累加成功匹配到的地图点数目
                nmatchesMap++;
        }
    }
    
    // 跟踪成功的地图点数目超过10才认为跟踪成功，否则认为跟踪失败
    return nmatchesMap>=10;
}

2. 恒速模型跟踪

（1）应用场景

​ 参考关键帧跟踪涉及到词袋匹配、非线性优化等过程，比较复杂，实时性难以实现。

​ 恒速模型跟踪是假设在相邻帧间极短的时间内，相机处于匀速运动状态，使用上一帧位姿以及通过参考关键帧跟踪获取到的速度来估计当前帧位姿。相比参考关键帧跟踪而言，恒速模型跟踪估计位姿更简单、更快，可实现SLAM的实时性要求。因此，在初始使用参考关键帧跟踪后，大部分时间使用恒速模型跟踪。

注意：这里说的速度是指上一帧到当前帧的位姿变化，并不是指实际中的物理速度。

（2）原理

​ 恒速模型中认为相邻帧间的速度（位姿变换）是不变的，即认为上一帧到当前最新帧之间的位姿变换和上上一帧到上一帧之间的位姿变换相同，那么当前帧的位姿就可以通过上一帧位姿以及速度（上一帧到当前帧的位姿变换）计算出来：
$$T_{cw}=V{T}_{lw}$$
​ 式中，$c$表示$current$，即当前普通帧；$l$表示$last$，即上一帧；$w$表示$world$，即世界坐标系。

​ 对于速度$V$，它在跟踪线程Track中是不断更新的，对应代码中的成员变量$mVelocity$，更新方程为：
$$V=T_{cl}=T_{cw}{T}_{wl}$$
​ 对于上一帧位姿$T_{lw}$​，在上一帧跟踪成功后可直接获取。但由于上一帧可能是普通帧，没有参与优化，对应的位姿不太准确，而关键帧会在局部建图线程中进一步优化。因此，代码中是通过上一帧对应的参考关键帧来获取上一帧的准确位姿，计算如下：
$$T_{lw}=T_{lr}{T}_{rw}$$
​ 式中，$r$表示$reference$，即参考关键帧。$T_{lr}$表示参考关键帧到上一帧的位姿变换，对应代码中的成员变量$mlRelativeFramePoses$，在跟踪线程Track中不断更新，$T_{r}w$表示参考关键帧的位姿。

（3）更新上一帧位姿UpdateLastFrame

​ 对于单目相机，只需要获取上一帧位姿即可。对于双目/RGB-D相机，还需要将深度值符合要求，且未创建为地图点的3D点创建为临时地图点，用以提高跟踪的鲁棒性。

大致流程

• 利用参考关键帧更新上一帧在世界坐标系下的位姿，具体实现如下：
  • 获取上一帧的参考关键帧pRef
  • 获取参考关键帧的位姿pRef->GetPose()
  • 从mlRelativaFramePoses中获取参考关键帧到上一帧的位姿变换Tlr
  • 通过公式获取上一帧的位姿
• 对于双目/RGB-D相机，还需要为上一帧生成临时地图点mlpTemporalPoints，其目的是用来增加跟踪的稳定性，在跟踪结束后会丢弃掉，具体实现如下：
  • 获取上一帧每个地图点对应的深度值，并将深度值按照从小到大的顺序进行排序
  • 遍历上一帧的地图点，若该地图点不存在，或没有被观测到，则将bCreateNew置为True
  • 在bCreateNew为True的前提下，将该3D点创建为临时地图点
  • 若遍历到的3D点深度值超过阈值，或遍历到的点个数超过100，则停止创建临时地图点。因为遍历前已经将深度值进行从小到大排序，若遍历到的点深度值过大或者遍历点数过多，说明此时的点已经较远，无需再考虑了。

代码实现

/**
 * @brief 更新上一帧位姿，在上一帧中生成临时地图点
 * 单目情况：只计算了上一帧在世界坐标系下的位姿
 * 双目和rgbd情况：选取有深度值的并且没有被选为地图点的点生成新的临时地图点，提高跟踪鲁棒性
 */
void Tracking::UpdateLastFrame()
{
   
    // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿
    KeyFrame* pRef = mLastFrame.mpReferenceKF;  // 获取上一帧的参考关键帧
    // ref_keyframe 到 lastframe的位姿变换  相对位姿
    cv::Mat Tlr = mlRelativeFramePoses.back();  // mlRelativeFramePoses是在Track中更新的？？

    // 将上一帧在世界坐标系下的位姿计算出来  Tlw = Tlr*Trw    l:last, r:reference, w:world
    // Trw:参考关键帧在世界坐标系下的位姿  Tlr:参考关键帧到上一帧的位姿 Tlw: 上一帧在世界坐标系下的位姿
    mLastFrame.SetPose(Tlr*pRef->GetPose()); 
    
    // 如果上一帧为关键帧，或者单目的情况，则退出  (已计算出上一帧的位姿)
    if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)
        return;
    
    // Step 2：对于双目或rgbd相机，因为其本身是有深度值的，所以可为上一帧生成新的临时地图点
    // Step 2.1：得到上一帧中具有有效深度值的特征点（不一定是地图点）
    vector<pair<float,int> > vDepthIdx;  // 存储的是特征点深度值及其索引的键值对
    vDepthIdx.reserve(mLastFrame.N);
    for(int i=0; i<mLastFrame.N;i++) 
    {
   
        float z = mLastFrame.mvDepth[i];  // 获取遍历到的上一帧中地图点的深度值
        if(z>0)  // 若深度值存在
        {
   
            // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id
            vDepthIdx.push_back(make_pair(z,i));
        }
    }
    // 如果上一帧中没有有效深度的点,那么就直接退出
    if(vDepthIdx.empty())
        return;
    
    // 按照深度从小到大排序
    sort(vDepthIdx.begin()