在SYSTEM类中,开了新线程,一直调用LocalMapping中的Run函数

mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);

第一步,用InsertKeyFrame往mlploopkeyframequeue队列中插入pKF

void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    if(pKF->mnId!=0)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

存在需要处理后的KF后,调用ProcessNewKeyFrame(),主要作用是更新地图点的观测数据,地图点记录关键帧对其的观测,并更新帧之间的共视关系,将关键帧插入地图中

void LocalMapping::ProcessNewKeyFrame()
{
    {
        unique_lock<mutex> lock(mMutexNewKFs);
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        mlNewKeyFrames.pop_front();
    }

    // Compute Bags of Words structures
    //计算当前关键帧的词袋模型。1:计算某一描述子的位置。2.压缩整张图片的表示
    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
    mpAtlas->AddKeyFrame(mpCurrentKeyFrame);
}

MapPointCulling:对地图点进行处理,删除不满足要求的地图点

void LocalMapping::MapPointCulling()
{
    // Check Recent Added MapPoints
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;

    int borrar = mlpRecentAddedMapPoints.size();

    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)
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
        {
            lit++;
            borrar--;
        }
    }
}

CreateNewMapPoints()检查相机基线,如果视差明显,将ORB提取的两关键帧进行匹配,通过对体几何实现2D to 3D,来创建新的地图点

SearchInNeighbors(),用于更新共视图何地图点的融合和修复

LocalBundleAdjustment:使用g2o来优化关键帧的位资和地图点的坐标

KeyFrameCulling()遍历邻居关键帧,通过地图点来判断是否为冗余关键帧(一个关键帧上的90%地图点能被其他多个关键帧观测到)

ResetIfRequested();判断系统是否需要重置

更多推荐