ORB-SLAM2 ---- Tracking::Relocalization函数

目录

1.函数作用

2.步骤

3.code 

4.函数解释 

4.1 将当前帧的描述子转化为BoW向量

4.2 用词袋找到与当前帧相似的候选关键帧

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

4.4 通过一系列操作，直到找到能够匹配上的关键帧

---

1.函数作用

        我们先用最近的关键帧来跟踪当前的普通帧，失败！

        根据恒速模型失败了，我们再根据参考关键帧来跟踪，失败！

        如果跟踪状态不成功，那么就只能重定位了！如果这再失败，那就重置slam系统了！

2.步骤

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

函数返回值是true或者false表示是否跟踪成功！

3.code 

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(nmatches<15)
            {
                vbDiscarded[i] = true;
                continue;
            }
            else
            {
                // 如果匹配数目够用，用匹配结果初始化EPnPsolver
                // 为什么用EPnP? 因为计算复杂度低，精度高
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(
                    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; i vbInliers;     
            
            //内点数
            int nInliers;
            
            // 表示RANSAC已经没有更多的迭代次数可用 -- 也就是说数据不够好，RANSAC也已经尽力了。。。
            bool bNoMore;

            // Step 4.1：通过EPnP算法估计姿态，迭代5次
            PnPsolver* pSolver = vpPnPsolvers[i];
            cv::Mat Tcw = pSolver->iterate(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(NULL);

                // If few inliers, search by projection in a coarse window and optimize again
                // Step 4.3：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解
                // 前面的匹配关系是用词袋匹配过程得到的
                if(nGood<50)
                {
                    // 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中, 生成新的匹配
                    int nadditional = matcher2.SearchByProjection(
                        mCurrentFrame,          //当前帧
                        vpCandidateKFs[i],      //关键帧
                        sFound,                 //已经找到的地图点集合，不会用于PNP
                        10,                     //窗口阈值，会乘以金字塔尺度
                        100);                   //匹配的ORB描述子距离应该小于这个阈值

                    // 如果通过投影过程新增了比较多的匹配特征点对
                    if(nadditional+nGood>=50)
                    {
                        // 根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿
                        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)
                        {
                            // 用更小窗口、更严格的描述子阈值，重新进行投影搜索匹配
                            sFound.clear();
                            for(int ip =0; ip=50)
                            {
                                nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                                //更新地图点
                                for(int io =0; io=50)
                {
                    bMatch = true;
                    // 只要有一个候选关键帧重定位成功，就退出循环，不考虑其他候选关键帧了
                    break;
                }
            }
        }//一直运行,知道已经没有足够的关键帧,或者是已经有成功匹配上的关键帧
    }

    // 折腾了这么久还是没有匹配上，重定位失败
    if(!bMatch)
    {
        return false;
    }
    else
    {
        // 如果匹配上了,说明当前帧重定位成功了(当前帧已经有了自己的位姿)
        // 记录成功重定位帧的id，防止短时间多次重定位
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    }
}

4.函数解释 

4.1 将当前帧的描述子转化为BoW向量

ORB-SLAM2 ---- Frame::ComputeBoW函数（TrackReferenceKeyFrame调用版）https://blog.csdn.net/qq_41694024/article/details/128007040?csdn_share_tail=%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22128007040%22%2C%22source%22%3A%22qq_41694024%22%7D        这是笔者自己写的博客！里面介绍的很清楚。

4.2 用词袋找到与当前帧相似的候选关键帧

        我们从SLAM系统中所有的关键帧找到和当前帧相似的关键帧作为候选重定位关键帧。

ORB-SLAM2 ---- KeyFrameDatabase::DetectRelocalizationCandidates函数解析https://blog.csdn.net/qq_41694024/article/details/128086208        这是笔者自己写的博客！里面介绍的很清楚。

        如果没有和当前帧相似的关键帧，则退出。

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

        我们在进行这步之前先了解一下EPnP算法：它是一个成熟的算法，具体原理不用我们知道，我们只要知道怎么调用它就可以了。

        它的输入为：

        ①n个世界坐标系下的3D点，论文中称为3D参考点

        ②这个3D点投影在图像上的2D坐标

        ③相机内参矩阵 ，包括焦距和主点

        它的输出为：

        ①相机的位姿R,t
        我们看看代码中它的临时变量：

        ①vpPnPsolvers 每个关键帧的解算器

        ②vector > vvpMapPointMatches 每个关键帧和当前帧中特征点的匹配关系

        ③vbDiscarded 放弃某个关键帧的标记

        进入正题了：

        我们首先遍历所有待匹配的关键帧。对于一帧来说：

        pKF获取当前候选关键帧，我们用当前待重跟踪关键帧mCurrentFrame和pKF利用BoW进行粗略的特征点匹配，匹配结果存放在vvpMapPointMatches[i]中。如果匹配结果小于15个特征点，则我们放弃此帧，置vbDiscarded为true。我们用mCurrentFrame和vvpMapPointMatches[i]初始化PnPsolver，将PnP求解器放在vpPnPsolvers中，并将有效候选关键帧个数nCandidates+1。

        结束这个循环后，我们记录了候选追踪关键帧的有效个数nCandidates，并有了nCandidates个PnPSovler。

4.4 通过一系列操作，直到找到能够匹配上的关键帧

        我们先了解这段代码中的临时变量：

        ①bMatch 是否已经找到相匹配的关键帧的标志

        ②vbInliers 内点标记

        ③nInliers 内点数

        ④bNoMore 表示RANSAC已经没有更多的迭代次数可用 -- 也就是说数据不够好，RANSAC也已经尽力了。。。

        ⑤sFound  EPnP里RANSAC后的内点的集合

        ⑥np 内点的数量

        让我们看看这段代码的流程吧！

        遍历当前所有的候选关键帧nKFs，针对于nKFs[i]:

        ①若在上步标记为vbDiscarded则放弃此帧。

        ②通过EPnP算法估计姿态，迭代5次，得到Tcw（相机位姿）。若RANSAC已经没有更多的迭代次数可用，置此帧为vbDiscarded[i]=true，候选追踪关键帧的有效个数nCandidates--。

        ③若相机位姿Tcw计算出来了，对内点进行BA优化。遍历所有内点，设置该关键帧mCurrentFrame可以观测到的地图点，并将此地图点放到sFound集合中。只优化位姿，不优化地图点的坐标，返回的是内点的数量nGood，如果优化后的内点数目不多（小于10），则跳过当前候选关键帧，但是却没有放弃当前帧的重定位。

        ④如果优化后的内点数目足够多，则删除外点对应的地图点。

        ⑤如果内点较少（1030)，可以挽救一下，最后垂死挣扎。如果对于当前的候选关键帧已经有足够的内点(50个)了，那么就认为重定位成功。

        若失败重新执行上一步的过程，只不过使用更小的搜索窗口。如果成功挽救回来，匹配数目达到要求，最后BA优化一下。

        折腾了这么久还是没有匹配上，重定位失败。如果匹配上了，说明当前帧重定位成功了(当前帧已经有了自己的位姿)，记录成功重定位帧的idmnLastRelocFrameId，防止短时间多次重定位。