ORB SLAM的local mapping部分的代码解析

  从ORB SLAM的系统框架中就可以很清楚知道Local Mapping部分主要的实现。这个线程主要通过MapPoints维护关键帧之间的共视关系(covisibility),通过局部BA优化共视关键帧位姿和MapPoints。

如上图所示,Local Mapping 主要完成以下几个操作,所有操作在LocalMapping::Run()成员函数中被调用:
1、关键帧的插入
2、地图点的删除
3、新地图点的插入
4、局部地图点的融合
5、局部的Bundle Ajustment
6、局部关键帧的删除

下面就从这五个方面来说明代码中具体是怎么实现的。

1、关键帧的插入(ProcessNewKeyFrame())
  在Tracking线程挑选出 keyframe了之后(挑选keyframe的条件可以见[1]),Local Mapping的线程就要把该关键帧插入到Local Map中去。在代码中,会有一个List来管理keyframes,所以Local Mapping线程首先会通过CheckNewKeyFrames()函数检查这个队列里面有没有keyframes,如果有keyframes,则调用ProcessNewKeyFrame()来完成关键帧的插入。

代码解释:
  如上图所示,要插入的当前的keyframe帧为mpCurrentKeyframe,它是一个keyframe的指针变量,最开始就把这一帧加入keyframes的队列里面,然后计算mpCurrentKeyframe的BOW,然后找出地图中的点(MapPoints)同时也在mpCurrentKeyframe中的,给这些点添加约束关系(AddObservation之类的)。最后给这一帧找到共视帧,并且加入到map中去。

2、地图点的删除
  在上一步中会将一部分地图点加入到mlpRecentAddedMapPoints这个vector中,这一步需要做的就是剔除地图中新加入的但是质量不好的点。

如上图所示,评价一个点需要被删除掉的条件为:
(1)如果这个点是bad的
(2)IncreaseFound / IncreaseVisible < 0.25
(3)观测到该点的关键帧数量太少

3、新地图点的插入(CreateNewMapPoints())
  在将一些不好的点删除之后,接下来做的就是在地图中插入新的点。所有功能在CreateNewMapPoints()函数中完成的。
  首先通过下面的函数GetBestCovisibilityKeyFrames()来获得共视程度大的keyframes

const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

  然后通过下列代码先计算出当前的keyframe和有共视的keyframes之间的baseline,如果baseline太小,则跳过换下一个keyframe来进行三角化。

for(size_t i=0; i<vpNeighKFs.size(); i++)
    {
        if(i>0 && CheckNewKeyFrames())
            return;

        KeyFrame* pKF2 = vpNeighKFs[i];

        // Check first that baseline is not too short
        cv::Mat Ow2 = pKF2->GetCameraCenter();
        cv::Mat vBaseline = Ow2-Ow1;
        const float baseline = cv::norm(vBaseline);

        if(!mbMonocular)
        {
            if(baseline<pKF2->mb)
            continue;
        }

  再计算当前keyframe和对比的keyframe之间的F矩阵,挑选出满足极线约束的一些匹配点,具体实现如下所示:

        cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

        // Search matches that fullfil epipolar constraint
        vector<pair<size_t,size_t> > vMatchedIndices;
        matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

  找到匹配点之后就进行三角化了。对每一对匹配点算它们的parallax,求解出深度(代码中是z1和z2)

for(int ikp=0; ikp<nmatches; ikp++)
        {
            const int &idx1 = vMatchedIndices[ikp].first;
            const int &idx2 = vMatchedIndices[ikp].second;

  求解出深度之后要重投影到两帧中,计算当前keyframe到之前keyframe的重投影误差和之前keyframe到当前keyframe的重投影误差。如果误差过大则舍弃当前这对匹配点。否则就将恢复出来的点加入到地图点中去。

//Check reprojection error in first keyframe
    if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
    continue;
//Check reprojection error in second keyframe
    if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
    continue;
// Triangulation is succesfull
            MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);

            pMP->AddObservation(mpCurrentKeyFrame,idx1);            
            pMP->AddObservation(pKF2,idx2);

            mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
            pKF2->AddMapPoint(pMP,idx2);

            pMP->ComputeDistinctiveDescriptors();

            pMP->UpdateNormalAndDepth();

            mpMap->AddMapPoint(pMP);
            mlpRecentAddedMapPoints.push_back(pMP);

            nnew++;

4、局部地图点的融合
  产生新的地图点之后,需要检查当前keyframe和之前的keyframe之间的相同的MapPoints,并且进行融合,整个操作是在SearchInNeighbors()函数中进行的。具体解释如下:

  如上图所示,首先是将当前keyframe的相邻的关键帧加入到vpTargetKFs这个vector中,然后二级搜索,即把当前keyframe相邻的关键帧再相邻的关键帧也找出来,加入到vpTargetKFs这个vector中。

  然后通过两帧之间相互投影,计算出待fuse的点加入到vpFuseCandidates这个vector中,再进行一次matcher的fuse。然后再进行一次update,如下图所示,计算出融合后的深度。

5、局部的Bundle Ajustment
  局部BA就是在做当前keyframe相连的关键帧及MapPoints做局部的BA优化,优化工具是g2o,定义好了顶点和边之后就可以进行优化了。这一部分具体不多讲了,详细的看看源代码就知道了。

6、局部关键帧的删除
  在local keyframes中剔除冗余的关键帧,剔除冗余的关键帧的条件是:
  如果一个keyframes可以看到的MapPoints 90%以上都可以在其他至少三个关键帧中找到,那么就删除这个关键帧。

        if(nRedundantObservations>0.9*nMPs)
            pKF->SetBadFlag();

参考文献:
[1] ORB SLAM的tracking部分代码解析
[2]【泡泡机器人公开课】第三十六课:ORB-SLAM2源码详解 by 吴博
[3] https://github.com/raulmur/ORB_SLAM2
[4] Mur-Artal, Raul, Jose Maria Martinez Montiel, and Juan D. Tardos. “ORB-SLAM: a versatile and accurate monocular    SLAM system.” IEEE Transactions on Robotics 31.5 (2015): 1147-1163.
[5] Mur-Artal, Raul, and Juan D. Tardós. “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras.” IEEE Transactions on Robotics 33.5 (2017): 1255-1262.

猜你喜欢

转载自blog.csdn.net/osgoodwu/article/details/80091017