22跟踪丢失后重定位

ORBSLAM2

---

---

代码

bool Tracking::Relocalization()
{
    // Compute Bag of Words Vector
    mCurrentFrame.ComputeBoW();//计算当前帧的词袋模型

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

    if(vpCandidateKFs.empty())//如果没有候选关键帧就退出
        return false;

    const int nKFs = vpCandidateKFs.size();//定义nKFs为候选关键帧的数量

    // 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<PnPsolver*> vpPnPsolvers;//每个关键帧解算器
    vpPnPsolvers.resize(nKFs);

    vector<vector<MapPoint*> > vvpMapPointMatches;//每个关键帧与当前帧的特征点的匹配关系

    vvpMapPointMatches.resize(nKFs);

    vector<bool> vbDiscarded;//放弃某个关键帧的标记
    vbDiscarded.resize(nKFs);

    int nCandidates=0;

    for(int i=0; i<nKFs; i++)//开始遍历候选关键帧，通过词袋进行加速匹配
    {
        KeyFrame* pKF = vpCandidateKFs[i];
        if(pKF->isBad())
            vbDiscarded[i] = true;
        else
        {
            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);//词袋加速匹配
            if(nmatches<15)//小于15不行
            {
                vbDiscarded[i] = true;
                continue;
            }
            else//用pnp
            {
                PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
                pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
                vpPnPsolvers[i] = pSolver;
                nCandidates++;
            }
        }
    }

    // Alternatively perform some iterations of P4P RANSAC
    // Until we found a camera pose supported by enough inliers
    bool bMatch = false;
    ORBmatcher matcher2(0.9,true);//通过一些操作找到能匹配上的关键帧

    while(nCandidates>0 && !bMatch)
    {
        for(int i=0; i<nKFs; i++)
        {
            if(vbDiscarded[i])
                continue;

            // Perform 5 Ransac Iterations
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;

            PnPsolver* pSolver = vpPnPsolvers[i];//通过EPNP来算位姿
            cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

            // If Ransac reachs max. iterations discard keyframe
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If a Camera Pose is computed, optimize
            if(!Tcw.empty())
            {
                Tcw.copyTo(mCurrentFrame.mTcw);//如多恢复了位姿，就给当前帧

                set<MapPoint*> sFound;//EPNP的内点

                const int np = vbInliers.size();

                for(int j=0; j<np; j++)
                {
                    if(vbInliers[j])//遍历所有内点
                    {
                        mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
                        sFound.insert(vvpMapPointMatches[i][j]);
                    }
                    else
                        mCurrentFrame.mvpMapPoints[j]=NULL;
                }
				//对位姿进行优化
                int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

                if(nGood<10)//内点的个数小于10，放弃
                    continue;

                for(int io =0; io<mCurrentFrame.N; io++)//删除外点
                    if(mCurrentFrame.mvbOutlier[io])
                        mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);

                // If few inliers, search by projection in a coarse window and optimize again
                if(nGood<50)//小于50
                {
                    int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);//将关键帧中未匹配的地图点投影当前帧

                    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
                        if(nGood>30 && nGood<50)//30-50和前面一样
                        {
                            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
                            if(nGood+nadditional>=50)//加完后>50可以进行位姿优化
                            {
                                nGood = Optimizer::PoseOptimization(&mCurrentFrame);

                                for(int io =0; io<mCurrentFrame.N; io++)
                                    if(mCurrentFrame.mvbOutlier[io])
                                        mCurrentFrame.mvpMapPoints[io]=NULL;
                            }
                        }
                    }
                }


                // If the pose is supported by enough inliers stop ransacs and continue
                if(nGood>=50)
                {
                    bMatch = true;
                    break;
                }
            }
        }
    }

    if(!bMatch)
    {
        return false;
    }
    else
    {
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    }

}

vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)//传参：传的是当前普通帧
{
    list<KeyFrame*> lKFsSharingWords;

    // Search all keyframes that share a word with current frame//找出与当前帧有公共单词的所有关键帧
    {
        unique_lock<mutex> lock(mMutex);

        for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)//开始遍历当前普通帧的BowVector
        {
            list<KeyFrame*> &lKFs =   mvInvertedFile[vit->first];//依据普通帧中所有的wordid来找出包含这个的所有关键帧，逆向索引？？

            for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)//开始遍历包含这个word的所有关键帧
            {
                KeyFrame* pKFi=*lit;//取出关键帧
                if(pKFi->mnRelocQuery!=F->mnId)//pkFi还没有标记为重定位候选帧
                {
                    pKFi->mnRelocWords=0;
                    pKFi->mnRelocQuery=F->mnId;//就是相当于给关键帧打上一个标记，标记普通帧于其相匹配的ID
                    lKFsSharingWords.push_back(pKFi);
                }
                pKFi->mnRelocWords++;//记录关键帧中有几个单词与普通帧是一样的
            }
        }
    }
    if(lKFsSharingWords.empty())//如果没有关键帧，那就返回
        return vector<KeyFrame*>();

    // Only compare against those keyframes that share enough words//统计关键帧中与当前帧中单词最多的关键帧
    int maxCommonWords=0;
    for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)//遍历关键帧
    {
        if((*lit)->mnRelocWords>maxCommonWords)//关键帧中的几个单词一样的数目大于阈值
            maxCommonWords=(*lit)->mnRelocWords;//将最多的设为阈值
    }

    int minCommonWords = maxCommonWords*0.8f;//最少的相同单词数量是最大的0.8倍

    list<pair<float,KeyFrame*> > lScoreAndMatch;

    int nscores=0;

    // Compute similarity score.计算相似得分，遍历所有关键帧
    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));//记录相似度得分
        }
    }

    if(lScoreAndMatch.empty())//如果是空的话
        return vector<KeyFrame*>();//返回

    list<pair<float,KeyFrame*> > lAccScoreAndMatch;
    float bestAccScore = 0;

    // Lets now accumulate score by covisibility
    for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)//开始迭代存放分数与关键帧的list
    {
        KeyFrame* pKFi = it->second;//取出关键帧
        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);//将于其有共视关系的前十个关键帧帧拿出来

        float bestScore = it->first;//记录最佳的分数
        float accScore = bestScore;//将最佳的分数赋值给accScore作为初值
        KeyFrame* pBestKF = pKFi;//最佳关键帧
        for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)//开始遍历有共视关系的十个关键帧
        {
            KeyFrame* pKF2 = *vit;//取出他们
            if(pKF2->mnRelocQuery!=F->mnId)//如果他们与当前普通帧没有相同的单词，continue
                continue;

            accScore+=pKF2->mRelocScore;//累加得分，就是将有共视关系的关键帧分数进行累加
            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
    float minScoreToRetain = 0.75f*bestAccScore;//得到最得分最大的阈值，以及组内得分最高的关键帧，只有大于0.75*最高总分才有资格进行评选
    set<KeyFrame*> spAlreadyAddedKF;
    vector<KeyFrame*> vpRelocCandidates;
    vpRelocCandidates.reserve(lAccScoreAndMatch.size());
    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)//开始遍历组的分数
    {
        const float &si = it->first;//取出总得分
        if(si>minScoreToRetain)//大于阈值的话
        {
            KeyFrame* pKFi = it->second;//关键帧的最佳得分
            if(!spAlreadyAddedKF.count(pKFi))//判断是否在队列中
            {
                vpRelocCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpRelocCandidates;//候选关键帧
}

} //namespace ORB_SLAM