欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

重读ORB_SLAM之LoopClosing线程难点

程序员文章站 2024-03-25 23:47:28
...

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

只优化位姿,里面包含当前回环边信息,所有帧与父边信息,它的回环边信息,以及共视边信息。优化所有帧位姿。