我们以ORBSLAM中的重定位举例，来分析重定位的过程。当跟踪失败的时候，系统会启动重定位。 找出一些候选关键帧，对每个候选关键帧，用ransac和EPNP估计位姿，然后更新当前帧的地图点匹配，然后优化位姿，如果内点较少，则通过投影的方式对之前未匹配的点进行匹配，再进行优化求解，直到有足够的内点支持，重定位完成。
下面简要梳理重定位的过程——
- 用关键帧库的DetectRelocalizationCandidates函数找到当前帧对应的候选关键帧
- 对每个候选关键帧，用SearchByBow匹配当前帧和该关键帧（即更新当前帧特征点对应的地图点，更新mvMapPoints列表）， 用当前帧和当前帧的特征匹配关系来初始化PnPSolver
- 对每个候选关键，用EPNP估计位姿
- 如果估计成功，用估计所得的内点更新当前帧的特征匹配
- 调用PoseOptimization，优化当前帧的位姿
- 优化后，剔除特征匹配的外点（即去除误匹配的地图点）
- 如果优化后的内点过少，用SearchByProjection对未匹配的特征点匹配当前帧与关键帧
- 所有的匹配点（内点加新增的匹配点）大于50，调用PoseOptimization
- 若优化后，新的内点数还是较少（30~50），用更小的窗口再次调用SearchByProjection
- 所有的匹配点数大于50，调用PoseOptimization（最后一次优化），并剔除外点
- 如优化后的内点大于50，中断，重定位成功，当前帧的ID就是最近一次的重定位ID帧
在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;
}
}