ORBSLAM2源码学习（9） LoopClosing类

最新推荐文章于 2023-02-05 21:45:35 发布
最新推荐文章于 2023-02-05 21:45:35 发布
阅读量1k 收藏 1
点赞数 1
分类专栏： SLAM研究 文章标签： SLAM
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/u012936940/article/details/81814018
版权

该类负责闭环的检测和校正，逻辑关系比较简明

while(1){
        // Check if there are keyframes in the queue
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空
        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }

        ResetIfRequested();

        if(CheckFinish())
            break;

        //usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

检测到闭环后就进行Sim3的计算，再进行闭环校正，之后创建一个线程进行全局BA

#include "LoopClosing.h"
#include "Sim3Solver.h"
#include "Converter.h"
#include "Optimizer.h"
#include "ORBmatcher.h"
#include<mutex>
#include<thread>

namespace ORB_SLAM2
{
LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale):
    mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
    mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true),
    mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0)
{
    mnCovisibilityConsistencyTh = 3;
}

void LoopClosing::SetTracker(Tracking *pTracker)
{
    mpTracker=pTracker;
}

void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
{
    mpLocalMapper=pLocalMapper;
}

void LoopClosing::Run()
{
    mbFinished =false;

    while(1)
    {
        // Check if there are keyframes in the queue
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空
        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }

        ResetIfRequested();

        if(CheckFinish())
            break;

        //usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));
	}

    SetFinish();
}

void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    if(pKF->mnId!=0)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

// 查看列表中是否有等待被插入的关键帧
bool LoopClosing::CheckNewKeyFrames()
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    return(!mlpLoopKeyFrameQueue.empty());
}

bool LoopClosing::DetectLoop()
{
    {
        // 取出一个关键帧
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        // Avoid that a keyframe can be erased while it is being process by this thread
        mpCurrentKF->SetNotErase();
    }

    //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
    // 如果距离上次闭环小于10帧，则不进行闭环检测
    if(mpCurrentKF->mnId<mLastLoopKFid+10)
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    // Compute reference BoW similarity score
    // This is the lowest score to a connected keyframe in the covisibility graph
    // We will impose loop candidates to have a higher similarity than this
    // 遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，得到最低得分minScore
    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;
    }

    // Query the database imposing the minimum score
    // 在所有关键帧中找出闭环备选帧,去除当前帧的共视帧
    vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

    // If there are no loop candidates, just add new keyframe and return false
    if(vpCandidateKFs.empty())
    {
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroups.clear();
        mpCurrentKF->SetErase();
        return false;
    }

    // For each loop candidate check consistency with previous loop candidates
    // Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
    // A group is consistent with a previous group if they share at least a keyframe
    // We must detect a consistent loop in several consecutive keyframes to accept it
    // 在候选帧中检测具有连续性的候选帧
    // 每个候选帧将与自己相连的关键帧构成一个“候选组spCandidateGroup”
    // 检测“候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，将该“候选组”放入“当前连续组vCurrentConsistentGroups”
    // 如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
    mvpEnoughConsistentCandidates.clear();	// 最终筛选后得到的闭环帧

    vector<ConsistentGroup> vCurrentConsistentGroups;
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
    for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
    {
        KeyFrame* pCandidateKF = vpCandidateKFs[i];

        // 将自己以及与自己相连的关键帧构成一个“候选组”
        set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);

        bool bEnoughConsistent = false;
        bool bConsistentForSomeGroup = false;
        // 遍历之前的“连续组”
        for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
        {
            // 取出一个之前的连续组
            set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;

            // 遍历每个“候选组”，检测候选组中每一个关键帧在“连续组”中是否存在
            bool bConsistent = false;
            for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
            {
                if(sPreviousGroup.count(*sit))
                {
                    bConsistent=true;
                    bConsistentForSomeGroup=true;// 该“候选组”至少与一个”连续组“相连
                    break;
                }
            }

            if(bConsistent)
            {
                int nPreviousConsistency = mvConsistentGroups[iG].second;
                int nCurrentConsistency = nPreviousConsistency + 1;
                if(!vbConsistentGroup[iG])
                {
                    ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
                    vCurrentConsistentGroups.push_back(cg);
                    vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
                }
                if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
                {
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    bEnoughConsistent=true; //this avoid to insert the same candidate more than once
                }
            }
        }

        // If the group is not consistent with any previous group insert with consistency counter set to zero
        // 如果该“候选组”的所有关键帧都不存在于“连续组”，那么vCurrentConsistentGroups将为空，
        // 于是就把“子候选组”全部拷贝到vCurrentConsistentGroups，并最终用于更新mvConsistentGroups，计数设为0，重新开始
        if(!bConsistentForSomeGroup)
        {
            ConsistentGroup cg = make_pair(spCandidateGroup,0);
            vCurrentConsistentGroups.push_back(cg);
        }
    }

    // Update Covisibility Consistent Groups
    mvConsistentGroups = vCurrentConsistentGroups;
    // Add Current Keyframe to database
    mpKeyFrameDB->add(mpCurrentKF);

    if(mvpEnoughConsistentCandidates.empty())
    {
        mpCurrentKF->SetErase();
        return false;
    }
    else
    {
        return true;
    }
    mpCurrentKF->SetErase();
    return false;
}

// 计算当前帧与闭环帧的Sim3变换
// 通过Bow匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3
// 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3
// 将闭环帧以及闭环帧相连的关键帧的MapPoints与当前帧的点进行匹配
bool LoopClosing::ComputeSim3()
{
    // For each consistent loop candidate we try to compute a Sim3
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();

    // We compute first ORB matches for each candidate
    // If enough matches are found, we setup a Sim3Solver
    ORBmatcher matcher(0.75,true);

    vector<Sim3Solver*> vpSim3Solvers;
    vpSim3Solvers.resize(nInitialCandidates);	// 每个候选帧都有一个Sim3Solver

    vector<vector<MapPoint*> > vvpMapPointMatches;
    vvpMapPointMatches.resize(nInitialCandidates);

    vector<bool> vbDiscarded;
    vbDiscarded.resize(nInitialCandidates);

    int nCandidates=0; //candidates with enough matches

    for(int i=0; i<nInitialCandidates; i++)
    {
        // 闭环候选帧中取出一帧关键帧pKF
        KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

        // avoid that local mapping erase it while it is being processed in this thread
        // 防止在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
        pKF->SetNotErase();

        if(pKF->isBad())
        {
            vbDiscarded[i] = true;// 直接舍弃
            continue;
        }

        // 将当前帧mpCurrentKF与闭环候选关键帧pKF匹配
        // 通过bow加速得到mpCurrentKF与pKF之间的匹配特征点，vvpMapPointMatches是匹配特征点对应的MapPoints
        int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

        // 匹配的特征点数太少，剔除
        if(nmatches<20)
        {
            vbDiscarded[i] = true;
            continue;
        }
        else
        {
            // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono)
            // 构造Sim3求解器
            // 如果mbFixScale为true，则是6DoFf优化，如果是false，则是7DoF优化（单目）
            Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
            pSolver->SetRansacParameters(0.99,20,300);
            vpSim3Solvers[i] = pSolver;
        }
        // 参与Sim3计算的候选关键帧数加1
        nCandidates++;
    }

    bool bMatch = false;	// 标记是否有一个候选帧通过Sim3的求解

    // Perform alternatively RANSAC iterations for each candidate
    // until one is succesful or all fail
    // 一直循环所有的候选帧，每个候选帧迭代5次，如果5次迭代后得不到结果，就换下一个候选帧
    // 直到有一个候选帧首次迭代成功，或者某个候选帧总的迭代次数超过限制，直接将它剔除
    while(nCandidates>0 && !bMatch)
    {
        for(int i=0; i<nInitialCandidates; i++)
        {
            if(vbDiscarded[i])
                continue;

            KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

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

            // 对有较好的匹配的关键帧求取Sim3变换
            Sim3Solver* pSolver = vpSim3Solvers[i];
            cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

            // If Ransac reachs max. iterations discard keyframe
            // 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

            // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
            if(!Scm.empty())
            {
                vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
                for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
                {
                    // 保存inlier的MapPoint
                    if(vbInliers[j])
                       vpMapPointMatches[j]=vvpMapPointMatches[i][j];
                }

                // 通过步骤3求取的Sim3变换引导关键帧匹配弥补步骤2中的漏匹配
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();
                // 查找更多的匹配 使用SearchByBoW进行特征点匹配时会有漏匹配
                // 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
                // 在该区域内通过描述子进行匹配pKF1和pKF2之前漏匹配的特征点，更新匹配vpMapPointMatches
                matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

                // Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
                g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
                // 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

                // If optimization is succesful stop ransacs and continue
                if(nInliers>=20)
                {
                    bMatch = true;
                    // mpMatchedKF是最终闭环检测出来与当前帧形成闭环的关键帧
                    mpMatchedKF = pKF;
                    // 得到从世界坐标系到该候选帧的Sim3变换
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
                    // 得到优化后从世界坐标系到当前帧的Sim3变换
                    mg2oScw = gScm*gSmw;
                    mScw = Converter::toCvMat(mg2oScw);

                    mvpCurrentMatchedPoints = vpMapPointMatches;
                    break;//跳出对其它候选帧的判断
                }
            }
        }
    }

    // 没有一个闭环匹配候选帧通过Sim3的求解与优化
    if(!bMatch)
    {
        for(int i=0; i<nInitialCandidates; i++)
             mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }

    // Retrieve MapPoints seen in Loop Keyframe and neighbors
    // 取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入mvpLoopMapPoints
    // 将mpMatchedKF相连的关键帧全部取出来放入vpLoopConnectedKFs
    vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    mvpLoopMapPoints.clear();
    for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
    {
        KeyFrame* pKF = *vit;
        vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
        for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
        {
            MapPoint* pMP = vpMapPoints[i];
            if(pMP)
            {
                if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
                {
                    mvpLoopMapPoints.push_back(pMP);
                    pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                }
            }
        }
    }

    // Find more matches projecting with the computed Sim3
    // 将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
    // 根据投影查找更多的匹配
    // 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，
    // 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新mvpCurrentMatchedPoints
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

    // If enough matches accept Loop
    // 断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
    int nTotalMatches = 0;
    for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
    {
        if(mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }

    if(nTotalMatches>=40)
    {
        for(int i=0; i<nInitialCandidates; i++)
            if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                mvpEnoughConsistentCandidates[i]->SetErase();
        return true;
    }
    else
    {
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
}


// 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置
// 将闭环帧以及闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配
// 通过MapPoints的匹配关系更新这些帧之间的连接关系
// 对Essential Graph进行优化，MapPoints的位置做相应的调整
// 全局Bundle Adjustment
void LoopClosing::CorrectLoop()
{
    cout << "Loop detected!" << endl;

    // Send a stop signal to Local Mapping
    // Avoid new keyframes are inserted while correcting the loop
    //请求局部地图停止
    mpLocalMapper->RequestStop();

    // If a Global Bundle Adjustment is running, abort it
    if(isRunningGBA())
    {
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;

        mnFullBAIdx++;

        if(mpThreadGBA)
        {
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
    }

    // Wait until Local Mapping has effectively stopped
    while(!mpLocalMapper->isStopped())
    {
        //usleep(1000);
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    // Ensure current keyframe is updated
    // 根据共视关系更新当前帧与其它关键帧之间的连接
    mpCurrentKF->UpdateConnections();

    // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
    // 得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
    // 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换
    // 取出与当前帧相连的关键帧，包括当前关键帧
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);

    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
    CorrectedSim3[mpCurrentKF]=mg2oScw;
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();

    {
        // Get Map Mutex
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

        // 得到Sim3调整后其它与当前帧相连关键帧的位姿
        for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKFi = *vit;

            cv::Mat Tiw = pKFi->GetPose();

            if(pKFi!=mpCurrentKF)
            {
                // 得到当前帧到pKFi帧的相对变换
                cv::Mat Tic = Tiw*Twc;
                cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
                cv::Mat tic = Tic.rowRange(0,3).col(3);
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
                // 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
                g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
                // Pose corrected with the Sim3 of the loop closure
                // 得到闭环g2o优化后各个关键帧的位姿
                CorrectedSim3[pKFi]=g2oCorrectedSiw;
            }

            cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
            cv::Mat tiw = Tiw.rowRange(0,3).col(3);
            g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
            // Pose without correction
            // 当前帧相连关键帧，没有进行闭环优化的位姿
            NonCorrectedSim3[pKFi]=g2oSiw;
        }

        // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
        // 上一步得到调整相连帧位姿后，修正这些关键帧的地图点
        for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
        {
            KeyFrame* pKFi = mit->first;
            g2o::Sim3 g2oCorrectedSiw = mit->second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();

            g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];

            vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
            for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
            {
                MapPoint* pMPi = vpMPsi[iMP];
                if(!pMPi)
                    continue;
                if(pMPi->isBad())
                    continue;
                if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) // 防止重复修正
                    continue;

                // Project with non-corrected pose and project back with corrected pose
                cv::Mat P3Dw = pMPi->GetWorldPos();
                Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
                Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));

                cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                pMPi->SetWorldPos(cvCorrectedP3Dw);
                pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                pMPi->mnCorrectedReference = pKFi->mnId;
                pMPi->UpdateNormalAndDepth();
            }

            // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
            Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
            Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
            double s = g2oCorrectedSiw.scale();

            eigt *=(1./s); //[R t/s;0 1]

            cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);

            pKFi->SetPose(correctedTiw);

            // Make sure connections are updated
            pKFi->UpdateConnections();
        }

        // Start Loop Fusion
        // Update matched map points and replace if duplicated
        // 检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
        for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
        {
            if(mvpCurrentMatchedPoints[i])
            {
                MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
                MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
                if(pCurMP)	// 如果有重复的MapPoint，则用匹配帧的代替现有的
                    pCurMP->Replace(pLoopMP);
                else	// 如果当前帧没有该MapPoint，则直接添加
                {
                    mpCurrentKF->AddMapPoint(pLoopMP,i);
                    pLoopMP->AddObservation(mpCurrentKF,i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        }

    }

    // Project MapPoints observed in the neighborhood of the loop keyframe
    // into the current keyframe and neighbors using corrected poses.
    // Fuse duplications.
    // 通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
    SearchAndFuse(CorrectedSim3);

    // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
    // 更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
    map<KeyFrame*, set<KeyFrame*> > LoopConnections;

    // 遍历当前帧相连关键帧
    for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        // 得到与当前帧相连关键帧的相连关键帧（二级相连）
        vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();

        // Update connections. Detect new links.
        // 更新一级相连关键帧的连接关系
        pKFi->UpdateConnections();
        // 取出该帧更新后的连接关系
        LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
        // 从连接关系中去除闭环之前的二级连接关系，剩下的是由闭环得到的连接关系
        for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
        {
            LoopConnections[pKFi].erase(*vit_prev);
        }
        // 从连接关系中去除闭环之前的一级连接关系，剩下的是由闭环得到的连接关系
        for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
        {
            LoopConnections[pKFi].erase(*vit2);
        }
    }

    // Optimize graph
    // 进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

    // Add loop edge
    // 添加当前帧与闭环匹配帧之间的边
    mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);

    // Launch a new thread to perform Global Bundle Adjustment
    // 新建一个线程用于全局BA优化
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);

    // Loop closed. Release Local Mapping.
    mpLocalMapper->Release();    

    cout << "Loop Closed!" << endl;

    mLastLoopKFid = mpCurrentKF->mnId;
}

// 通过将闭环时相连关键帧的MapPoints投影到这些关键帧中，进行MapPoints检查与替换
void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
{
    ORBmatcher matcher(0.8);

    // 遍历闭环相连的关键帧
    for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
    {
        KeyFrame* pKF = mit->first;

        g2o::Sim3 g2oScw = mit->second;
        cv::Mat cvScw = Converter::toCvMat(g2oScw);

        // 将闭环相连帧的MapPoints坐标变换到pKF帧坐标系，然后投影，检查冲突并融合
        vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));
        matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);

        // Get Map Mutex
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
        const int nLP = mvpLoopMapPoints.size();
        for(int i=0; i<nLP;i++)
        {
            MapPoint* pRep = vpReplacePoints[i];
            if(pRep)
            {
                pRep->Replace(mvpLoopMapPoints[i]);
            }
        }
    }
}


void LoopClosing::RequestReset()
{
    {
        unique_lock<mutex> lock(mMutexReset);
        mbResetRequested = true;
    }

    while(1)
    {
        {
        unique_lock<mutex> lock2(mMutexReset);
        if(!mbResetRequested)
            break;
        }
		//usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));

    }
}

void LoopClosing::ResetIfRequested()
{
    unique_lock<mutex> lock(mMutexReset);
    if(mbResetRequested)
    {
        mlpLoopKeyFrameQueue.clear();
        mLastLoopKFid=0;
        mbResetRequested=false;
    }
}

void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
{
    cout << "Starting Global Bundle Adjustment" << endl;

    int idx =  mnFullBAIdx;
    Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false);

    // Update all MapPoints and KeyFrames
    // Local Mapping was active during BA, that means that there might be new keyframes
    // not included in the Global BA and they are not consistent with the updated map.
    // We need to propagate the correction through the spanning tree
    {
        unique_lock<mutex> lock(mMutexGBA);
        if(idx!=mnFullBAIdx)
            return;

        if(!mbStopGBA)
        {
            cout << "Global Bundle Adjustment finished" << endl;
            cout << "Updating map ..." << endl;
            mpLocalMapper->RequestStop();
            // Wait until Local Mapping has effectively stopped

            while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
            {
				//usleep(1000);
				std::this_thread::sleep_for(std::chrono::milliseconds(1));
			}

            // Get Map Mutex
            unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

            // Correct keyframes starting at map first keyframe
            list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());

            while(!lpKFtoCheck.empty())
            {
                KeyFrame* pKF = lpKFtoCheck.front();
                const set<KeyFrame*> sChilds = pKF->GetChilds();
                cv::Mat Twc = pKF->GetPoseInverse();
                for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
                {
                    KeyFrame* pChild = *sit;
                    if(pChild->mnBAGlobalForKF!=nLoopKF)
                    {
                        cv::Mat Tchildc = pChild->GetPose()*Twc;
                        pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;
                        pChild->mnBAGlobalForKF=nLoopKF;

                    }
                    lpKFtoCheck.push_back(pChild);
                }

                pKF->mTcwBefGBA = pKF->GetPose();
                pKF->SetPose(pKF->mTcwGBA);
                lpKFtoCheck.pop_front();
            }

            // Correct MapPoints
            const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();

            for(size_t i=0; i<vpMPs.size(); i++)
            {
                MapPoint* pMP = vpMPs[i];

                if(pMP->isBad())
                    continue;

                if(pMP->mnBAGlobalForKF==nLoopKF)
                {
                    // If optimized by Global BA, just update
                    pMP->SetWorldPos(pMP->mPosGBA);
                }
                else
                {
                    // Update according to the correction of its reference keyframe
                    KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();

                    if(pRefKF->mnBAGlobalForKF!=nLoopKF)
                        continue;

                    // Map to non-corrected camera
                    cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
                    cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
                    cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;

                    // Backproject using corrected camera
                    cv::Mat Twc = pRefKF->GetPoseInverse();
                    cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
                    cv::Mat twc = Twc.rowRange(0,3).col(3);

                    pMP->SetWorldPos(Rwc*Xc+twc);
                }
            }

            mpLocalMapper->Release();

            cout << "Map updated!" << endl;
        }

        mbFinishedGBA = true;
        mbRunningGBA = false;
    }
}

void LoopClosing::RequestFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinishRequested = true;
}

bool LoopClosing::CheckFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinishRequested;
}

void LoopClosing::SetFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinished = true;
}

bool LoopClosing::isFinished()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinished;
}
} //namespace ORB_SLAM
  1. CheckNewKeyFrames() 检查关键帧队列是否为空
  • 检查的变量为mlpLoopKeyFrameQueue，该变量中的关键帧是在LocalMapping中发送过来的，mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

     2.DetectLoop() 闭环检测

  • 从队列中取出最前一个关键帧，设置为mpCurrentKF
  • 计算mpCurrentKF和其共视关键帧的BoW得分，记录最低得分minScore
  • 在关键帧数据库中找出闭环候选帧，vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);注意这里的候选帧要去掉当前帧的共视帧，接下来开始筛选闭环候选帧，遍历每个闭环候选帧pCandidateKF：
  • 将和pCandidateKF相连的关键帧及其自身构成一个候选组spCandidateGroup，取出之前的子连续组，遍历当前候选组中的关键帧，如果有一帧共同存在于当前候选组和之前的子连续组中，则认为候选组至少和之前的一个子连续组相连，nCurrentConsistency++
  • nCurrentConsistency++大于等于3的话，该候选组通过检测，插入mvpEnoughConsistentCandidates中，为筛选得到的闭环帧

     3.检测到闭环帧后，ComputeSim3()

  • 给每个闭环帧设置一个Sim3Solver，对于每个闭环帧pKF：
  • 通过SearchByBoW将当前帧mpCurrentKF和pKF的特征点进行匹配，若匹配的点太少，则直接剔除该闭环帧，否则构造Sim3求解器
  • 对每一个构造了求解器的闭环帧，当还没有闭环帧通过Sim3的求解时：
  • RANSAC求解相似变换，若迭代次数达到最大还未求出好的解，则剔除该闭环帧
  • 若得到了解，则通过SearchBySim3函数搜索匹配点(mpCurrentKF和pKF之间)，弥补之前漏掉的匹配点，之后利用重新搜索匹配后的点优化上一步得到的Sim3，若优化后内点较多，则记录相关变量，直接退出大循环，停止对其他闭环帧的判断
  • 上一步闭环匹配上的帧为mpMatchedKF，将其全部共视帧及自身一起加入vpLoopConnectedKFs中，并将这些帧的地图点取出加入mvpLoopMapPoints中，mvpLoopMapPoints将用于后续的地图点融合
  • 通过SearchByProjection函数将mvpLoopMapPoints投影至当前帧进行匹配，根据尺度确定搜索范围，匹配结果放入mvpCurrentMatchedPoints中
  • 若有足够的匹配点返回true，否则false

     4.CorrectLoop() 闭环校正

  • 局部图停止，更新共视关系
  • 取出当前帧的共视帧，由于当前帧的Sim3已经计算得出，因此可以计算出共视帧的Sim3位姿，即调整后的位姿，将其保存，同时将优化前的位姿也保存
  • 将相邻关键帧的地图点根据更新后的相机位姿重新计算地图点坐标
  • 根据共视关系更新图之间的连接；进行地图点的融合，如果有重复的地图点，则用匹配帧的点代替现有的，否则直接添加
  • 运动全局BA对整个地图进行优化

 

Rap_God
关注
  • 1
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
ISCC2021 美人计
半岛铁盒的博客
05-26 3087
一张图,一个文档 图片进入winhex 是AES,秘钥是 ISCC2021 文档里面的二维码扫描 U2FsdGVkX1/Ka+sScszwQkwhO+VLiJwV/6IFg5W+TfNHGxG2qZsIr2iwMwb9X9Iu 3GuGWmPOtO27z8vNppD2D50fwsD+8VWhdtW9J4cewYivH/Z/7GoUvcJXJMrvf+vu +CBqWDGp6HWd0e5whGhuzlK0ZtBcDZdPDSIHA7+GuUlifp8PcFCtJPgiuk143REE +pKFiSJXo.
ORB-SLAM2 --- ORBmatcher::SearchByProjection函数
qq_41694024的博客
12-19 828
ORB-SLAM2 --- ORBmatcher::SearchByProjection函数解析
详谈ORB-SLAM2的回环线程LoopClosing
极客园地
02-05 1050
ORB-SLAM2的回环线程LoopClosing分为2个步骤，非常简单。回环线程第一步是检测回环，就是因为局部建图线程使用关键帧结束后，把关键帧传给回环检测线程，所以两个之间也是有缓冲队列的。第二步就是闭环校正。回环检测的主函数也是一个`无限循环`，不断检测是否出现新关键帧，缓冲队列中检测新的关键帧是否出现闭环，如果形成闭环，那就计算闭环关键帧与闭环关键帧参考的对应场景，两帧之间的匹配关系，做一个相似变换。实际上对于双目相机来说是一个刚体变换，单目相机运行时间久导致基线消失，尺寸会漂移，所以中间会产生一个
ORB-SLAM2中LoopClosing线程主要流程梳理
03-25 944
LoopClosing线程主要工作是两个：一个是对当前新到来的关键帧进行闭环检测，看看之前的关键帧中哪个和当前关键帧形成闭环；另一个是纠正闭环，就是在检测到闭环的时候要将当前关键帧和闭环关键帧的位姿拉到一起，形象点说就是进行融合（既然都存在闭环了，那肯定得合并啊）。 一、闭环检测DetectLoop 1.从关键帧列表中获取列表头的关键帧，也就是获取最新插入到LoopClosing线程的关键帧，...
ORB-SLAM2从理论到代码实现（九）：LocalMapping程序详解
人无远虑，必有近忧
09-18 698
LocalMapping线程负责对新加入的KeyFrames和MapPoints筛选融合，剔除冗余的KeyFrames和MapPoints，维护稳定的KeyFrame集合，传给后续的LoopClosing线程。 主要的功能点在： 处理新的关键帧ProcessNewKeyFrame() 剔除不合格地图点MapPointCulling() 三角化恢复新地图点CreateNewMapPoints() 融合当前帧与相邻帧重复的地图点SearchInNeighbors() 局
ORBSLAM2源码（附带详细注释）
12-26
ORBSLAM2源码详细注释
ORB_SLAM2源码资源包
04-17
该资源包是提取和跟踪的特征点使用ORB。ORB特征的提取过程非常快，适合用于实时性强的系统。该资源包没有进行编译，而是自己需要手动编译的！
ORB_SLAM2源码工作空间（带位姿发布功能）
04-22
此资源包里包括ORB_SLAM2算法源码以及发布位姿信息所需要的文件，以及对应的文件正确的放置位置。仅供参考。
ORB SLAM2中文源码解释
03-01
ORB SLAM2中文源码解释
ORB-SLAM2源码中文详解.pdf
08-17
东北大学吴博讲解ORB_SLAM的文档，仅有文档。
【特征匹配】ORB原理与源码解析
Min220的博客
03-25 904
主要转载自http://blog.csdn.net/luoshixian099/article/details/48523267，本博也会陆续有部分修改       转载请注明出处： http://blog.csdn.net/luoshixian099/article/details/48523267 CSDN-勿在浮沙筑高台       为了满足实时性的要求，前面文章中介绍过快速...
ORB-SLAM2学习笔记——闭环检测DetectLoop
weixin_51326570的博客
04-16 780
ORB-SLAM2学习笔记——闭环检测DetectLoop 1、理论部分 2、代码详解 #include "KeyFrameDatabase.h" #include "KeyFrame.h" #include "Thirdparty/DBoW2/DBoW2/BowVector.h" #include<mutex> using namespace std; namespace ORB_SLAM2 { KeyFrameDatabase::KeyFrameDatabase (const O
ORB-SLAM2的源码阅读（十一）：LoopClosing类
Felaim的博客
03-05 1684
第一次过ORB_SLAM2的代码，其实还有很多算法细节还是没有弄明白，接下来的任务是弄明白具体算法并对其增加部分小功能O(∩_∩)O哈哈~ #ifndef LOOPCLOSING_H #define LOOPCLOSING_H #include "KeyFrame.h" #include "LocalMapping.h" #include "Map.h" #include "ORBVocabu...
orbslam2学习笔记 -- loopclosing 流程梳理
michaelhan3的博客
05-06 1717
读之前提醒：首先，该文章是按照时间顺序讲述，并且不提前讲各种类关系和数据结构关系，而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的，然后告诉你这个东西是啥。如果你还没有读过tracking的部分，请先读tracking，因为很多概念会在tracking里先讲出来，后面再遇到，就不会细讲了。 tracking :https://blog.csdn.ne...
ORB_SLAM2代码阅读(4)——LoopClosing线程
Mr.Andy的博客
06-04 3119
ORB_SLAM2代码阅读（4）——LoopClosing线程1.说明2.简介3.检测回环 1.说明 在前几篇博客介绍完tracking和localmaping线程后，本文接着介绍闭环检测（LoopClosing）线程中的内容。该部分内容中有很多细节部分还没有弄清楚，暂时先将整体思路捋顺，更多细节内容以后再补充。 2.简介 LoopClosing线程整体思路比较简介清晰，主要分为三大部分：检测回环...
ORB-SLAM2代码整理--LoopClosing线程
xiaoshuiyisheng的博客
08-20 1075
找到线程的Run函数 void LoopClosing::Run() { mbFinished =false; while(1) { // Check if there are keyframes in the queue // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracki...
一起学ORBSLAM2（8）ORBSLAM的loopclosing
蚁族的坚持的博客
06-05 2443
ORBSLAM中的回环检测和重定位具有类似的方式，所以我们这里将两个部分放到同一篇文章中讲解。首先看tracking线程中的重定位。一．Tracking重定位重定位模式是在系统处于LOST（追踪线程失败）的情况下进行的系统挽救工作，它的基本流程是：（1）计算当前帧的BoW映射（2）找到与当前帧相似的候选关键帧（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw（RANSAC+EPNP算法）（4）...
ORBSLAM2学习笔记6(LoopClosing)
Bobsweetie的专栏
07-08 712
LoopClosing.h /** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza) * For more information see <https://github.com/raulmur/ORB_SLAM2> * * ORB-SLAM2 is free software:
ORB-SLAM2代码阅读笔记（十）：sim3求解
热门推荐
06-22 1万+
Table of Contents 1.sim3的简单概述 2.sim3算法介绍 1)3对匹配的3D点建立坐标系 2)旋转矩阵计算 3）平移向量计算 4）尺度计算 3.代码解析 1）sim3求解器构造函数Sim3Solver 2）sim3迭代求解Sim3Solver::iterate 本篇笔记对 ORB-SLAM2代码阅读笔记（八）：LoopClosing线程 中提到的s...
给我讲一下ORBSLAM2源码
最新发布
03-31
以下是ORB-SLAM2源码的主要组成部分： 1.ORB特征提取和描述 ORB-SLAM2使用基于FAST角点检测器和ORB特征描述符的方法来提取和描述图像中的特征点。它还提供了RANSAC算法来剔除错误匹配的特征点。 2.相机姿态估计 ...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值