(01)ORB-SLAM2源码无死角解析-(36) 跟踪线程→跟踪丢失后，重定位跟踪 Relocalization()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196   文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证  

一、前言

前面两篇博客讲解了考关键帧追踪TrackReferenceKeyFrame()，以及恒速模型跟踪当前普通帧TrackWithMotionModel()。完成初始化之后，以及添加关键帧之后，紧接着使用关键追踪。通常情况下都是恒速模型跟踪，如果恒速模型追踪失败，则使用参考关键帧跟踪，如果参考关键帧跟踪也失败了则认为跟踪丢失。那么此时使用重定位跟踪 Relocalization()。这就是本章博客需要讲解的详细的内容。该函数的实现位于 src/Tracking.cc 文件中。  

二、代码流程

源码实现的流程主要如下所示(整体代码注释在后面)：

( 01 ) : \color{blue}{(01)}: (01): 计算当前帧特征点的词袋向量→核心代码mCurrentFrame.ComputeBoW();

( 02 ) : \color{blue}{(02)}: (02): 找到与当前帧相似的候选关键帧→核心代码mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

( 03 ) : \color{blue}{(03)}: (03): 通过BoW进行匹配→核心代码matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);

( 04 ) : \color{blue}{(04)}: (04): 通过EPnP算法估计姿态→核心代码pSolver->SetRansacParameters()

( 05 ) : \color{blue}{(05)}: (05): 通过PoseOptimization对姿态进行优化求解→核心代码Optimizer::PoseOptimization(&mCurrentFrame);

( 06 ) : \color{blue}{(06)}: (06): 如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解→核心代码matcher2.SearchByProjection()

从上面可以看出，Relocalization()中涉及重要函数还是挺多的，对于mCurrentFrame.ComputeBoW() 函数前面已经讲解过多次，所以就不进行重复，主要功能是将BRIEF描述子转BoW向量mBowVec(记录单词的id以及对应的权重) ，以及mFeatVec(node id及其对应的图像 feature对应的索引)  

三、DetectRelocalizationCandidates()

接着，来看看 DetectRelocalizationCandidates() 函数，其被调用的代码如下：

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    // Step 2：用词袋找到与当前帧相似的候选关键帧
    vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

其功能还是比较单一的，简单说: 通过词袋从所有关键帧中，找到 N 张与当前帧有联系的帧，称为候选帧。该函数中涉及到一个比较重要的变量 mvInvertedFile，如下图所示： 红色的矩形框表示的直接索引，以图像为单位，记录图像中包含的节点 id，以及节点的信息。绿色矩形框表示倒序索引，以节点为单位，记录该节点存在于那些图像之中，以及图像对应的信息。

mvInvertedFile使用的是倒序索引，如: mvInvertedFile[i]表示包含了第i个word id的所有关键帧。代码流程如下：

( 01 ) : \color{blue}{(01)}: (01): 对当前帧 F 的所有Bow向量(单词id以及对应的权重) 进行遍历，先获得单个Bow向量的 id，通过 mvInvertedFile 获得该 id 的所有关键帧。最终找出和当前帧具有公共单词的所有关键帧，存放于 lKFsSharingWords 变量之中。

( 02 ) : \color{blue}{(02)}: (02): 统计 lKFsSharingWords 中 键帧中与当前帧F具有共同单词最多的单词数 maxCommonWords，然后乘以 0.8 作为最小公共单词数的阈值一minCommonWords。

( 03 ) : \color{blue}{(03)}: (03): 遍历 lKFsSharingWords 中的关键帧，挑选出共有单词数大于阈值一 minCommonWords，及其和当前帧单词匹配得分存入lScoreAndMatch。两个 Bow 的相似度越高则得分越高。

( 04 ) : \color{blue}{(04)}: (04): 计算lScoreAndMatch中每个关键帧的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值二，单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧共视程度最高的前十个关键帧归为一组，计算累计得分。其中核心函数为 pKFi->GetBestCovisibilityKeyFrames(10); 记录所有组中最高的得分为 bestAccScore。

( 05 ) : \color{blue}{(05)}: (05): 以0.75f*bestAccScore为第二个阈值 minScoreToRetain，得到所有组中总得分大于阈值minScoreToRetain的，组内得分最高的关键帧，作为候选关键帧组 vpRelocCandidates。

通过以上流程的筛选即可得到 F 帧相似的候选关键帧组，其代码注释如下：

/*
 * @brief 在重定位中找到与该帧相似的候选关键帧组
 * Step 1. 找出和当前帧具有公共单词的所有关键帧
 * Step 2. 只和具有共同单词较多的关键帧进行相似度计算
 * Step 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
 * Step 4. 只返回累计得分较高的组中分数最高的关键帧
 * @param F 需要重定位的帧
 * @return  相似的候选关键帧数组
 */
vector KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
    list lKFsSharingWords;

    // Search all keyframes that share a word with current frame
    // Step 1：找出和当前帧具有公共单词(word)的所有关键帧
    {
        unique_lock lock(mMutex);

        // mBowVec 内部实际存储的是std::map
        // WordId 和 WordValue 表示Word在叶子中的id 和权重
        for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
        {
            // 根据倒排索引，提取所有包含该wordid的所有KeyFrame
            list &lKFs = mvInvertedFile[vit->first];

            for(list::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
            {
                KeyFrame* pKFi=*lit;
                // pKFi->mnRelocQuery起标记作用，是为了防止重复选取
                if(pKFi->mnRelocQuery!=F->mnId)
                {
                    // pKFi还没有标记为F的重定位候选帧
                    pKFi->mnRelocWords=0;
                    pKFi->mnRelocQuery=F->mnId;
                    lKFsSharingWords.push_back(pKFi);
                }
                pKFi->mnRelocWords++;
            }
        }
    }
    // 如果和当前帧具有公共单词的关键帧数目为0，无法进行重定位，返回空
    if(lKFsSharingWords.empty())
        return vector();

    // Only compare against those keyframes that share enough words
    // Step 2：统计上述关键帧中与当前帧F具有共同单词最多的单词数maxCommonWords，用来设定阈值1
    int maxCommonWords=0;
    for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        if((*lit)->mnRelocWords>maxCommonWords)
            maxCommonWords=(*lit)->mnRelocWords;
    }

    // 阈值1：最小公共单词数为最大公共单词数目的0.8倍
    int minCommonWords = maxCommonWords*0.8f;

    list lScoreAndMatch;

    int nscores=0;

    // Compute similarity score.
    // Step 3：遍历上述关键帧，挑选出共有单词数大于阈值1的及其和当前帧单词匹配得分存入lScoreAndMatch
    for(list::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;

        // 当前帧F只和具有共同单词较多（大于minCommonWords）的关键帧进行比较
        if(pKFi->mnRelocWords>minCommonWords)
        {
            nscores++;  // 这个变量后面没有用到
            // 用mBowVec来计算两者的相似度得分
            float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
            pKFi->mRelocScore=si;
            lScoreAndMatch.push_back(make_pair(si,pKFi));
        }
    }

    if(lScoreAndMatch.empty())
        return vector();

    list lAccScoreAndMatch;
    float bestAccScore = 0;

    // Lets now accumulate score by covisibility
    // Step 4：计算lScoreAndMatch中每个关键帧的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值2
    // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧共视程度最高的前十个关键帧归为一组，计算累计得分
    for(list::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        KeyFrame* pKFi = it->second;
        // 取出与关键帧pKFi共视程度最高的前10个关键帧
        vector vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        // 该组最高分数
        float bestScore = it->first; 
        // 该组累计得分
        float accScore = bestScore;  
        // 该组最高分数对应的关键帧
        KeyFrame* pBestKF = pKFi;   
        // 遍历共视关键帧，累计得分 
        for(vector::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            if(pKF2->mnRelocQuery!=F->mnId)
                continue;
            // 只有pKF2也在重定位候选帧中，才能贡献分数
            accScore+=pKF2->mRelocScore;

            // 统计得到组里分数最高的KeyFrame
            if(pKF2->mRelocScore>bestScore)
            {
                pBestKF=pKF2;
                bestScore = pKF2->mRelocScore;
            }

        }

        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));

        // 记录所有组中最高的得分
        if(accScore>bestAccScore) 
            bestAccScore=accScore; 
    }

    // Return all those keyframes with a score higher than 0.75*bestScore
    // Step 5：得到所有组中总得分大于阈值2的，组内得分最高的关键帧，作为候选关键帧组
    //阈值2：最高得分的0.75倍
    float minScoreToRetain = 0.75f*bestAccScore; 
    set spAlreadyAddedKF;
    vector vpRelocCandidates;
    vpRelocCandidates.reserve(lAccScoreAndMatch.size());
    for(list::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
    {
        const float &si = it->first;
        // 只返回累计得分大于阈值2的组中分数最高的关键帧
        if(si>minScoreToRetain)
        {
            KeyFrame* pKFi = it->second;
            // 判断该pKFi是否已经添加在队列中了
            if(!spAlreadyAddedKF.count(pKFi))
            {
                vpRelocCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpRelocCandidates;
}

} //namespace ORB_SLAM

 

四、SetRansacParameters()

通过 DetectRelocalizationCandidates() 可以获得候选帧组 vector vpCandidateKFs。遍历所有的候选关键帧，通过词袋进行快速匹配。如果该关键帧匹配特征点数目足够，则用匹配结果初始化PnP Solver。这个内容暂且放一下，后续章节进行讨论。这里需要注意两个变量: vbDiscarded: 放弃某个关键帧的标记，如果 vpCandidateKFs 的关键帧与当前帧没有匹配上，则vbDiscarded对应的位置会被赋值为true。 vpPnPsolvers: 如果vpCandidateKFs 的关键帧与当前帧没有匹配上，则其初始化之后的PnPsolver* pSolver 存储于vpPnPsolvers变量中。

 

五、SearchByProjection()

循环遍历对匹配特征点数目足够的关键帧(根据vbDiscarded变量)，需要从这些候选关键帧中，进行位置回复。重定位跟踪是比较严肃的过程，比如其与闭环的步骤有相似的地方，也就是说重定位如果出现问题会可能会导致误闭环。其步骤主要如下。代码于Tracking.cc 中的 while(nCandidates>0 && !bMatch) 处进行讲解。

( 01 ) : \color{blue}{(01)}: (01): 使用 for 循环遍历所有的候选关键帧，通过EPnP算法估计姿态(后面博客做详细讲解)。

( 02 ) : \color{blue}{(02)}: (02): 如果 EPnP 未求解出姿态，则跳过当前候选帧，对下一候选帧进行处理。

( 03 ) : \color{blue}{(03)}: (03): 如果 EPnP 求解出姿态，则还需要做一系列复杂的操作，避免误闭环。         ①遍历所有内点，获得内点对应的地图点，如果该内点无对应的地图点，则设置为 NULL。         ②使用Optimizer::PoseOptimization();优化位姿(不优化地图点的坐标)，返回优化之后的内点数量。         ③如果优化之后的内点数目低于10个(原本内点为15个以上，优化之后内点反而变少了)，则跳过当前候选关键帧,但是却没有放弃当前帧的重定位，同时删除外点对应的地图点         ④如果优化之后的内点为10~50之间，则通过SearchByProjection投影的方式对之前EPnP未匹配成功的点进行匹配(注意:其是经过优化再匹配的)。         ⑤通过SearchByProjection投影匹配之后，超过50个匹配，则再次使用 Optimizer::PoseOptimization(); 对位姿进行优化。如果低于30个匹配则放弃该候选帧         ⑥通过SearchByProjection投影匹配之后，如果处于30~50个匹配之间，则用附加之后的匹配点再次优化位置，接着再进行投影。最终再计算新的匹配，看是否能够超过50，超过则成功，失败则彻底放弃该候选帧。

SearchByProjection()函数在上一篇博客中进行了详细的讲解，简单的说，就是把候选帧的地图点，通过相机投影模型，得到投影到当前帧的像素坐标。然后在该像素点的周边进行汉明距离的搜索匹配。

 

六、Relocalization()注释

关于Relocalization()函数的注释如下(位于src/Tracking.cc)文件中：

/**
 * @details 重定位过程
 * @return true 
 * @return false 
 * 
 * Step 1：计算当前帧特征点的词袋向量
 * Step 2：找到与当前帧相似的候选关键帧
 * Step 3：通过BoW进行匹配
 * Step 4：通过EPnP算法估计姿态
 * Step 5：通过PoseOptimization对姿态进行优化求解
 * Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
 */
bool Tracking::Relocalization()
{
    // Compute Bag of Words Vector
    // Step 1：计算当前帧特征点的词袋向量
    mCurrentFrame.ComputeBoW();

    // Relocalization is performed when tracking is lost
    // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
    // Step 2：用词袋找到与当前帧相似的候选关键帧
    vector vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
    
    // 如果没有候选关键帧，则退出
    if(vpCandidateKFs.empty())
        return false;

    const int nKFs = vpCandidateKFs.size();

    // We perform first an ORB matching with each candidate
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.75,true);
    //每个关键帧的解算器
    vector vpPnPsolvers;
    vpPnPsolvers.resize(nKFs);

    //每个关键帧和当前帧中特征点的匹配关系
    vector vvpMapPointMatches;
    vvpMapPointMatches.resize(nKFs);
    
    //放弃某个关键帧的标记
    vector vbDiscarded;
    vbDiscarded.resize(nKFs);

    //有效的候选关键帧数目
    int nCandidates=0;

    // Step 3：遍历所有的候选关键帧，通过词袋进行快速匹配，用匹配结果初始化PnP Solver
    for(int i=0; iisBad())
            vbDiscarded[i] = true;
        else
        {
            // 当前帧和候选关键帧用BoW进行快速匹配，匹配结果记录在vvpMapPointMatches，nmatches表示匹配的数目
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            // 如果和当前帧的匹配数小于15,那么只能放弃这个关键帧
            if(nmatchesSetRansacParameters(
                    0.99,   //用于计算RANSAC迭代次数理论值的概率
                    10,     //最小内点数, 但是要注意在程序中实际上是min(给定最小内点数,最小集,内点数理论值),不一定使用这个
                    300,    //最大迭代次数
                    4,      //最小集(求解这个问题在一次采样中所需要采样的最少的点的个数,对于Sim3是3,EPnP是4),参与到最小内点数的确定过程中
                    0.5,    //这个是表示(最小内点数/样本总数);实际上的RANSAC正常退出的时候所需要的最小内点数其实是根据这个量来计算得到的
                    5.991); // 自由度为2的卡方检验的阈值,程序中还会根据特征点所在的图层对这个阈值进行缩放
                vpPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }

    // Alternatively perform some iterations of P4P RANSAC
    // Until we found a camera pose supported by enough inliers
    // 这里的 P4P RANSAC是Epnp，每次迭代需要4个点
    // 是否已经找到相匹配的关键帧的标志
    bool bMatch = false;
    ORBmatcher matcher2(0.9,true);

    // Step 4: 通过一系列操作,直到找到能够匹配上的关键帧
    // 为什么搞这么复杂？答：是担心误闭环
    while(nCandidates>0 && !bMatch)
    {
        //遍历当前所有的候选关键帧
        for(int i=0; iiterate(5,bNoMore,vbInliers,nInliers);

            // If Ransac reachs max. iterations discard keyframe
            // bNoMore 为true 表示已经超过了RANSAC最大迭代次数，就放弃当前关键帧
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If a Camera Pose is computed, optimize
            if(!Tcw.empty())
            {
                //  Step 4.2：如果EPnP 计算出了位姿，对内点进行BA优化
                Tcw.copyTo(mCurrentFrame.mTcw);
                
                // EPnP 里RANSAC后的内点的集合
                set sFound;

                const int np = vbInliers.size();
                //遍历所有内点
                for(int j=0; j