ORBSLAM—重定位(一)

ORBSLAM—重定位

文章与公众号“SLAM源码之路”同步推送，扫描文后二维码关注。

变量含义：

mCurrentFrame: 当前帧 Frame类

vpCandidateKFs: 存储与当前帧相似的候选关键帧容器 vector<KeyFrame*>

nKFs: 候选关键帧的数量

vpPnPsolvers: 当前帧与候选关键帧的pnp求解器容器

vpPnPsolvers 记录每一个候选关键帧的mappoint。 类型：vector<vector<MapPoint*> >

vbDiscarded: 有效候选关键帧标志位。

bNoMore: RansacPnP是否成功标志位

sFound: 当前帧已经匹配上的内点

nGood：通过BA优化计算得到内点的数量

bMatch: 重定位如果成功为ture

step1:计算当前帧的词袋向量。

mCurrentFrame.ComputeBoW();

step2:在关键帧数据库中找到与当前帧相似的一些关键帧。

DetectRelocalizationCandidates(&mCurrentFrame);

step3:对于上一步得到的每一个关键帧进行词袋匹配

step3.1 对每一个候选帧进行词袋匹配

int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); 如果点匹配数量小于15个，则退出此候选帧。 step3.2 初始化PnP求解器，同时设置Ransac参数，vvpMapPointMatches变量记录每一个候选关键帧的mappoint，nCandidates变量为有效候选关键帧数量。 PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]); pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);

step4:通过EPNP算法估计姿态

step4.1 通过5次EPNP+Ransac迭代计算当前帧位姿

cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

step4.2 PnP得到的位姿作为优化的初值，同时用匹配上的特征点更新当前帧mvpMapPoints成员，用于后续BA优化时进行投影。vvpMapPointMatches[i][j]是第i个候选关键帧的第j个内点。

step4.3 BA优化，仅优化相机位姿，返回认为正确匹配点的数量，如果正确匹配数量小于10，则放弃此候选帧。

int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

step4.4 如果上一步得到的内点数量在10到50之间，则通过投影的方式对之前未进行匹配的点再次进行匹配。如果最后得到的内点大于50，则认为与当前候选关键帧计算得到的Pose为正确的Pose，重定位成功，结束循环。

step4.4.1 对之前没有进行匹配上的点在一定的阈值之内投影匹配

int nadditional=matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);

step4.4.2 额外获得匹配点加上之前BA得到的内点大于50个，则再次进行BA优化计算位姿。此时如果BA优化后得到的内点数量在30-50之间，那么在更小的搜索区间内通过投影匹配得到更多的匹配点。

if(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); // 最后一次优化， if(nGood+nadditional>=50) { nGood = Optimizer::PoseOptimization(&mCurrentFrame); for(int io =0; io<mCurrentFrame.N; io++) if(mCurrentFrame.mvbOutlier[io]) mCurrentFrame.mvpMapPoints[io]=NULL; } }