27 局部建图线程处理新关键帧、检查删除地图点、关键帧之间生成新的地图点

ORBSLAM2

---

---

代码

线程主函数在localmappin.cc中的run（）函数中，与tracking线程是并行的

void LocalMapping::Run()
{

    mbFinished = false;//标记函数，表示该线程没有结束，正在运行

    while(1)//开始进行循环
    {
        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(false);//设置接收追踪线程中的关键帧，当前localmapping线程处于繁忙状态，不要发送关键帧

        // Check if there are keyframes in the queue
        if(CheckNewKeyFrames())//检查待处理的关键帧列表是否为空
        {
            // BoW conversion and insertion in Map
            ProcessNewKeyFrame();//这个函数是获取关键帧列表中第一个帧为当前帧，包括计算词袋，跟新观测、描述子、共视图、插入到地图等

            // Check recent MapPoints
            MapPointCulling();//根据地图点的观测，删除不好的地图点

            // Triangulate new MapPoints
            CreateNewMapPoints();//当前地图点与相邻地图点进行三角化产生新的地图点

            if(!CheckNewKeyFrames())//处理完关键帧中最后一帧
            {
                // Find more matches in neighbor keyframes and fuse point duplications
                SearchInNeighbors();//检查并融合当前关键帧与相邻关键帧的重复地图点
            }

            mbAbortBA = false;//终止BA

            if(!CheckNewKeyFrames() && !stopRequested())//处理完列队中最后一个关键帧并且闭环检测没有停止local mapping
            {
                // Local BA如果局部地图中的关键帧大于两个，就进行局部BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

                // Check redundant local Keyframes
                KeyFrameCulling();//检测并删除冗余的关键帧
            }

            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);//将关键帧加入闭环检测对列
        }
        else if(Stop())//当要求终止线程的时候，如果列表中为空
        {
            // Safe area to stop如果没有终止，那就进行等待
            while(isStopped() && !CheckFinish())
            {
                usleep(3000);
            }
            if(CheckFinish())//确定终止，就break
                break;
        }

        ResetIfRequested();//查看是否有复位请求

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(true);//可接受新关键帧

        if(CheckFinish())
            break;

        usleep(3000);
    }

    SetFinish();
}

void LocalMapping::ProcessNewKeyFrame()
{
    {
        unique_lock<mutex> lock(mMutexNewKFs);
        mpCurrentKeyFrame = mlNewKeyFrames.front();//获取关键帧列表中的第一个关键帧为当前关键帧
        mlNewKeyFrames.pop_front();//删除列表中第一个关键帧
    }

    // Compute Bags of Words structures
    mpCurrentKeyFrame->ComputeBoW();//计算当前关键帧的词袋模型

    // Associate MapPoints to the new keyframe and update normal and descriptor
    const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();//获取当前关键帧的地图点匹配关系

    for(size_t i=0; i<vpMapPointMatches.size(); i++)//开始遍历每一个匹配好的地图点
    {
        MapPoint* pMP = vpMapPointMatches[i];//取出地图点
        if(pMP)
        {
            if(!pMP->isBad())//判断该点是不是坏点
            {
                if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))//检查该地图点是否在当前的帧中
                {
                    pMP->AddObservation(mpCurrentKeyFrame, i);//如果不是，为他添加观测信息
                    pMP->UpdateNormalAndDepth();//获取平均观测方向
                    pMP->ComputeDistinctiveDescriptors();//获得地图点最佳描述子
                }
                else // this can only happen for new stereo points inserted by the Tracking
                {
                    mlpRecentAddedMapPoints.push_back(pMP);//当前帧包含了地图点，但是地图点中却没有包含关键帧的信息
                }
            }
        }
    }    

    // Update links in the Covisibility Graph
    mpCurrentKeyFrame->UpdateConnections();//跟新关键帧间的共视图

    // Insert Keyframe in Map
    mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

void LocalMapping::MapPointCulling()
{
    // Check Recent Added MapPoints只检查新增的地图点
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();//定义一个迭代器
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;//获取当前关键帧的ID 

    int nThObs;//定义阈值
    if(mbMonocular)//如果单目。为2
        nThObs = 2;
    else//否则为3
        nThObs = 3;
    const int cnThObs = nThObs;

    while(lit!=mlpRecentAddedMapPoints.end())//遍历每一个新增地图点
    {
        MapPoint* pMP = *lit;//取出
        if(pMP->isBad())//判断是不是坏点
        {
            lit = mlpRecentAddedMapPoints.erase(lit);//是的话删除
        }
        else if(pMP->GetFoundRatio()<0.25f )//地图点被多少帧看到（包括普通帧）/地图点 应该看到的次数
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)//从地图点建立开始到现在已经超过两关键帧，且被观测次数少于2
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)//从建立开始，经过三个关键帧，还没被删除，则从列表中删除，说明这个点的质量高
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}