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;
}
}