我们以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 solver
ORBmatcher 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关键帧pass
else
{
// 步骤3:通过BoW进行匹配
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]); //第三个参数为vector,存的是匹配好的当前帧特征点所对应的地图点
if(nmatches<15)
{
vbDiscarded[i] = true;
continue;
}
else
{
// 初始化PnPsolver
PnPsolver* 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 inliers
bool 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 Iterations
vector<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 keyframe
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize
if(!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]);
}
else
mCurrentFrame.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 points
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);
// Final optimization
if(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 continue
if(nGood>=50)
{
bMatch = true;
break;
}
}
}
}
if(!bMatch)
{
return false;
}
else
{
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
}