ORB-SLAM2代码详解09: 闭环线程LoopClosing

已于 2023-10-26 21:08:44 修改
阅读量4.1k 收藏 35
点赞数 9
分类专栏： SLAM 文章标签： ORB-SLAM2
于 2021-05-14 11:44:20 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/ncepu_Chen/article/details/116785474
版权

pdf版本笔记的下载地址: ORB-SLAM2代码详解09_闭环线程LoopClosing,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

ORB-SLAM2代码详解09: 闭环线程LoopClosing

可以看看我录制的视频5小时让你假装大概看懂ORB-SLAM2源码

请添加图片描述

各成员函数/变量

闭环主函数: Run()

请添加图片描述

void LoopClosing::Run() {
    while (1) {
        if (CheckNewKeyFrames()) {
            if (DetectLoop()) {
                if (ComputeSim3()) {
                    CorrectLoop();
                }
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

闭环检测: DetectLoop()

请添加图片描述

LoopClosing类中定义类型ConsistentGroup,表示关键帧组.

typedef pair<set<KeyFrame *>, int> ConsistentGroup
  • 第一个元素表示一组共视关键帧.
  • 第二个元素表示该关键帧组的连续长度.

所谓连续,指的是两个关键帧组中存在相同的关键帧.

成员函数/变量访问控制意义
KeyFrame *mpCurrentKFprotected当前关键帧
KeyFrame *mpMatchedKFprotected当前关键帧的闭环匹配关键帧
std::vector<ConsistentGroup> mvConsistentGroupsprotected前一关键帧的闭环候选关键帧组
vCurrentConsistentGroups局部变量当前关键帧的闭环候选关键帧组
std::vector<KeyFrame *> mvpEnoughConsistentCandidatesprotected所有达到足够连续数的关键帧

闭环检测原理: 若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,且这些闭环匹配关键帧组间是连续的,则认为实现闭环,

请添加图片描述

具体来说,回环检测过程如下:

  1. 找到当前关键帧的闭环候选关键帧vpCandidateKFs:

    闭环候选关键帧取自于与当前关键帧具有相同的BOW向量但不存在直接连接的关键帧.

    请添加图片描述

  2. 将闭环候选关键帧和其共视关键帧组合成为关键帧组vCurrentConsistentGroups:

    请添加图片描述

  3. 在当前关键组和之前的连续关键组间寻找连续关系.

    • 若当前关键帧组在之前的连续关键帧组中找到连续关系,则当前的连续关键帧组的连续长度加1.
    • 若当前关键帧组在之前的连续关键帧组中没能找到连续关系,则当前关键帧组的连续长度为0.

    关键帧组的连续关系是指两个关键帧组间是否有关键帧同时存在于两关键帧组中.

    请添加图片描述

    若某关键帧组的连续长度达到3,则认为该关键帧实现闭环.

bool LoopClosing::DetectLoop() {
	// step1. 取出缓冲队列头部的关键帧,作为当前检测闭环关键帧,设置其不被优化删除
    {
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        mpCurrentKF->SetNotErase();
    }

    // step2. 距离上次闭环时间太短,不再检测闭环
    if (mpCurrentKF->mnId < mLastLoopKFid + 10) {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

	// step3. 计算当前关键帧与共视关键帧间最大相似度
    const vector<KeyFrame *> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    float minScore = 1;
    for (KeyFrame *pKF : vpConnectedKeyFrames) {
        const DBoW2::BowVector &BowVec = pKF->mBowVec;
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
        if (score < minScore)
            minScore = score;
    }

    // step4. 寻找当前关键帧的闭环候选关键帧
    vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
    if (vpCandidateKFs.empty()) {
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroups.clear();
        mpCurrentKF->SetErase();
        return false;
    }
     
    // step5. 在当前关键帧组和之前的连续关键帧组之间寻找匹配
    mvpEnoughConsistentCandidates.clear();
    vector<ConsistentGroup> vCurrentConsistentGroups;
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(), false);	// 之前的连续关键帧组在当前关键帧组中是否存在连续

    // 遍历当前闭环候选关键帧
    for (KeyFrame *pCandidateKF : vpCandidateKFs) {
		// step5.1. 构建关键帧组,包括候选关键帧及其共视关键帧
        set<KeyFrame *> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);

		// step5.2. 遍历之前的连续关键帧组,寻找连续关系        
        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;
            // step5.2. 若当前连续关键帧组中某关键帧也在前一帧的候选关键帧组中,则找到了连续关系
            for (KeyFrame * previousKeyFrame : spCandidateGroup.begin()) {
                if (sPreviousGroup.count(previousKeyFrame)) {
                    bConsistent = true;
                    bConsistentForSomeGroup = true;
                    break;
                }
            }

            // step5.3. 更新当前关键帧组的连续次数
            if (bConsistent) {
                int nCurrentConsistency = mvConsistentGroups[iG].second + 1;
                if (!vbConsistentGroup[iG]) {
                    ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency);
                    vCurrentConsistentGroups.push_back(cg);
                    vbConsistentGroup[iG] = true;
                }
                // 若当前关键帧组的连续次数达到3,则完成闭环,将其加入到mvpEnoughConsistentCandidates中
                if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) {
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    bEnoughConsistent = true;
                }
            }
        }
		
        // 5.4. 若当前关键帧组在前一关键帧的闭环候选关键帧组中找不到连续关系,则将两虚次数置零
        if (!bConsistentForSomeGroup) {
            ConsistentGroup cg = make_pair(spCandidateGroup, 0);
            vCurrentConsistentGroups.push_back(cg);
        }
    }
    
    // step6. 维护循环变量
    mvConsistentGroups = vCurrentConsistentGroups;		// 更新连续关键帧组
    mpKeyFrameDB->add(mpCurrentKF);						// 将当前关键帧加入到关键帧数据库中

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

当前关键帧的闭环候选关键帧取自于与当前关键帧具有相同BOW向量但不直接相连的关键帧.

// 寻找当前关键帧的闭环候选关键帧
vector<KeyFrame *> KeyFrameDatabase::DetectLoopCandidates(KeyFrame *pKF, float minScore) {
    
    // step1. 找出当前关键帧的所有共视关键帧
    set<KeyFrame *> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();


    // step2. 找出所有具有相同BOW但不直接相连的关键帧
    list<KeyFrame *> lKFsSharingWords;		// 存储闭环候选关键帧
    {
        unique_lock<mutex> lock(mMutex);
		// 遍历所有BOW词向量
        for (DBoW2::BowVector vit : pKF) {
            // 遍历所有含有该词向量的关键帧
            for (KeyFrame *pKFi : mvInvertedFile[vit.first]) {
                if (pKFi->mnLoopQuery != pKF->mnId) {
                    pKFi->mnLoopWords = 0;
                    // 若该关键帧与当前关键帧不直接相连,才能作为闭环候选
                    if (!spConnectedKeyFrames.count(pKFi)) {
                        pKFi->mnLoopQuery = pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnLoopWords++;
            }
        }
    }

    // step3. 以最大相似度的0.8倍为阈值筛选筛选候选关键帧
    int maxCommonWords = 0;
    list<pair<float, KeyFrame *> > lScoreAndMatch;
    for (KeyFrame *pKFi : lKFsSharingWords) {
        if (*pKFi->mnLoopWords > maxCommonWords)
            maxCommonWords = *pKFi->mnLoopWords;
    }
    int minCommonWords = maxCommonWords * 0.8f;
    for (KeyFrame *pKFi : lKFsSharingWords) {
        if (pKFi->mnLoopWords > minCommonWords) {
            float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec);
            pKFi->mLoopScore = si;
            if (si >= minScore)
                lScoreAndMatch.push_back(make_pair(si, pKFi));
        }
    }

    // step4. 统计候选关键帧的共视关键帧组的相似度得分
    list<pair<float, KeyFrame *> > lAccScoreAndMatch;
    float bestAccScore = minScore;
    for (list<pair<float, KeyFrame *> >::iterator it : lScoreAndMatch) {
        KeyFrame *pKFi = it->second;
        vector<KeyFrame *> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
        float bestScore = it->first; 
        float accScore = it->first;  
        KeyFrame *pBestKF = pKFi;    
        for (vector<KeyFrame *>::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) {
            KeyFrame *pKF2 = *vit;
            if (pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) {
                accScore += pKF2->mLoopScore;
                if (pKF2->mLoopScore > bestScore) {
                    pBestKF = pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }

        lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF));
        if (accScore > bestAccScore)
            bestAccScore = accScore;
    }

	// step5. 取相似度得分高于最高相似度0.75的组的最优匹配关键帧    
    float minScoreToRetain = 0.75f * bestAccScore;
    set<KeyFrame *> spAlreadyAddedKF;
    vector<KeyFrame *> vpLoopCandidates;
    for (list<pair<float, KeyFrame *> >::iterator it : lAccScoreAndMatch.begin()) {
        if (it->first > minScoreToRetain) {
            KeyFrame *pKFi = it->second;
            if (!spAlreadyAddedKF.count(pKFi)) {
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }

    return vpLoopCandidates;
}

计算Sim3变换: ComputeSim3()

请添加图片描述

成员函数/变量访问控制意义
std::vector<KeyFrame *> mvpEnoughConsistentCandidatesprotected在函数LoopClosing::DetectLoop()中找到的有足够连续性的闭环关键帧
g2o::Sim3 mg2oScw
cv::Mat mScw		protected
protected		世界坐标系w到相机坐标系c的Sim3变换
std::vector<MapPoint *> mvpLoopMapPointsprotected闭环关键帧组中的地图点
std::vector<MapPoint *> mvpCurrentMatchedPointsprotected当前帧到mvpLoopMapPoints的匹配关系
mvpCurrentMatchedPoints[i]表示当前帧第i个特征点对应的地图点

请添加图片描述

bool LoopClosing::ComputeSim3() {
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
    ORBmatcher matcher(0.75, true);

    vector<Sim3Solver *> vpSim3Solvers;					// 保存每个闭环匹配关键帧的Sim3Solver
    vector<vector<MapPoint *> > vvpMapPointMatches;		// 保存当前关键帧到每个闭环匹配关键帧的匹配关系
    vector<bool> vbDiscarded;							// 保存每个闭环匹配关键帧是否是误匹配

    // step1. 为每个有超过20个匹配点的闭环关键帧创建Sim3Solver
    int nCandidates = 0;
    for (int i = 0; i < nInitialCandidates; i++) {
        KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
        pKF->SetNotErase();
        int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);
        if (nmatches < 20) {
            vbDiscarded[i] = true;
            continue;
        } else {
            Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale);
	   		pSolver->SetRansacParameters(0.99, 20, 300);
            vpSim3Solvers[i] = pSolver;
        }
        nCandidates++;
    }

	// step2. 对每个闭环候选关键帧求解优化Sim3
    bool bMatch = false;		// 是否有帧通过Sim3求解
    while (nCandidates > 0 && !bMatch) {
        for (int i = 0; i < nInitialCandidates; i++) {
            if (vbDiscarded[i])
                continue;

            KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;
            // step2.1. 进行Sim3迭代求解
            Sim3Solver *pSolver = vpSim3Solvers[i];
            cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
            if (bNoMore) {
                vbDiscarded[i] = true;
                nCandidates--;
            }

            if (!Scm.empty()) {
				// step2.2 根据计算出的Sim3搜索匹配点
                vector<MapPoint *> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint *>(NULL));
                for (size_t j = 0, jend = vbInliers.size(); j < jend; j++) {
                    if (vbInliers[j])
                        vpMapPointMatches[j] = vvpMapPointMatches[i][j];
                }
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();
                matcher.SearchBySim3(mpCurrentKF, pKF, vpMapPointMatches, s, R, t, 7.5);

                // step2.3. 根据搜索出的匹配点优化Sim3
                g2o::Sim3 gScm(Converter::toMatrix3d(R), Converter::toVector3d(t), s);
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
                if (nInliers >= 20) {
                    bMatch = true;
                    mpMatchedKF = pKF;
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0);
                    mg2oScw = gScm * gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
                    mvpCurrentMatchedPoints = vpMapPointMatches;
                    break;
                }
            }
        }
    }

    // step2.4. 优化失败,退出函数
    if (!bMatch) {
        for (int i = 0; i < nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }

    // step3. 将闭环关键帧及其共视关键帧的所有地图点 投影到 当前关键帧
    vector<KeyFrame *> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    for (KeyFrame *pKF : vpLoopConnectedKFs) {
        for (MapPoint *pMP : pKF->GetMapPointMatches()) {
            if (pMP && !pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) {
                mvpLoopMapPoints.push_back(pMP);
                pMP->mnLoopPointForKF = mpCurrentKF->mnId;
            }
        }
    }
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints, 10);

    // step5. 根据投影成功的地图点数判断Sim3计算的是否准确
    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;
    }
}

闭环矫正: CorrectLoop()

请添加图片描述

函数LoopClosing::CorrectLoop()的主要流程:

  1. Sim3位姿传播:

    • 将Sim3位姿传播到局部关键帧组上.
    • 将Sim3位姿传播到局部地图点上.

  2. 地图点融合:

    • 将闭环关键帧组地图点投影到当前关键帧上.
    • 将闭环关键帧组地图点投影到局部关键帧组上.

  3. BA优化

    • 本质图BA优化: 优化所有地图点和关键帧位姿,基于本质图.
    • 全局BA优化: 优化所有地图点和关键帧位姿,基于地图点到关键帧的投影关系.
void LoopClosing::CorrectLoop() {

    cout << "Loop detected!" << endl;

    // step0. 更新当前关键帧组与地图点的连接
    mpCurrentKF->UpdateConnections();

    // step1. Sim3位姿传播
    // step1.1. 构建局部关键帧组
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    map<KeyFrame*, g2o::Sim3> CorrectedSim3, NonCorrectedSim3;	// 存放局部关键帧组Sim3位姿传播前后的位姿
    CorrectedSim3[mpCurrentKF] = mg2oScw;    
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();
    
    {
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

        // step1.2 将Sim3位姿传播到局部关键帧组中
        for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
            cv::Mat Tiw = pKFi->GetPose();
            if (pKFi != mpCurrentKF) {
                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);
                g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;
                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);
            NonCorrectedSim3[pKFi] = g2oSiw;
        }
		
        // step1.3. 将Sim3位姿传播到局部地图点上
        for (pair<KeyFrame*, g2o::Sim3> mit : CorrectedSim3) {
            KeyFrame *pKFi = mit.first;
            g2o::Sim3 g2oCorrectedSiw = mit.second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
            g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];

            for (MapPoint *pMPi : pKFi->GetMapPointMatches()) {
                if (!pMPi || pMPi->isBad())
                    continue;
                if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId) 	// 标记,防止重复矫正
                    continue;
                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();
            }

			// 将更新后的Sim3位姿赋值给关键帧变量
            Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
            Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
            double s = g2oCorrectedSiw.scale();
            eigt *= (1. / s);
            cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt);
            pKFi->SetPose(correctedTiw);
            pKFi->UpdateConnections();
        }
	
        // step2. 地图点融合
        // step2.1 将闭环关键帧组地图点融合到当前关键帧上
        for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
            if (mvpCurrentMatchedPoints[i]) {
                MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];
                MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);
                if (pCurMP)
                    pCurMP->Replace(pLoopMP);		// 闭环关键帧组地图点在地图中的时间更长,位姿更准确
                else {
                    mpCurrentKF->AddMapPoint(pLoopMP, i);
                    pLoopMP->AddObservation(mpCurrentKF, i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        }

    }

	// step2.2 将闭环关键帧组地图点融合到局部关键帧组上
    SearchAndFuse(CorrectedSim3);


    // step3. BA优化
    // step3.0. 查找回环连接边,用于和生成树共同组成本质图
    map<KeyFrame *, set<KeyFrame *> > LoopConnections;
    for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
        vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
        pKFi->UpdateConnections();
        // 闭环矫正后的共视关系 - 闭环矫正前的共视关系 = 闭环带来的新共视关系
        for (KeyFrame* prev_KFi : vpPreviousNeighbors) {
            LoopConnections[pKFi].erase(*prev_KFi);
        }
        for (KeyFrame* prev_KFi : mvpCurrentConnectedKFs) {
            LoopConnections[pKFi].erase(prev_KFi);
        }
    }

    // step3.1. 本质图BA优化
	mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

	// step3.2. 全局BA优化
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);

    cout << "Loop Closed!" << endl;
    mLastLoopKFid = mpCurrentKF->mnId;
}

pdf版本笔记的下载地址: ORB-SLAM2代码详解09_闭环线程LoopClosing,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

Y
N
发生回环
未发生回环
计算失败或重投影误差过大
计算成功
队列中是否存在未处理的关键帧
检查是否发生回环
计算Sim3变换
修正回环
当前线程暂停5毫秒
std::this_thread::sleep_for(std::chrono::milliseconds(5))
匹配成功
匹配失败
求解成功
求解失败
优化成功
优化失败
匹配失败
匹配成功
根据词袋向量粗匹配
进行Sim3求解
Sim3优化:只优化Sim3
根据之前计算出的sim3进行投影匹配
计算失败
计算成功
ncepu_Chen
关注
  • 9
    点赞
  • 踩
  • 35
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
专栏目录
(01)ORB-SLAM2源码无死角解析-(60) 闭环线程→闭环矫正: CorrectLoop→地图点融合、共视关系更新
江南才尽江南山，年少无知年少狂！
09-21 822
这里需要回顾一个前面的知识点，那就是 mvpCurrentMatchedPoints 这个变量。mvpCurrentMatchedPoints 记录的是经过 SearchBySim3 得到的已经匹配的点对(当前关键帧、闭环候选帧中有效的关键帧)。这里的匹配点我们认为是正确的，因为其是使用Sim3进行匹配的。另外再上一篇博客中，对地图点更新是不包含当前关键帧的，所以还需要对当前关键帧地图点进行更新。
VI_ORB_SLAM2:基于ORB-SLAM2的MonocularStereo视觉惯性ORB-SLAM
05-14
VI_ORB_SLAM2：基于ORB-SLAM2的单目/立体视觉惯性ORB-SLAM 该存储库包括Visual-Inertial ORB-SLAM的Monocular版本和Stereo版本。 这两个是的和的。 有关详细信息，您可以参考Examples/Monocular/mono_euroc_VI.cc和...
ORBSLAM2总结之Loop_Closing
joun772的博客
02-06 681
下列函数代码都在Loop_Closing.cpp中 参考博客 一、回环检测流程 代码对应函数为：void LoopClosing::Run() 在这里插入代码片 二、闭环帧的检测 代码中对应函数为：DetectLoop() 首先通过函数CheckNewKeyFrames()检测mlpLoopKeyFrameQueue里的关键帧是否为空，如果不空则进行闭环检测的处理：也就是DetectLoop()。（Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracki
一起学ORBSLAM2（8）ORBSLAM的loopclosing
蚁族的坚持的博客
06-05 2443
ORBSLAM中的回环检测和重定位具有类似的方式，所以我们这里将两个部分放到同一篇文章中讲解。首先看tracking线程中的重定位。一．Tracking重定位重定位模式是在系统处于LOST（追踪线程失败）的情况下进行的系统挽救工作，它的基本流程是：（1）计算当前帧的BoW映射（2）找到与当前帧相似的候选关键帧（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw（RANSAC+EPNP算法）（4）...
ORB-SLAM2 Loop Closing
UNICORN GUNDAM
09-27 300
ORB-SLAM2整体分为tracking、local mapping、loop closing以及viewer四部分。其中viewer用来进行关键帧、位姿以及地图点的绘制，和SLAM整体过程无关。 Loop Closing处理的是从mlpLoopKeyFrameQueue中依次提取的关键帧。（Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Trac...
ORB-SLAM2 --- LoopClosing::CorrectLoop函数
qq_41694024的博客
01-12 1068
调整位姿、融合地图点、全局优化
ORB-SLAM2.xmind
01-19
ORB-SLAM2代码阅读思维导图，xmind8编辑，可用xmind8或者xmind zen打开。自己做的，可能有错误，如果有问题可直接私信给我。 ORB-SLAM2 是我上手的第一个比较大的SLAM系统和c++项目，因此方便建立了思维导图更深的...
ORB_SLAM2-Chinese-annotation:ORB-SLAM2中文注释版（适用入门学习）
04-28
ORB-SLAM2（中文注释版） 评论员： 作者： ， ， 和 （ ） 2017年1月13日：现在支持OpenCV 3和Eigen 3.3。 2016年12月22日：添加了AR演示（请参见第7节）。 ORB-SLAM2是用于单目，立体声和RGB-D相机的实时SLAM库...
ORB-SLAM2源码中文详解.pdf
08-17
东北大学吴博讲解ORB_SLAM的文档，仅有文档。
基于ORB-SLAM2的实时网格地图构建
04-30
针对目前视觉SLAM（同时定位与地图构建）系统只能输出相机的运动轨迹图而不能生成用于路径规划和导航地图的缺点，提出了一种基于ORB-SLAM2的网格地图实时构建算法。首先，建立了一个适用于视觉SLAM的逆传感器模型...
ORB-SLAM学习（3）—— LoopClosing线程
羽逸非凡的博客
01-17 547
毋庸置疑的是，随着相机的运动，我们计算的相机位姿，三角化得到的点云位置，都是有误差的，即使我们使用局部的或全局的BA去优化，仍然会存在累积误差。而消除误差最有效的办法是发现闭环，并根据闭环对所有结果进行优化。闭环是一个比BA更加强烈、更加准确的约束，所有如果能够检测到闭环，并对其优化，就可以让结果更加准确。 代码框架 整个LoopClosing模块是在线程中完成，并在创建线程时调用LoopClos...
ORB-SLAM2中的闭环检测1-算法流程+代码注释
zysss_的博客
08-10 1302
mlpLoopKeyFrameQueue是通过下面的函数插入的在候选帧中检测具有连续性的候选帧5.1 遍历刚刚得到的候选帧5.2 将自己以及与候选帧自己相连的关键帧构成一个“”，每个候选组都构成一个子候选组5.3 遍历前一次闭环检测到的，每次遍历用set容器装的，子连续组是连续组链中的其中一个组5.4 在5.3的遍历中所有“”中的关键帧，判断中是否有与子候选组相同的关键帧 ，由于用set容器装的子连续组，所以.count()就能判断了5.5 在5.3的遍历中，与5.4同级，判断与后，将。..........
ORB-SLAM2 --- LoopClosing::Run 回环检测线程解析
qq_41694024的博客
01-12 1992
LoopClosing::Run 回环检测线程解析
ORB-SLAM2双目开源框架 （4） LoopClosing解析
qq_38766208的博客
11-17 352
闭环检测 (1)计算当前关键帧与其共视关键帧的最低相似性得分minScore，在关键帧数据库中与当前关键帧相似性得分高于minScore的关键帧为候选闭环关键帧vpCandidateKFs (2)将候选关键帧扩展为候选关键帧组，包含候选闭环关键帧及其共视关键帧。对候选关键帧组进行一致性测试：是否与其他候选关键帧共有相同的关键帧。 (3)当候选关键帧组的一致性次数超过给定阈值mnCovisibilityConsistencyTh时，则对应的候选关键帧加入mvpEnoughConsistentCandidate
ORB-SLAM2中LoopClosing线程主要流程梳理
03-25 944
LoopClosing线程主要工作是两个：一个是对当前新到来的关键帧进行闭环检测，看看之前的关键帧中哪个和当前关键帧形成闭环；另一个是纠正闭环，就是在检测到闭环的时候要将当前关键帧和闭环关键帧的位姿拉到一起，形象点说就是进行融合（既然都存在闭环了，那肯定得合并啊）。 一、闭环检测DetectLoop 1.从关键帧列表中获取列表头的关键帧，也就是获取最新插入到LoopClosing线程的关键帧，...
ORB-SLAM2代码阅读笔记（八）：LoopClosing线程
06-12 1990
Table of Contents 1.LoopClosing线程工作内容 2.LoopClosing线程代码流程分析 1）闭环检测 2）计算sim3 3）闭环校正 3.小结 在上一篇笔记ORB-SLAM2代码阅读笔记（七）：LocalMapping线程 中，可以看到LocalMapping线程中会将关键帧插入到LoopClosing线程的列表中，对应代码语句如下： //关键帧...
ORBSLAM2源码学习（9） LoopClosing类
Fearless的博客
08-18 1087
该类负责闭环的检测和校正，逻辑关系比较简明 while(1){ // Check if there are keyframes in the queue // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的 // 在LocalMapping中通过InsertKeyFram...
ORB-SLAM2从理论到代码实现（十）：LoopClosing程序详解
人无远虑，必有近忧
09-18 556
LoopClosing是专门负责做闭环的类，它的主要功能就是检测闭环，计算闭环帧的相对位姿病以此做闭环修正。
orbslam2学习笔记 -- loopclosing 流程梳理
michaelhan3的博客
05-06 1717
读之前提醒：首先，该文章是按照时间顺序讲述，并且不提前讲各种类关系和数据结构关系，而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的，然后告诉你这个东西是啥。如果你还没有读过tracking的部分，请先读tracking，因为很多概念会在tracking里先讲出来，后面再遇到，就不会细讲了。 tracking :https://blog.csdn.ne...
orb-slam3代码详解
最新发布
07-25
很抱歉，我无法提供ORB-SLAM3代码的详细解释。作为一个AI助手，我无法提供具体的代码实现。然而，我可以告诉你一些关于ORB-SLAM3的信息。ORB-SLAM3是ORB-SLAM系列的最新版本，它是一种用于实时单目、双目和RGB-D相机...

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

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

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值