orb-slam系列 LoopClosing线程 ComputeSim3(十一)

ComputeSim3

/*********************************   计算每一个回环候选关键帧与当前关键帧之间的相似矩阵*      1. 首先通过BOW向量将回环候选关键帧和当前关键帧进行匹配,得到匹配地图点,通过匹配地图点初始化相似矩阵求解器*       2. 遍历所有的回环候选关键帧和当前关键帧计算sim矩阵,并优化sim矩阵,根据优化sim矩阵确定匹配内点数量,从而确定此sim矩阵的准确性,以及是否可以判定为一回环.*       3. 在找到的回环关键帧周围查找共视关键帧   并匹配共视关键帧的地图点和当前关键帧  相当与是匹配covisual graph和当前关键帧  根据匹配点数量确定当前帧是否发生了回环**************************************/
bool LoopClosing::ComputeSim3()
{// For each consistent loop candidate we try to compute a Sim3const int nInitialCandidates = mvpEnoughConsistentCandidates.size();  //候选回环帧// We compute first ORB matches for each candidate// If enough matches are found, we setup a Sim3SolverORBmatcher matcher(0.75,true);// 相似性矩阵求解器vector<Sim3Solver*> vpSim3Solvers;vpSim3Solvers.resize(nInitialCandidates);// 匹配地图点容器   vector<vector<MapPoint*> > vvpMapPointMatches;   //vvpMapPointMatches[i][j]   候选回环帧i 对应于 待回环帧第j地图点 对应的地图点vvpMapPointMatches.resize(nInitialCandidates);// 取消候选回环关键帧的标志变量vector<bool> vbDiscarded;vbDiscarded.resize(nInitialCandidates);int nCandidates=0; //candidates with enough matches// 初始化当前帧和候选关键帧的相似矩阵求解器,计算匹配地图点,取消回环关键帧标志变量的赋初值for(int i=0; i<nInitialCandidates; i++){KeyFrame* pKF = mvpEnoughConsistentCandidates[i];// avoid that local mapping erase it while it is being processed in this threadpKF->SetNotErase();if(pKF->isBad()){vbDiscarded[i] = true;continue;}// 待回环关键帧与回环候选关键帧进行匹配得到匹配地图点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++;}bool bMatch = false;// Perform alternatively RANSAC iterations for each candidate// until one is succesful or all fail// RANSAC迭代每一个回环候选关键帧和当前待回环关键帧// 通过SearchByBoW匹配得到初步匹配点,根据此匹配计算两关键帧之间的sim矩阵// while(nCandidates>0 && !bMatch){   for(int i=0; i<nInitialCandidates; i++){if(vbDiscarded[i])  //小于20continue;KeyFrame* pKF = mvpEnoughConsistentCandidates[i];// Perform 5 Ransac Iterationsvector<bool> vbInliers;int nInliers;bool bNoMore;Sim3Solver* pSolver = vpSim3Solvers[i];cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// If Ransac reachs max. iterations discard keyframe   如果迭代次数达到最大  则证明当前候选帧不是回环帧if(bNoMore){vbDiscarded[i]=true;nCandidates--;}// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences// 如果RANSAC求取了一个合适的相似变换矩阵sim   则通过sim矩阵重新进行地图点匹配,并优化sim矩阵并确定内点  根据内点数量判定sim矩阵是否符合要求if(!Scm.empty()){vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));    //匹配  待回环帧i帧 对应的两地图点 =>> vpMapPointMatches[i] = loopkfi.map[j] 此帧i点 对应于 回环帧的j点for(size_t j=0, jend=vbInliers.size(); j<jend; j++){if(vbInliers[j])vpMapPointMatches[j]=vvpMapPointMatches[i][j];   //第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);g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);   //sim3  current点到 回环帧点的sim3const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);// If optimization is succesful stop ransacs and continue  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;}}}}if(!bMatch){for(int i=0; i<nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();mpCurrentKF->SetErase();return false;}// Retrieve MapPoints seen in Loop Keyframe and neighbors   // 在找到的回环关键帧周围查找共视关键帧   并匹配共视关键帧的地图点和当前关键帧  相当于是匹配covisual graph和当前关键帧  改动关联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  匹配共视关键帧的地图点和当前关键帧matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);// If enough matches accept Loop   如果有足够的匹配点 则说明找到了回环,否则查找回环失败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)  //回环帧是 mpMatchedKFmvpEnoughConsistentCandidates[i]->SetErase();return true;}else{for(int i=0; i<nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();mpCurrentKF->SetErase();return false;}}

int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

1）每一个候选回环帧都要跟待回环帧进行bow的词袋模型的匹配，并且得到匹配点。
（同一单词中两帧的关键点进行最佳匹配）
2）匹配点数大于等于20的时候进行sim3计算。

Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);

3）通过pSolver->iterate(5,bNoMore,vbInliers,nInliers); 计算出Scm
在坐标系帧一帧二中任意选3个对点计算sim3算法（也可以icp计算）得到位姿变化 T12
4）matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5); 进行sim3验证

双向匹配:
根据相似矩阵将关键帧1中的地图点向关键帧2中投影,确定投影区域,并在投影区域内寻找关键帧1中地图点的匹配
根据相似矩阵将关键帧2中的地图点向关键帧1中投影,确定投影区域,并在投影区域内寻找关键帧2中地图点的匹配
根据双向匹配结果,如果两次匹配都能成功,则确定该对匹配是有效的.将其存入vpMatches12容器
最终返回匹配点对个数

帧一的地图三维点，在相机一的坐标，用优化的T12转化到相机二的坐标。验证帧二是否有这个像素点，帧二的阈值范围内找特征点匹配，找到最佳匹配。
双向同理。
都成功是匹配点。

5）const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale)
优化的策略
关键点1的uv值- K1 * t12 * p3dc2
同理关键点2的uv值- K2 * t21 * p3dc1
优化得到sim3

6）修正

g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);mg2oScw = gScm*gSmw;

因为gSmw是待回环帧在世界坐标系的位姿，计算出来当前帧在正确世界坐标系的位姿

7）mvpCurrentMatchedPoints是当前帧和回环帧的匹配点。mpMatchedKF是回环帧
8）mvpLoopMapPoints 回环帧及其共视帧的地图点 vpLoopConnectedKFs回环帧及其共视帧
9）matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
注意里面3d点的来源是 mvpLoopMapPoints 也就是正确的位置3d点
p3Dc得到了在当前相机坐标系下的坐标，阈值范围内找最佳匹配返回匹配点数目
10）数额足够完成

orb-slam系列 LoopClosing线程 ComputeSim3(十一)相关推荐

orb-slam系列 LoopClosing线程 DetectLoop(十)
DetectLoop /******************************************************************************** 功能:检测是否 ...
ORB_SLAM2代码阅读(4)——LoopClosing线程
ORB_SLAM2代码阅读(4)--LoopClosing线程 1.说明 2.简介 3.检测回环 4.计算Sim3 4.1 为什么在进行回环检测的时候需要计算相似变换矩阵,而不是等距变换? 4.2 累 ...
ORB-SLAM2源码阅读（四）—LoopClosing线程SIM3变换
目录 LoopClosing线程代码解析 1.DetectLoop() 闭环检测 2.计算当前帧与闭环帧的Sim3变换ComputeSim3() 1.SIM3变换 1.计算SIM3平移计算SIM3尺 ...
SLAM系列——机器人顶刊T-RO！用于关联、建图和高级任务的物体级SLAM框架
系列文章目录 SLAM系列--第一讲预备知识[2023.1] SLAM系列--第二讲初识SLAM[2023.1] SLAM系列--第三讲三维空间刚体运动[2023.1] SLAM系列--第四讲 ...
SLAM系列——第三讲三维空间刚体运动[2023.1]
系列文章目录 SLAM系列--第一讲预备知识[2023.1] SLAM系列--第二讲初识SLAM[2023.1] SLAM系列--第三讲三维空间刚体运动[2023.1] SLAM系列--第四讲 ...
Elasticsearch源码分析—线程池(十一) ——就是从队列里处理请求
Elasticsearch源码分析-线程池(十一) 转自:https://www.felayman.com/articles/2017/11/10/1510291570687.html 线程池每个节 ...
Java多线程系列--“JUC线程池”06之 Callable和Future
转载自 Java多线程系列--"JUC线程池"06之 Callable和Future Callable 和 Future 简介 Callable 和 Future 是比较有趣的一 ...
Django 系列博客（十一）
Django 系列博客(十一) 前言本篇博客介绍使用 ORM 来进行多表的操作,当然重点在查询方面. 创建表实例: 作者模型:一个作者有姓名和年龄. 作者详细模型:把作者的详情放到详情表,包含生日 ...
java任务流程_死磕 java线程系列之线程池深入解析——普通任务执行流程
(手机横屏看源码更方便) 注:java源码分析部分如无特殊说明均基于 java8 版本. 注:线程池源码部分如无特殊说明均指ThreadPoolExecutor类. 简介前面我们一起学习了Java中 ...

orb-slam系列 LoopClosing线程 ComputeSim3(十一)

ComputeSim3

orb-slam系列 LoopClosing线程 ComputeSim3(十一)相关推荐

最新文章

热门文章