3.6ORB-SLAM3之重定位Relocalization

0总述

重定位是ORB-SLAM系列保持跟踪稳定性的保障，从ORB-SLAM沿用至ORB-SLAM3。主要作用是在跟踪失败时，通过词袋向量搜索在关键帧数据库中寻找和当前帧相似的关键帧作为匹配帧，建立数据关联并计算当前帧的位姿，恢复相机的运动。

1重定位触发条件

当上一帧在跟踪运动模型环节TrackWithMotionModel()或者跟踪参考关键帧环节TrackReferenceKeyFrame跟踪失败时，会对当前帧的跟踪状态有一个判断。

• 条件1：当前地图中关键帧数目较多（大于10）
• 条件2（隐藏条件）：当前帧距离上次重定位帧超过1s（说明还比较争气，值的救）
• 条件3：不是IMU模式

同时满足条件1，2 || 1,3 则将状态标记为RECENTLY_LOST;如果是纯视觉模式，则在下一帧会进入重定位函数；如果是VI模式，则在下一帧会通过预积分更新位姿。

2重定位过程

• Step 1：计算当前帧特征点的词袋向量
• Step 2：找到与当前帧相似的候选关键帧
• Step 3：通过BoW进行匹配
• Step 4：通过MLPnP算法估计姿态
• Step 5：通过PoseOptimization对姿态进行优化求解
• Step 6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解

2.1计算当前帧特征点的词袋向量

即将当前帧特征点对应的描述子向量转化为词袋向量，

mCurrentFrame.ComputeBoW();

主要使用DBOW库的函数,和跟踪参考关键帧中过程相同

mpORBvocabulary->transform(vCurrentDesc,// 当前的描述子vector
                                    mBowVec,// 输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
                                    mFeatVec,// 输出，记录node id及其对应的图像 feature对应的索引
                                    4);// 4表示从叶节点向前数的层数

2.2在当前子地图找到与当前帧相似的候选关键帧组

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

步骤1：找出和当前帧具有公共单词的所有关键帧
遍历当前帧词袋向量中的每一个单词，对于每一个单词，会在关键帧数据库中寻找包含该单词的关键帧，并记录该关键帧与当前帧的公共单词数量pKFi->mnRelocWords。如果找不到候选关键帧，本次重定位就宣告失败了。

// 步骤1：找出和当前帧具有公共单词的所有关键帧
{
    unique_lock<mutex> lock(mMutex);
    // 遍历当前帧所有的单词
    for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
    {
        // lKFs表示包含了第vit->first个单词的所有关键帧
        // 在mvInvertedFile中，每一个单词都对应一个list<KeyFrame*>
        list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
        // 遍历这些关键帧
        for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
        {
            KeyFrame* pKFi=*lit;
            // 如果pKFi还没有标记为重定位的候选帧，将该关键帧重定位相关的属性初始化一下
            // 因为两帧之间可能有多个公共单词，所以在遍历之前的单词时就将该关键帧进行了标记
            if(pKFi->mnRelocQuery!=F->mnId)
            {
                pKFi->mnRelocWords=0;// 共有的单词数
                pKFi->mnRelocQuery=F->mnId;// 具有相同单词的F的ID
                lKFsSharingWords.push_back(pKFi);// 放入候选
            }
            // 如果已经被标记过了，直接将公共单词数自增1
            pKFi->mnRelocWords++;
        }
    }
}

步骤2：只和具有共同单词较多的关键帧进行相似度计算
为了进一步保证重定位的鲁棒性，在候选关键帧比较多的的情况下会择优进行筛选，挑选和当前帧公共单词较多的关键帧进行后面相似度计算等相关操作。

对于关键帧和普通的图像帧来说，该帧对应的描述子都可以被转化为一个对应的词袋向量，这个词袋向量包含了很多单词，向量非零部分的元素表示该图像包含某个单词，0表示不包含某个单词。相似度计算就是指计算两个图像帧之间词袋向量的差异，公共单词数越多，最终相似性得分就越高。在ORB-SLAM系列中采用L1范数计算两个词袋向量之间的差异。具体计算过程参考下图：

    // 遍历所有闭环候选帧，挑选出共有单词数大于阈值minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
    // 遍历所有闭环候选帧，挑选出共有单词数大于阈值minCommonWords和相似性得分
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;
        if(pKFi->mnRelocWords > minCommonWords)
        {
            //nscores++;
            float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);// 计算相似性得分
            pKFi->mRelocScore=si;// 记录相似性得分
            lScoreAndMatch.push_back(make_pair(si,pKFi));// 先不进行筛选，全都放到容器中
        }
    }

步骤3：将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
为了保证所选出的候选关键帧和当前帧具备较好的相似性，在进行上述操作初步筛选出与当前帧相似性较高的候选关键帧后，还要更进一步得看这些关键帧的共视关键帧与当前帧的相似性，具体操作如下：

首先，对于每个候选关键帧，选择与其共视程度最高，权重最高的前10个关键帧划分为一组，如果少于10个则选择全部的共视关键帧

要注意的是，对于候选关键帧的共视关键帧，只有和当前帧存在公共单词的共视关键帧才会参与到下一步的计算

vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

然后，遍历每一个组的关键帧，统计每个组得分最高的关键帧，以及总得分最高的组对应的最高分数bestAccScore

    // 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分
    // 具体而言：lScoreAndMatch中每一个KeyFrame都把与自己共视程度较高的帧归为一组，每一组会计算组得分并记录该组分数最高的KeyFrame，记录于lAccScoreAndMatch
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
    {
        // 得到与该关键帧连接的前N个关键帧(已按权值排序)，如果连接的关键帧少于N，则返回所有连接的关键帧
        KeyFrame* pKFi = it->second;
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);

        float bestScore = it->first;// 该组最高分数
        float accScore = bestScore;// 该组累计得分
        KeyFrame* pBestKF = pKFi;// 该组最高分数对应的关键帧
        // 遍历该组的10个关键帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKF2 = *vit;
            if(pKF2->mnRelocQuery!=F->mnId)
                continue;

            accScore+=pKF2->mRelocScore;// 只有pKF2也在闭环候选帧中(重定位候选帧中)，才能贡献分数
            if(pKF2->mRelocScore>bestScore)// 统计得到组里分数最高的KeyFrame
            {
                pBestKF=pKF2;
                bestScore = pKF2->mRelocScore;
            }

        }
        lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));// 记录所有组中组得分最高的关键帧
        if(accScore>bestAccScore)
            bestAccScore=accScore;// 得到所有组中最高的累计得分
    }

然后，根据最高分数bestAccScore再从每个组得分最高的关键帧里挑选大于阈值的关键帧，然后去掉不属于当前活跃子地图的关键帧，其余的作为最后的候选关键帧返回。

2.3遍历所有的候选关键帧，通过BoW进行快速匹配，用匹配结果初始化PnP Solver

• 首先，通过BOW词袋向量搜索，快速查找候选关键帧与当前帧之间的匹配关系。这个过程与跟踪参考关键帧中的SearchByBoW相同，详细见之前的博客。

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

• 然后，将计算得到的匹配关系初始化MLPNP求解器pSolver，并重新设置RanSAC迭代求解器的相关参数

    for(int i=0; i<nKFs; i++)
    {
        KeyFrame* pKF = vpCandidateKFs[i];
        // 如果当前帧已经被判定为无效帧，跳过（什么时候会被判定为无效帧？？？？）
        if(pKF->isBad())
            vbDiscarded[i] = true;
        else
        {
            // 当前帧和候选关键帧用BoW进行快速匹配，匹配结果记录在vvpMapPointMatches，nmatches表示匹配的数目
            // vvpMapPointMatches是一个2维vector数组，每一个元素为该关键帧中和当前帧成功匹配的所有特征点id
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
            // 如果匹配数目小于15,认为该帧无效，跳过
            if(nmatches<15)
            {
                vbDiscarded[i] = true;
                continue;
            }
            else
            {
                // 如果匹配数目够用，用匹配结果初始化MLPnPsolver
                // ? 为什么用MLPnP? 因为考虑了鱼眼相机模型，解耦某些关系？
                // 参考论文《MLPNP-A REAL-TIME MAXIMUM LIKELIHOOD SOLUTION TO THE PERSPECTIVE-N-POINT PROBLEM》
                MLPnPsolver* pSolver = new MLPnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                // 初始化Ransac迭代器参数，构造函数调用了一遍，这里重新设置参数
                // 0.99,      // 模型最大概率值，默认0.9
                // 10,        // 内点的最小阈值，默认8
                // 300,       // 最大迭代次数，默认300
                // 6,         // 最小集，每次采样六个点，即最小集应该设置为6，论文里面写着I > 5
                // 0.5,       // 理论最少内点个数，这里是按照总数的比例计算，所以epsilon是比例，默认是0.4
                // 5.991;     // 卡方检验阈值 //This solver needs at least 6 points
                pSolver->SetRansacParameters(0.99,10,300,6,0.5,5.991);  //This solver needs at least 6 points
                // 对于每一对较好的匹配，都需要使用匹配结果初始化MLPNP求解器
                vpMLPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }

2.4遍历所有符合条件的候选关键帧,直到找到能够匹配上的关键帧

1.基于RanSAC使用MLPNP求解当前帧的初始位姿

对于每一对匹配，在步骤3都构造了一个对应的求解器。求解器pSolver共迭代5次，对于每一次都使用RanSAC算法估计当前帧的位姿，RanSAC停止迭代的条件是达到最大的迭代次数或者提前找到的小于阈值的最优解。

如果是因为迭代次数达到最大值而停止迭代，说明该候选关键帧不好，将其设置为无效帧。

位姿求解器最终返回位姿是否计算成功的flag、每一对匹配点的匹配情况即是否为内点、内点的数量和初始位姿

// pnp求解位姿
bool bTcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers, eigTcw);

2.如果MLPNP计算出了位姿，对内点进行优化

这里的位姿优化和跟踪运动模型或者跟踪参考关键帧使用的是同一个函数，以MLPNP的求解结果作为初始位姿，使用BA对当前的位姿进行优化，并进一步更新内点的情况，在优化函数的内部隐式地更新当前帧的位姿，返回内点的数量。

int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

3.如果内点数量较少，则使用跟踪运动模型中的投影匹配

这一步又和跟踪运动模型相同了，主要是关键帧对应3D地图点到当前帧的投影匹配计算相机位姿，每经过一次投影匹配查找就会使用一次非线性优化，更新相机位姿,投影匹配详细的内容可以见另一篇跟踪参考关键帧的介绍文章。

首先将参考关键帧中对应的地图点根据相机内参投影到当前帧图像上(因为地图点都是在世界坐标系下，因此可以直接通过内参投影到当前帧)

根据特征点的网格信息和搜索半径，减小特征匹配的搜索范围加速特征匹配

然后就是进行描述子距离计算和旋转一致性检测，获得最终的匹配关系。

最终根据优化后内点的数量确定重定位是否成功，阈值为50，内点数量大于50则判定重定位成功，下一帧继续正常跟踪，反之则跟踪彻底凉凉，等待系统的重新初始化。

if(nGood<50)
{
    // 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配
    int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
    // 根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿
    if(nadditional+nGood>=50)
    {
        // 记录再次优化后的内点数量
        nGood = Optimizer::PoseOptimization(&mCurrentFrame);

        // If many inliers but still not enough, search by projection again in a narrower window
        // the camera has been already optimized with many points
        // Step 4.4：如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 最后垂死挣扎 
        // 重新执行上一步 4.3的过程，只不过使用更小的搜索窗口
        // 这里的位姿已经使用了更多的点进行了优化,应该更准，所以使用更小的窗口搜索
        if(nGood>30 && nGood<50)
        {
            // 重新获取经过上述BOW匹配以及投影匹配后得到的Mappoint
            sFound.clear();
            for(int ip =0; ip<mCurrentFrame.N; ip++)
                if(mCurrentFrame.mvpMapPoints[ip])
                    sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
            // 缩小搜索半径，再次将关键帧与当前帧进行投影匹配
            nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);

            // Final optimization
            // 最终经过BOW匹配-优化-投影匹配-优化-缩小半径投影匹配之后，内点数量依然不到50,
            // 判定基于该候选关键帧进行的重定位失败
            // 如果成功了，就更新下当前帧的Mappoint
            if(nGood+nadditional>=50)
            {
                nGood = Optimizer::PoseOptimization(&mCurrentFrame);

                for(int io =0; io<mCurrentFrame.N; io++)
                    if(mCurrentFrame.mvbOutlier[io])
                        mCurrentFrame.mvpMapPoints[io]=NULL;
            }
        }//if(nGood>30 && nGood<50)
    }//if(nadditional+nGood>=50)
}//if(nGood<50)

综上可以看到，重定位函数其实就是跟踪运动模型和跟踪参考关键帧的综合，首先通过BOW搜索寻找匹配关系然后MLPNP求解位姿和BA优化；如果结果不满足要求再进行投影匹配的BA优化，根据最终优化后内点的数量判断重定位是否成功。

2.5 MLPNP算法简介

MLPNP的核心思想是对于每一个3D空间点，连接空间点到相机光心可以得到一个法向量v，以及在切平面对应的一组正交基[r,s],然后将2D-3D点的投影匹配转换为使用正交基之间进行计算。