目录
5.4.1.4 UpdateNormalAndDepth()
5.4.1.5 ComputeDistinctiveDescriptors()
5.7.2.1 GetBestCovisibilityKeyFrames()
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 busy SetAcceptKeyFrames(false); // 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; if(!CheckNewKeyFrames() && !stopRequested()) { // Local 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()) { std::this_thread::sleep_for(std::chrono::microseconds(3000)); } if(CheckFinish()) break; } ResetIfRequested(); // Tracking will see that Local Mapping is busy SetAcceptKeyFrames(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 busy SetAcceptKeyFrames(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 queue if(CheckNewKeyFrames())如果插入新关键帧，则执行下面程序。
5.3.1 CheckNewKeyFrames()
bool LocalMapping::CheckNewKeyFrames() { unique_lock<mutex> lock(mMutexNewKFs); return(!mlNewKeyFrames.empty()); }
5.4 处理新关键帧
// BoW conversion and insertion in Map ProcessNewKeyFrame();
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.1 mpCurrentKeyFrame->ComputeBoW(); // Associate MapPoints to the new keyframe and update normal and descriptor //从关键帧中提出所有已经匹配的地图点，详见5.4.1.2 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)) { //统计地图点被关键帧观测的次数，详见5.4.1.3 pMP->AddObservation(mpCurrentKeyFrame, i); //更新地图点法向量、可观测距离范围，详见5.4.1.4 pMP->UpdateNormalAndDepth(); //为地图点选择一个具有代表性的描述子，详见5.4.1.5 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); }
5.4.1.1 ComputeBoW()
此处调用的是关键帧词袋计算函数，详见4.1.1.2：
https://blog.youkuaiyun.com/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.youkuaiyun.com/weixin_45728280/article/details/152323101?spm=1011.2415.3001.5331
5.4.1.4 UpdateNormalAndDepth()
详见：
https://blog.youkuaiyun.com/weixin_45728280/article/details/152320646?spm=1011.2415.3001.5331
5.4.1.5 ComputeDistinctiveDescriptors()
详见：
https://blog.youkuaiyun.com/weixin_45728280/article/details/152320324?spm=1011.2415.3001.5331
5.4.1.6 UpdateConnections()
详见：
5.5 清理坏点
// Check recent MapPoints MapPointCulling();
5.5.1 MapPointCulling()
（1）作用
对最近加入的地图点（MapPoints）进行剔除（Culling），去掉那些不可靠或观测太少的点，从而保证地图质量。
（2）参数及含义
（3）定义
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; 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，并且该点的观测次数≤cnThObs else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs) { pMP->SetBadFlag(); lit = mlpRecentAddedMapPoints.erase(lit); } //如果当前关键帧ID与首次创建关键帧ID相差≥3 else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3) lit = mlpRecentAddedMapPoints.erase(lit); else lit++; } }
5.6 三角化新地图
// Triangulate new MapPoints CreateNewMapPoints();
5.6.1 CreateNewMapPoints()
详见：
5.7 搜索并融合邻近关键帧中重复点
if(!CheckNewKeyFrames()) { // Find more matches in neighbor keyframes and fuse point duplications SearchInNeighbors(); }
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 keyframes int 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 KFs ORBmatcher 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 KF vector<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 points vpMapPointMatches = 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 graph mpCurrentKeyFrame->UpdateConnections(); }
5.7.2.1 GetBestCovisibilityKeyFrames()
详见4.1.1：
https://blog.youkuaiyun.com/weixin_45728280/article/details/153046478?spm=1011.2415.3001.5331
5.8 局部BA优化+剔除冗余关键帧
mbAbortBA = false; if(!CheckNewKeyFrames() && !stopRequested()) { // Local BA if(mpMap->KeyFramesInMap()>2) Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap); // Check redundant local Keyframes KeyFrameCulling(); }
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()
详见：
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; //如果其他关键帧层级<=当前关键帧层级+1 if(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 stop while(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 busy SetAcceptKeyFrames(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(); }
扫一扫
953
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
