ORB_SLAM2原理及代码解析:LocalMapping 线程——LocalMapping::Run()

目录

1 作用

2 运行逻辑

3 参数及含义

4 代码

5 解析

5.1 线程初始化

5.2 主循环开始

5.2.1 SetAcceptKeyFrames()

5.3 检查关键帧队列

5.3.1 CheckNewKeyFrames()

5.4 处理新关键帧

5.4.1 ProcessNewKeyFrame()

5.4.1.1 ComputeBoW()

5.4.1.2 GetMapPointMatches()

5.4.1.3 AddObservation()

5.4.1.4 UpdateNormalAndDepth()

5.4.1.5 ComputeDistinctiveDescriptors()

5.4.1.6 UpdateConnections()

5.5 清理坏点

5.5.1 MapPointCulling()

5.6 三角化新地图

5.6.1 CreateNewMapPoints()

5.7 搜索并融合邻近关键帧中重复点

5.7.1 CheckNewKeyFrames()

5.7.2 SearchInNeighbors()

5.7.2.1 GetBestCovisibilityKeyFrames()

5.8 局部BA优化+剔除冗余关键帧

5.8.1 CheckNewKeyFrames()

5.8.2 stopRequested()

5.8.3 KeyFramesInMap()

5.8.4 LocalBundleAdjustment()

5.8.5 KeyFrameCulling()

5.9 通知闭环检测线程

5.10 没有新关键帧停止

5.11 重置检查

5.11.1 ResetIfRequested()

5.12 允许接受新关键帧

5.12.1 SetAcceptKeyFrames()

5.13 检查结束条件

5.14 线程休眠与结束

---

1 作用

该函数是局部建图线程的主循环函数，在系统运行期间持续执行，负责：

（1）从队列中接收新关键帧（由 Tracking 插入）；

（2）插入地图并更新局部结构（添加地图点、清理冗余点）；

（3）进行局部优化（Local Bundle Adjustment）；

（4）检查并删除冗余关键帧。

Tracking 生成关键帧 → LocalMapping 优化局部地图 → LoopClosing 检查全局闭环

2 运行逻辑

                     

3 参数及含义

4 代码

void LocalMapping::Run()
{mbFinished = false;while(1){// Tracking will see that Local Mapping is busySetAcceptKeyFrames(false);// Check if there are keyframes in the queueif(CheckNewKeyFrames()){// BoW conversion and insertion in MapProcessNewKeyFrame();// Check recent MapPointsMapPointCulling();// Triangulate new MapPointsCreateNewMapPoints();if(!CheckNewKeyFrames()){// Find more matches in neighbor keyframes and fuse point duplicationsSearchInNeighbors();}mbAbortBA = false;if(!CheckNewKeyFrames() && !stopRequested()){// Local BAif(mpMap->KeyFramesInMap()>2)Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);// Check redundant local KeyframesKeyFrameCulling();}mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);}else if(Stop()){// Safe area to stopwhile(isStopped() && !CheckFinish()){std::this_thread::sleep_for(std::chrono::microseconds(3000));}if(CheckFinish())break;}ResetIfRequested();// Tracking will see that Local Mapping is busySetAcceptKeyFrames(true);if(CheckFinish())break;std::this_thread::sleep_for(std::chrono::microseconds(3000));}SetFinish();
}

5 解析

5.1 线程初始化

    mbFinished = false;

线程开启。

5.2 主循环开始

    while(1){// Tracking will see that Local Mapping is busySetAcceptKeyFrames(false);

不接收关键帧。

5.2.1 SetAcceptKeyFrames()

void LocalMapping::SetAcceptKeyFrames(bool flag)
{unique_lock<mutex> lock(mMutexAccept);mbAcceptKeyFrames=flag;
}

5.3 检查关键帧队列

        // Check if there are keyframes in the queueif(CheckNewKeyFrames())

如果插入新关键帧，则执行下面程序。

5.3.1 CheckNewKeyFrames()

bool LocalMapping::CheckNewKeyFrames()
{unique_lock<mutex> lock(mMutexNewKFs);return(!mlNewKeyFrames.empty());
}

5.4 处理新关键帧

           // BoW conversion and insertion in MapProcessNewKeyFrame();

5.4.1 ProcessNewKeyFrame()

（1）作用

       将从Tracking线程传来的新关键帧正式插入地图，并建立其与已有MapPoints的关联关系，更新共视图（Covisibility Graph），为后续地图点生成、优化和闭环检测打好基础。

（2）参数及含义

（3）声明：LocalMapping.h

    void ProcessNewKeyFrame();

（4）定义：LocalMapping.cc

void LocalMapping::ProcessNewKeyFrame()
{{unique_lock<mutex> lock(mMutexNewKFs);//把当前队列中第一个等待的关键帧地址赋给当前关键帧 mpCurrentKeyFrame = mlNewKeyFrames.front();//从队列中删除这个关键帧，上面已经存储mlNewKeyFrames.pop_front();}// Compute Bags of Words structures//计算该关键帧BoW向量和特征向量，详见5.4.1.1mpCurrentKeyFrame->ComputeBoW();// Associate MapPoints to the new keyframe and update normal and descriptor//从关键帧中提出所有已经匹配的地图点，详见5.4.1.2const 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)){//统计地图点被关键帧观测的次数，详见5.4.1.3pMP->AddObservation(mpCurrentKeyFrame, i);//更新地图点法向量、可观测距离范围，详见5.4.1.4pMP->UpdateNormalAndDepth();//为地图点选择一个具有代表性的描述子，详见5.4.1.5pMP->ComputeDistinctiveDescriptors();}else // this can only happen for new stereo points inserted by the Tracking{mlpRecentAddedMapPoints.push_back(pMP);}}}}    // Update links in the Covisibility GraphmpCurrentKeyFrame->UpdateConnections();// Insert Keyframe in MapmpMap->AddKeyFrame(mpCurrentKeyFrame);
}

5.4.1.1 ComputeBoW()

此处调用的是关键帧词袋计算函数，详见4.1.1.2：

https://blog.csdn.net/weixin_45728280/article/details/152559357?spm=1011.2415.3001.5331

5.4.1.2 GetMapPointMatches()

（1）声明：KeyFrame.h

（2）定义：KeyFrame.cc

vector<MapPoint*> KeyFrame::GetMapPointMatches()
{unique_lock<mutex> lock(mMutexFeatures);return mvpMapPoints;
}

5.4.1.3 AddObservation()

详见2.4.3.3：

https://blog.csdn.net/weixin_45728280/article/details/152323101?spm=1011.2415.3001.5331

5.4.1.4 UpdateNormalAndDepth()

详见：

https://blog.csdn.net/weixin_45728280/article/details/152320646?spm=1011.2415.3001.5331

5.4.1.5 ComputeDistinctiveDescriptors()

详见：

https://blog.csdn.net/weixin_45728280/article/details/152320324?spm=1011.2415.3001.5331

5.4.1.6 UpdateConnections()

详见：

https://blog.csdn.net/weixin_45728280/article/details/152809802?sharetype=blogdetail&sharerId=152809802&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.5 清理坏点

            // Check recent MapPointsMapPointCulling();

5.5.1 MapPointCulling()

（1）作用

       对最近加入的地图点（MapPoints）进行剔除（Culling），去掉那些不可靠或观测太少的点，从而保证地图质量。  

（2）参数及含义

（3）定义

void LocalMapping::MapPointCulling()
{// Check Recent Added MapPointslist<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;//判定新加进来的地图点是否可靠的最少观测次数int nThObs;if(mbMonocular)nThObs = 2;elsenThObs = 3;const int cnThObs = nThObs;while(lit!=mlpRecentAddedMapPoints.end()){MapPoint* pMP = *lit;if(pMP->isBad()){lit = mlpRecentAddedMapPoints.erase(lit);}//GetFoundRatio()表示该MapPoint被Tracking成功匹配的比例else if(pMP->GetFoundRatio()<0.25f ){pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);}//如果当前关键帧ID与首次创建关键帧ID相差≥2，并且该点的观测次数≤cnThObselse if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs){pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);}//如果当前关键帧ID与首次创建关键帧ID相差≥3else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)lit = mlpRecentAddedMapPoints.erase(lit);elselit++;}
}

5.6 三角化新地图

            // Triangulate new MapPointsCreateNewMapPoints();

5.6.1 CreateNewMapPoints()

详见：

https://blog.csdn.net/weixin_45728280/article/details/153046478?sharetype=blogdetail&sharerId=153046478&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.7 搜索并融合邻近关键帧中重复点

           if(!CheckNewKeyFrames()){// Find more matches in neighbor keyframes and fuse point duplicationsSearchInNeighbors();}

5.7.1 CheckNewKeyFrames()

（1）作用

  检测 Tracking 线程是否已经插入新的关键帧。

（2）代码

bool LocalMapping::CheckNewKeyFrames()
{unique_lock<mutex> lock(mMutexNewKFs);return(!mlNewKeyFrames.empty());
}

5.7.2 SearchInNeighbors()

（1）作用

  在当前关键帧及其邻居关键帧之间，通过投影匹配与几何约束；
  融合重复的地图点（MapPoints），更新描述子与连接关系；
  从而优化共视图（Covisibility Graph）结构。

（2）参数及含义

（3）代码

void LocalMapping::SearchInNeighbors()
{// Retrieve neighbor keyframesint nn = 10;if(mbMonocular)nn=20;const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);//实际用于匹配和融合的目标关键帧集合vector<KeyFrame*> vpTargetKFs;for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;//mnFuseTargetForKF：标志某地图点是否已作为候选点使用if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi);pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// Extend to some second neighbors，扩展到邻居的邻居const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++){KeyFrame* pKFi2 = *vit2;if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi2);}}// Search matches by projection from current KF in target KFsORBmatcher matcher;vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;matcher.Fuse(pKFi,vpMapPointMatches);}// Search matches by projection from target KFs in current KFvector<MapPoint*> vpFuseCandidates;vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++){KeyFrame* pKFi = *vitKF;vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++){MapPoint* pMP = *vitMP;if(!pMP)continue;if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)continue;pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;vpFuseCandidates.push_back(pMP);}}matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);// Update pointsvpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++){MapPoint* pMP=vpMapPointMatches[i];if(pMP){if(!pMP->isBad()){pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();}}}// Update connections in covisibility graphmpCurrentKeyFrame->UpdateConnections();
}

5.7.2.1 GetBestCovisibilityKeyFrames()

详见4.1.1：

https://blog.csdn.net/weixin_45728280/article/details/153046478?spm=1011.2415.3001.5331

5.8 局部BA优化+剔除冗余关键帧

            mbAbortBA = false;if(!CheckNewKeyFrames() && !stopRequested()){// Local BAif(mpMap->KeyFramesInMap()>2)Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);// Check redundant local KeyframesKeyFrameCulling();}

5.8.1 CheckNewKeyFrames()

详见本文5.3.1。

5.8.2 stopRequested()

（1）作用

  请求停止。

（2）代码

bool LocalMapping::stopRequested()
{unique_lock<mutex> lock(mMutexStop);return mbStopRequested;
}

5.8.3 KeyFramesInMap()

（1）作用

  关键帧数量。

（2）声明：Map.h

（3）代码：Map.cc

long unsigned int Map::KeyFramesInMap()
{unique_lock<mutex> lock(mMutexMap);return mspKeyFrames.size();
}

5.8.4 LocalBundleAdjustment()

详见：

https://blog.csdn.net/weixin_45728280/article/details/153055742?sharetype=blogdetail&sharerId=153055742&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.8.5 KeyFrameCulling()

（1）作用

       在当前局部地图中，检查相邻关键帧是否冗余；如果某个关键帧看到的地图点中 90% 以上也被其他至少 3 个关键帧看到，就认为它冗余，标记为“坏”关键帧。

（2）代码

void LocalMapping::KeyFrameCulling()
{// Check redundant keyframes (only local keyframes)// A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen// in at least other 3 keyframes (in the same or finer scale)// We only consider close stereo points//获取与当前关键帧共视的关键帧集合vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++){KeyFrame* pKF = *vit;//跳过ID==0的关键帧，前面是基于这个点建立的世界坐标if(pKF->mnId==0)continue;//获取关键帧能看到的所有地图点const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();int nObs = 3;const int thObs=nObs;int nRedundantObservations=0;int nMPs=0;for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++){MapPoint* pMP = vpMapPoints[i];if(pMP){if(!pMP->isBad()){if(!mbMonocular){//当前关键帧中第i个特征点深度大于阀值if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)continue;}nMPs++;//当前地图点被关键帧观测到的数大于阀值if(pMP->Observations()>thObs){const int &scaleLevel = pKF->mvKeysUn[i].octave;const map<KeyFrame*, size_t> observations = pMP->GetObservations();int nObs=0;//检查当前关键帧中某个地图点是否已经被“足够多的其他关键帧”在相似的尺度层级下观测到了for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){//跳过自己KeyFrame* pKFi = mit->first;if(pKFi==pKF)continue;const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;//如果其他关键帧层级<=当前关键帧层级+1if(scaleLeveli<=scaleLevel+1){nObs++;if(nObs>=thObs)break;}}if(nObs>=thObs){//冗余关键帧计数nRedundantObservations++;}}}}}  //如果关键帧中90%的地图点被其他关键帧观测，就判定为冗余关键帧if(nRedundantObservations>0.9*nMPs)pKF->SetBadFlag();}
}

5.9 通知闭环检测线程

           mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

5.10 没有新关键帧停止

        else if(Stop()){// Safe area to stopwhile(isStopped() && !CheckFinish()){std::this_thread::sleep_for(std::chrono::microseconds(3000));}if(CheckFinish())break;}

5.11 重置检查

        ResetIfRequested();

5.11.1 ResetIfRequested()

void LocalMapping::ResetIfRequested()
{unique_lock<mutex> lock(mMutexReset);if(mbResetRequested){mlNewKeyFrames.clear();mlpRecentAddedMapPoints.clear();mbResetRequested=false;}
}

5.12 允许接受新关键帧

        // Tracking will see that Local Mapping is busySetAcceptKeyFrames(true);

5.12.1 SetAcceptKeyFrames()

void LocalMapping::SetAcceptKeyFrames(bool flag)
{unique_lock<mutex> lock(mMutexAccept);mbAcceptKeyFrames=flag;
}

5.13 检查结束条件

        if(CheckFinish())break;

5.14 线程休眠与结束

        std::this_thread::sleep_for(std::chrono::microseconds(3000));}//线程标记为已完成SetFinish();
}