1. DetectLoop
这里有个ConsistenGroup概念,比较难懂。这里是最让人迷惑的地方。一旦vbConsistentGroup为真,其他帧的spCanditateGroup就进不来了。
if(!vbConsistentGroup[iG])
{
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
vCurrentConsistentGroups.push_back(cg);
vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
}
2. ComputeSim3
不太明白为什么都使用Sim3去计算相对位姿。在双目和RGBD情况下,地图点深度已知,可以不优化Scale的啊。关于sim3的优化,请参考此博客与paper:sim3优化,paper。这里面用到了三种不同的匹配方法:
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
//为了找到更多的匹配点,用于优化
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
//从回环帧和它的邻域帧里,得到更多地图点,并与当前帧匹配,如果内点多于某个阈值,则接受这个loop
// Retrieve MapPoints seen in Loop Keyframe and neighbors
vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
vpLoopConnectedKFs.push_back(mpMatchedKF);
mvpLoopMapPoints.clear();
for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
{
KeyFrame* pKF = *vit;
vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
{
mvpLoopMapPoints.push_back(pMP);
pMP->mnLoopPointForKF=mpCurrentKF->mnId;
}
}
}
}
// Find more matches projecting with the computed Sim3
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
3. EssentialGraph 的意义
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
只优化位姿,里面包含当前回环边信息,所有帧与父边信息,它的回环边信息,以及共视边信息。优化所有帧位姿。