orb-slam2回环检测3

for(int i=0; i<nInitialCandidates; i++）
{// 步骤1：从筛选的闭环候选帧中取出一帧关键帧pKFKeyFrame* pKF = mvpEnoughConsistentCandidates[i];// 防止在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除pKF->SetNotErase();if(pKF->isBad()){vbDiscarded[i] = true;// 直接将该候选帧舍弃continue;}// 步骤2：将当前帧mpCurrentKF与闭环候选关键帧pKF匹配// 通过bow加速得到mpCurrentKF与pKF之间的匹配特征点，vvpMapPointMatches是匹配特征点对应的MapPointsint nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);// 匹配的特征点数太少，该候选帧剔除if(nmatches<20){vbDiscarded[i] = true;continue;}else{// 构造Sim3求解器// 如果mbFixScale为true，则是6DoFf优化（双目 RGBD），如果是false，则是7DoF优化（单目）Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);pSolver->SetRansacParameters(0.99,20,300);// 至少20个inliers 300次迭代vpSim3Solvers[i] = pSolver;}// 参与Sim3计算的候选关键帧数加1nCandidates++;}bool bMatch = false;// 用于标记是否有一个候选帧通过Sim3的求解与优化// 一直循环所有的候选帧，每个候选帧迭代5次，如果5次迭代后得不到结果，就换下一个候选帧// 直到有一个候选帧首次迭代成功bMatch为true，或者某个候选帧总的迭代次数超过限制，直接将它剔除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;// 这是局部变量，在pSolver->iterate(...)内进行初始化// 步骤3：对步骤2中有较好的匹配的关键帧求取Sim3变换Sim3Solver* pSolver = vpSim3Solvers[i];// 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// 经过n次循环，每次迭代5次，总共迭代 n*5 次// 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除if(bNoMore){vbDiscarded[i]=true;nCandidates--;}// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondencesif(!Scm.empty()){vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));for(size_t j=0, jend=vbInliers.size(); j<jend; j++){// 保存inlier的MapPointif(vbInliers[j])vpMapPointMatches[j]=vvpMapPointMatches[i][j];}// 步骤4：通过步骤3求取的Sim3变换引导关键帧匹配弥补步骤2中的漏匹配// [sR t;0 1]cv::Mat R = pSolver->GetEstimatedRotation();// 候选帧pKF到当前帧mpCurrentKF的R（R12）cv::Mat t = pSolver->GetEstimatedTranslation();// 候选帧pKF到当前帧mpCurrentKF的t（t12），当前帧坐标系下，方向由pKF指向当前帧const float s = pSolver->GetEstimatedScale();// 候选帧pKF到当前帧mpCurrentKF的变换尺度s（s12）// 查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用SearchByBoW进行特征点匹配时会有漏匹配）// 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新匹配vpMapPointMatchesmatcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);// 步骤5：Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断// OpenCV的Mat矩阵转成Eigen的Matrix类型g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);// 如果mbFixScale为true，则是6DoFf优化（双目 RGBD），如果是false，则是7DoF优化（单目）// 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScmconst int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);// 卡方chi2检验阈值if(nInliers>=20){bMatch = true;// mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧mpMatchedKF = pKF;// 得到从世界坐标系到该候选帧的Sim3变换，Scale=1g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);// 得到g2o优化后从世界坐标系到当前帧的Sim3变换mg2oScw = gScm*gSmw;mScw = Converter::toCvMat(mg2oScw);mvpCurrentMatchedPoints = vpMapPointMatches;break;// 只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断}}}}// 没有一个闭环匹配候选帧通过Sim3的求解与优化if(!bMatch){// 清空mvpEnoughConsistentCandidatesfor(int i=0; i<nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();mpCurrentKF->SetErase();return false;}// 步骤6：取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入mvpLoopMapPoints// 注意是匹配上的那个关键帧：mpMatchedKF// 将mpMatchedKF相连的关键帧全部取出来放入vpLoopConnectedKFs// 将vpLoopConnectedKFs的MapPoints取出来放入mvpLoopMapPointsvector<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);// 标记该MapPoint被mpCurrentKF闭环时观测到并添加，避免重复添加pMP->mnLoopPointForKF=mpCurrentKF->mnId;}}}}// Find more matches projecting with the computed Sim3// 步骤7：将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配// 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）// 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，// 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新mvpCurrentMatchedPoints// mvpCurrentMatchedPoints将用于SearchAndFuse中检测当前帧MapPoints与匹配的MapPoints是否存在冲突matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);// 搜索范围系数为10// 步骤8：判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配int nTotalMatches = 0;for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++){if(mvpCurrentMatchedPoints[i])nTotalMatches++;}// 步骤9：清空mvpEnoughConsistentCandidatesif(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;}
}

orb-slam2回环检测3相关推荐

SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
综述 | SLAM回环检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨任旭倩来源丨计算机视觉life 在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一 ...
视觉SLAM回环检测、词袋模型和视觉位置识别--论文记录和实验简析
一.传统方法部分(词袋模型,bag of words,BoW) 1. 预先在环境中采集足够多的图像或者所有位置的图像(成千上万张图片)之后构建视觉词汇表参考论文:2012年的TRO顶刊 Gálvez ...
视觉slam十四讲 pdf_视觉SLAM十四讲|第12讲回环检测
1. 什么是回环检测前面有说过累积误差的问题,前一时刻的误差会积累到后面,导致画不成圈圈,如图12-1所示,而画圈圈(全局一致性)很重要,所以需要有一个步骤来纠正当前的计算偏差. 回环检测通过判断相 ...
【SLAM十四讲】ch11 回环检测词袋法实验得出相似分数后计算PR曲线 VPR实验编辑中
[SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 [SLAM十四讲]ch11 回环检测词袋法实验得出相似分数后计算PR曲线 DBow3库安装 ch11编译 ch11 词 ...
视觉SLAM⑪----回环检测
目录 11.0 本章目标 11.1 概述 11.1.1 回环检测的意义 11.1.2 回环检测的方法 11.1.3 准确率和召回率 11.2 词袋模型 11.3 字典 11.3.1 字典的结构 11. ...
SLAM学习——回环检测
1.回环检测回环检测的关键,就是如何有效的检测出相机经过同一个地方这件事.它关系到我们估计的轨迹和地图在长时间下的正确性. 由于回环检测提供了当前数据与所有历史数据的关联,在跟踪算法丢失后,我们还可 ...
回环检测算法综述之场景描述
在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累积误差.一个消除误差有效的办法是进行回环检测.回环检测判断 ...
回环检测线程检测闭环候选帧计算闭环处相似变换sim3 闭环处融合更新
回环检测线程检测闭环候选帧计算闭环处相似变换sim3 闭环处融合更新博文末尾支持二维码赞赏哦 github地址 /** * This file is part of ORB-SLAM2. * 回 ...
一文详解回环检测与重定位
标题:VINS-Mono代码解读-回环检测与重定位 pose graph loop closing 作者:Manii 来源:https://blog.csdn.net/qq_41839222/cate ...

orb-slam2回环检测3

orb-slam2回环检测3相关推荐

最新文章

热门文章