ORB-SLAM中的重定位解读及追踪

我们以ORBSLAM中的重定位举例，来分析重定位的过程。当跟踪失败的时候，系统会启动重定位。找出一些候选关键帧，对每个候选关键帧，用ransac和EPNP估计位姿，然后更新当前帧的地图点匹配，然后优化位姿，如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解，直到有足够的内点支持，重定位完成。　　　

下面简要梳理重定位的过程——

用关键帧库的DetectRelocalizationCandidates函数找到当前帧对应的候选关键帧
对每个候选关键帧，用SearchByBow匹配当前帧和该关键帧（即更新当前帧特征点对应的地图点，更新mvMapPoints列表），用当前帧和当前帧的特征匹配关系来初始化PnPSolver
对每个候选关键，用EPNP估计位姿
- 如果估计成功，用估计所得的内点更新当前帧的特征匹配
- 调用PoseOptimization，优化当前帧的位姿
- 优化后，剔除特征匹配的外点（即去除误匹配的地图点）
- 如果优化后的内点过少，用SearchByProjection对未匹配的特征点匹配当前帧与关键帧
- 所有的匹配点（内点加新增的匹配点）大于50，调用PoseOptimization
- 若优化后，新的内点数还是较少（30~50），用更小的窗口再次调用SearchByProjection
- 所有的匹配点数大于50，调用PoseOptimization（最后一次优化），并剔除外点
- 如优化后的内点大于50，中断，重定位成功，当前帧的ID就是最近一次的重定位ID帧

下面的几张图来自网上，不可全信，但可参考，真相还是要看ORBSLAM的源码——

在Tracking.cpp源文件中有重定位函数的实现

bool Tracking::Relocalization()  //
{// Compute Bag of Words Vector// 步骤1：计算当前帧特征点的Bow映射mCurrentFrame.ComputeBoW();// Relocalization is performed when tracking is lost// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation// 步骤2：找到与当前帧相似的候选关键帧vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);  //先取出候选关键帧if(vpCandidateKFs.empty())return false;const int nKFs = vpCandidateKFs.size();// We perform first an ORB matching with each candidate// If enough matches are found we setup a PnP solverORBmatcher matcher(0.75,true);vector<PnPsolver*> vpPnPsolvers;vpPnPsolvers.resize(nKFs);vector<vector<MapPoint*> > vvpMapPointMatches;vvpMapPointMatches.resize(nKFs);vector<bool> vbDiscarded;vbDiscarded.resize(nKFs);int nCandidates=0;for(int i=0; i<nKFs; i++){KeyFrame* pKF = vpCandidateKFs[i];if(pKF->isBad())vbDiscarded[i] = true;  //这个i关键帧passelse{// 步骤3：通过BoW进行匹配int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);  //第三个参数为vector,存的是匹配好的当前帧特征点所对应的地图点if(nmatches<15){vbDiscarded[i] = true;continue;}else{// 初始化PnPsolverPnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);vpPnPsolvers[i] = pSolver;nCandidates++;}}}// Alternatively perform some iterations of P4P RANSAC// Until we found a camera pose supported by enough inliersbool bMatch = false;ORBmatcher matcher2(0.9,true);while(nCandidates>0 && !bMatch){for(int i=0; i<nKFs; i++){if(vbDiscarded[i])continue;// Perform 5 Ransac Iterationsvector<bool> vbInliers;int nInliers;bool bNoMore;// 步骤4：通过EPnP算法估计姿态PnPsolver* pSolver = vpPnPsolvers[i];cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// If Ransac reachs max. iterations discard keyframeif(bNoMore){vbDiscarded[i]=true;nCandidates--;}// If a Camera Pose is computed, optimizeif(!Tcw.empty()){Tcw.copyTo(mCurrentFrame.mTcw);set<MapPoint*> sFound;const int np = vbInliers.size();for(int j=0; j<np; j++){if(vbInliers[j]){mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];sFound.insert(vvpMapPointMatches[i][j]);}elsemCurrentFrame.mvpMapPoints[j]=NULL;}// 步骤5：通过PoseOptimization对姿态进行优化求解int nGood = Optimizer::PoseOptimization(&mCurrentFrame);if(nGood<10)continue;for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);// If few inliers, search by projection in a coarse window and optimize again// 步骤6：如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解if(nGood<50){int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);  //SearchByProjection第三个重载函数了if(nadditional+nGood>=50){nGood = Optimizer::PoseOptimization(&mCurrentFrame);// If many inliers but still not enough, search by projection again in a narrower window// the camera has been already optimized with many pointsif(nGood>30 && nGood<50){sFound.clear();for(int ip =0; ip<mCurrentFrame.N; ip++)if(mCurrentFrame.mvpMapPoints[ip])sFound.insert(mCurrentFrame.mvpMapPoints[ip]);nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);// Final optimizationif(nGood+nadditional>=50){nGood = Optimizer::PoseOptimization(&mCurrentFrame);for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=NULL;}}}}// If the pose is supported by enough inliers stop ransacs and continueif(nGood>=50){bMatch = true;break;}}}}if(!bMatch){return false;}else{mnLastRelocFrameId = mCurrentFrame.mnId;return true;}}

ORB-SLAM中的重定位解读及追踪相关推荐

一文详解回环检测与重定位
标题:VINS-Mono代码解读-回环检测与重定位 pose graph loop closing 作者:Manii 来源:https://blog.csdn.net/qq_41839222/cate ...
作为SLAM中最常用的闭环检测方法，视觉词袋模型技术详解来了
摘自:https://mp.weixin.qq.com/s/OZnnuA31tEaVt0vnDOy5hQ 作为SLAM中最常用的闭环检测方法,视觉词袋模型技术详解来了原创小翼飞思实验室今天基 ...
关于机器人状态估计(10)-VSLAM与VIO的3D建图，重定位与世界观综述
近期我国迎来了cov海啸,其实我也不知道我羊了没有,但并没有什么不舒服同时因为我没有测,那自然是没有羊,或者是薛定谔的羊. 近年另外一块工作的综述,这篇科普的同时,也会包含部分有价值的信息. 一. 摘 ...
VSLAM与VIO的3D建图，重定位与世界观综述
作者 | 紫川Purple River 编辑 | 汽车人原文链接:zhuanlan.zhihu.com/p/592225457 点击下方卡片,关注"自动驾驶之心"公众号 ADA ...
最全综述 | SLAM中回环检测方法收藏
在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累积误差.一个消除误差有效的办法是进行回环检测.回环检测判断 ...
共享可写节包含重定位_PE结构学习01-DOS头-NT头-节表头
什么是PE结构:PE(Portable Execute)文件是Windows下可执行文件的总称,常见的有DLL,EXE,OCX,SYS等,就是只要在Windows下的可执行程序的内部结构都是PE结构, ...
Windows PE第6章栈与重定位表
第六章栈与重定位表本章主要介绍栈和代码重定位.站和重定位表两者并没有必然的联系,但都和代码有关.栈描述的是代码运行过程中,操作系统为调度程序之间相互调用关系,或临时存放操作数而设置的一种数据结构. ...
PE结构基址重定位表
PE体系 PE结构&整体叙述 PE结构&导入表 PE结构&导出表 PE结构&基址重定位表 PE结构&绑定导入实现 PE结构&延迟加载导入表重定位表定位 ...
S5PV210裸机之重定位
1.重定位相关概念位置无关码(PIC,position independent code):汇编源文件被编译成二进制可执行文件时编码方式与位置(内存地址)无关. 位置有关码:汇编源文件被编译成二进 ...

ORB-SLAM中的重定位解读及追踪

ORB-SLAM中的重定位解读及追踪相关推荐

最新文章

热门文章