SLAM——ORB-SLAM3代码分析LoopClosing

Posted 2021-12-15 Jevies

2021SC@SDUSC

LoopClosing分析（2）

这一篇博客依旧是LoopClosing的范畴，主要聚焦于分析NewDetectCommonRegions函数。由于代码相对较长，因此本篇博客都直接将分析以注释的形式写在代码中。

• NewDetectCommonRegions函数是用来进行闭环检测： 成功检测到闭环时返回true；反之返回false。

bool LoopClosing::NewDetectCommonRegions()

    
        // Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
        unique_lock<mutex> lock(mMutexLoopQueue);
        // 从队列头开始取，也就是先取早进来的关键帧
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        // 取出关键帧后从队列里弹出该关键帧
        mlpLoopKeyFrameQueue.pop_front();
        // 设置当前关键帧不要在优化的过程中被删除
        mpCurrentKF->SetNotErase();
        mpCurrentKF->mbCurrentPlaceRecognition = true;
        mpLastMap = mpCurrentKF->GetMap();
    
    
    // Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
    if(mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA1())
    
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    

    if(mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5) //12
    
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    

    if(mpLastMap->GetAllKeyFrames().size() < 12)
    
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    

    //Check the last candidates with geometric validation
    // Loop candidates
    bool bLoopDetectedInKF = false;
    bool bCheckSpatial = false;

    if(mnLoopNumCoincidences > 0)
    
        bCheckSpatial = true;
        // Find from the last KF candidates
        cv::Mat mTcl = mpCurrentKF->GetPose() * mpLoopLastCurrentKF->GetPoseInverse();
        g2o::Sim3 gScl(Converter::toMatrix3d(mTcl.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcl.rowRange(0, 3).col(3)),1.0);
        g2o::Sim3 gScw = gScl * mg2oLoopSlw;
        int numProjMatches = 0;
        vector<MapPoint*> vpMatchedMPs;
        // 当前帧与匹配闭环帧局部map进行匹配，优化，如果最终匹配数量超过一定阈值，认为闭环成立
        bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);
        if(bCommonRegion)
        

            bLoopDetectedInKF = true;

            mnLoopNumCoincidences++;
            mpLoopLastCurrentKF->SetErase();
            mpLoopLastCurrentKF = mpCurrentKF;
            mg2oLoopSlw = gScw;
            mvpLoopMatchedMPs = vpMatchedMPs;


            mbLoopDetected = mnLoopNumCoincidences >= 3;
            mnLoopNumNotFound = 0;

            if(!mbLoopDetected)
            
                cout << "PR: Loop detected with Reffine Sim3" << endl;
            
        
        else
        
            bLoopDetectedInKF = false;
            mnLoopNumNotFound++;
            if(mnLoopNumNotFound >= 2)
            
                mpLoopLastCurrentKF->SetErase();
                mpLoopMatchedKF->SetErase();
                //mg2oLoopScw;
                mnLoopNumCoincidences = 0;
                mvpLoopMatchedMPs.clear();
                mvpLoopMPs.clear();
                mnLoopNumNotFound = 0;
            

        
    

    //Merge candidates
    bool bMergeDetectedInKF = false;
    if(mnMergeNumCoincidences > 0)
    
        // Find from the last KF candidates

        cv::Mat mTcl = mpCurrentKF->GetPose() * mpMergeLastCurrentKF->GetPoseInverse();
        g2o::Sim3 gScl(Converter::toMatrix3d(mTcl.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcl.rowRange(0, 3).col(3)),1.0);
        g2o::Sim3 gScw = gScl * mg2oMergeSlw;
        int numProjMatches = 0;
        vector<MapPoint*> vpMatchedMPs;
        // 当前帧与匹配闭环帧局部map进行匹配，优化，如果最终匹配数量超过一定阈值，认为闭环成立
        bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpMergeMatchedKF, gScw, numProjMatches, mvpMergeMPs, vpMatchedMPs);
        if(bCommonRegion)
        
            //cout << "BoW: Merge reffined Sim3 transformation suscelful" << endl;
            bMergeDetectedInKF = true;

            mnMergeNumCoincidences++;
            mpMergeLastCurrentKF->SetErase();
            mpMergeLastCurrentKF = mpCurrentKF;
            mg2oMergeSlw = gScw;
            mvpMergeMatchedMPs = vpMatchedMPs;

            mbMergeDetected = mnMergeNumCoincidences >= 3;
        
        else
        
            //cout << "BoW: Merge reffined Sim3 transformation failed" << endl;
            mbMergeDetected = false;
            bMergeDetectedInKF = false;

            mnMergeNumNotFound++;
            if(mnMergeNumNotFound >= 2)
            
                /*cout << "+++++++Merge detected failed in KF" << endl;

                for(int i=0; i<mvpMergeLastKF.size(); ++i)
                
                    mvpMergeLastKF[i]->SetErase();
                    mvpMergeCandidateKF[i]->SetErase();
                    mvpMergeLastKF[i]->mbCurrentPlaceRecognition = true;
                

                mvpMergeCandidateKF.clear();
                mvpMergeLastKF.clear();
                mvg2oSim3MergeTcw.clear();
                mvnMergeNumMatches.clear();
                mvvpMergeMapPoints.clear();
                mvvpMergeMatchedMapPoints.clear();*/

                mpMergeLastCurrentKF->SetErase();
                mpMergeMatchedKF->SetErase();
                mnMergeNumCoincidences = 0;
                mvpMergeMatchedMPs.clear();
                mvpMergeMPs.clear();
                mnMergeNumNotFound = 0;
            


        
    

    if(mbMergeDetected || mbLoopDetected)
    
        //f_time_pr << "Geo" << " " << timeGeoKF_ms.count() << endl;
        mpKeyFrameDB->add(mpCurrentKF);
        return true;
    

    //-------------
    //TODO: This is only necessary if we use a minimun score for pick the best candidates
    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    /*float minScore = 1;
    for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
    
        KeyFrame* pKF = vpConnectedKeyFrames[i];
        if(pKF->isBad())
            continue;
        const DBoW2::BowVector &BowVec = pKF->mBowVec;

        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);

        if(score<minScore)
            minScore = score;
    */
    //-------------

    // Extract candidates from the bag of words
    vector<KeyFrame*> vpMergeBowCand, vpLoopBowCand;
    //cout << "LC: bMergeDetectedInKF: " << bMergeDetectedInKF << "   bLoopDetectedInKF: " << bLoopDetectedInKF << endl;
    if(!bMergeDetectedInKF || !bLoopDetectedInKF)
    
        // Search in BoW
        mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3);
    

    // Check the BoW candidates if the geometric candidate list is empty
    //Loop candidates
/*#ifdef COMPILEDWITHC11
    std::chrono::steady_clock::time_point timeStartGeoBoW = std::chrono::steady_clock::now();
#else
    std::chrono::monotonic_clock::time_point timeStartGeoBoW = std::chrono::monotonic_clock::now();
#endif*/

    if(!bLoopDetectedInKF && !vpLoopBowCand.empty())
    
        mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);
    
    // Merge candidates

    //cout << "LC: Find BoW candidates" << endl;

    if(!bMergeDetectedInKF && !vpMergeBowCand.empty())
    
        mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);
    

    //cout << "LC: add to KFDB" << endl;
    mpKeyFrameDB->add(mpCurrentKF);

    if(mbMergeDetected || mbLoopDetected)
    
        return true;
    

    //cout << "LC: erase KF" << endl;
    mpCurrentKF->SetErase();
    mpCurrentKF->mbCurrentPlaceRecognition = false;

    return false;

• 对于DetectAndReffineSim3FromLastKF函数，将当前帧与匹配闭环帧局部map进行匹配，优化，如果最终匹配数量超过一定阈值，认为闭环成立。

bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
                                                 std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)

    set<MapPoint*> spAlreadyMatchedMPs;
    // 提取候选匹配帧的共视帧集合，以及二级共视帧集合，作为一个大map，在当前帧中寻找2d匹配点，返回匹配点数量
    nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
    cout << "REFFINE-SIM3: Projection from last KF with " << nNumProjMatches << " matches" << endl;

    int nProjMatches = 30;
    int nProjOptMatches = 50;
    int nProjMatchesRep = 100;

    // 如果匹配数量超过阈值
    if(nNumProjMatches >= nProjMatches)
    
        cv::Mat mScw = Converter::toCvMat(gScw);
        cv::Mat mTwm = pMatchedKF->GetPoseInverse();
        g2o::Sim3 gSwm(Converter::toMatrix3d(mTwm.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTwm.rowRange(0, 3).col(3)),1.0);
        g2o::Sim3 gScm = gScw * gSwm;
        Eigen::Matrix<double, 7, 7> mHessian7x7;

        bool bFixedScale = mbFixScale;       // TODO CHECK; Solo para el monocular inertial
        if(mpTracker->mSensor==System::IMU_MONOCULAR && !pCurrentKF->GetMap()->GetIniertialBA2())
            bFixedScale=false;
        // 已知匹配点与相似变换，优化匹配
        int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10, bFixedScale, mHessian7x7, true);
        cout << "REFFINE-SIM3: Optimize Sim3 from last KF with " << numOptMatches << " inliers" << endl;

        // 如果匹配数量超过阈值
        if(numOptMatches > nProjOptMatches)
        
            g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
                           Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);

            vector<MapPoint*> vpMatchedMP;
            vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));

            // 提取候选匹配帧的共视帧集合，以及二级共视帧集合，作为一个大map，在当前帧中寻找2d匹配点，返回匹配点数量
            nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw_estimation, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
            //cout << "REFFINE-SIM3: Projection with optimize Sim3 from last KF with " << nNumProjMatches << " matches" << endl;
            if(nNumProjMatches >= nProjMatchesRep)
            
                gScw = gScw_estimation;
                return true;
            
        
    
    return false;

• FindMatchesByProjection函数会提取候选匹配帧的共视帧集合，以及二级共视帧集合，作为一个大map，在当前帧中寻找2d匹配点，返回匹配点数量。

int LoopClosing::FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
                                         set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
                                         vector<MapPoint*> &vpMatchedMapPoints)

    int nNumCovisibles = 5;
    // 排序共视关键帧的前N帧，候选闭环帧的局部邻近帧集合
    vector<KeyFrame*> vpCovKFm = pMatchedKFw->GetBestCovisibilityKeyFrames(nNumCovisibles);
    int nInitialCov = vpCovKFm.size();
    vpCovKFm.push_back(pMatchedKFw);
    // 候选闭环检测集合
    set<KeyFrame*> spCheckKFs(vpCovKFm.begin(), vpCovKFm.end());
    // 当前帧的共视关键帧集合
    set<KeyFrame*> spCurrentCovisbles = pCurrentKF->GetConnectedKeyFrames();
    // 遍历候选闭环帧的共视关键帧集合
    for(int i=0; i<nInitialCov; ++i)
    
        // 选5帧最近的关键帧
        vector<KeyFrame*> vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
        int nInserted = 0;
        int j = 0;
        while(j < vpKFs.size() && nInserted < nNumCovisibles)
        
            // 如果邻近帧不在检查集合中，同时也不在当前帧共视帧集合中，认为可能是候选闭环，加入检测集合
            if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end())
            
                spCheckKFs.insert(vpKFs[j]);
                ++nInserted;
            
            ++j;
        
        // 加入邻近帧
        vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
    
    set<MapPoint*> spMapPoints;
    vpMapPoints.clear();
    vpMatchedMapPoints.clear();
    // 局部候选map，提取MP
    for(KeyFrame* pKFi : vpCovKFm)
    
        for(MapPoint* pMPij : pKFi->GetMapPointMatches())
        
            if(!pMPij || pMPij->isBad())
                continue;

            if(spMapPoints.find(pMPij) == spMapPoints.end())
            
                spMapPoints.insert(pMPij);
                vpMapPoints.push_back(pMPij);
            
        
    
    //cout << "Point cloud: " << vpMapPoints.size() << endl;

    cv::Mat mScw = Converter::toCvMat(g2oScw);

    ORBmatcher matcher(0.9, true);

    vpMatchedMapPoints.resize(pCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
    /**
     * 3d-2d，闭环关键帧与其共视关键帧的MP，投影到当前关键帧中，计算描述子距离，寻找当前帧匹配特征点
     * 1、投影MP到当前关键帧，深度值不得超过估计范围，视差不得超过60°
     * 2、通过深度值估计特征点的层级，搜索框提取fast特征点集合
     * 3、匹配层级上下只允许浮动一层，描述子距离小于阈值即可
     * 4、当前关键帧已有的MP不参与上面的过程
     * @param pKF           当前关键帧
     * @param Scw           当前关键帧位姿
     * @param vpPoints      闭环关键帧与其共视关键帧的地图点
     * @param vpMatched     当前关键帧已经匹配的点
     * @param th            搜索窗大小
     * @param ratioHamming  描述子距离阈值系数
    */
    int num_matches = matcher.SearchByProjection(pCurrentKF, mScw, vpMapPoints, vpMatchedMapPoints, 3, 1.5);

    return num_matches;

由于LoopClosing的代码较多，这几篇博客都会是与其相关的，之后会继续分析。