pdf版本笔记的下载地址: ORB-SLAM2代码详解08_局部建图线程LocalMapping,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

可以看看我录制的视频5小时让你假装大概看懂ORB-SLAM2源码

各成员函数/变量

Tracking线程创建的所有关键帧都被插入到LocalMapping线程的缓冲队列mlNewKeyFrames中.

成员函数mbAcceptKeyFrames表示当前LocalMapping线程是否愿意接收关键帧,这会被Tracking线程函数Tracking::NeedNewKeyFrame()用作是否生产关键帧的参考因素之一;但即使mbAcceptKeyFrames为false,在系统很需要关键帧的情况下Tracking线程函数Tracking::NeedNewKeyFrame()也会决定生成关键帧.

局部建图主函数: Run()

函数LocalMapping::Run()是LocalMapping线程的主函数,该函数内部是一个死循环,每3毫秒查询一次当前线程缓冲队列mlNewKeyFrames.若查询到了待处理的新关键帧,就进行查询

void LocalMapping::Run() {while (1) {SetAcceptKeyFrames(false);     // 设置当前LocalMapping线程处于建图状态,不愿意接受Tracking线程传来的关键帧// step1. 检查缓冲队列内的关键帧if (CheckNewKeyFrames()) {// step2. 处理缓冲队列中第一个关键帧ProcessNewKeyFrame();// step3. 剔除劣质地图点MapPointCulling();// step4. 创建新地图点CreateNewMapPoints();if (!CheckNewKeyFrames()) {// step5. 将当前关键帧与其共视关键帧地图点融合SearchInNeighbors();// step6. 局部BA优化: 优化局部地图mbAbortBA = false;   Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);// step7. 剔除冗余关键帧KeyFrameCulling();}// step8. 将当前关键帧加入闭环检测中mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);} SetAcceptKeyFrames(true);       // 设置当前LocalMapping线程处于空闲状态,愿意接受Tracking线程传来的关键帧// 线程暂停3毫秒再开启下一轮查询std::this_thread::sleep_for(std::chrono::milliseconds(3));}
}

处理队列中第一个关键帧: ProcessNewKeyFrame()

在第3步中处理当前关键点时比较有意思,通过判断该地图点是否观测到当前关键帧(pMP->IsInKeyFrame(mpCurrentKeyFrame))来判断该地图点是否是当前关键帧中新生成的.

• 若地图点是本关键帧跟踪过程中匹配得到的(Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()和Tracking::SearchLocalPoints()中调用了ORBmatcher::SearchByProjection()和ORBmatcher::SearchByBoW()方法),则是之前关键帧中创建的地图点,只需添加其对当前帧的观测即可.

• 若地图点是本关键帧跟踪过程中新生成的(包括:1.单目或双目初始化Tracking::MonocularInitialization()、Tracking::StereoInitialization();2.创建新关键帧Tracking::CreateNewKeyFrame()),则该地图点中有对当前关键帧的观测,是新生成的地图点,放入容器mlNewKeyFrames中供LocalMapping::MapPointCulling()函数筛选.

void LocalMapping::ProcessNewKeyFrame() {// step1. 取出队列头的关键帧{unique_lock<mutex> lock(mMutexNewKFs);mpCurrentKeyFrame = mlNewKeyFrames.front();mlNewKeyFrames.pop_front();}// step2. 计算该关键帧的词向量mpCurrentKeyFrame->ComputeBoW();// step3. 根据地图点中是否观测到当前关键帧判断该地图是是否是新生成的const vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for (size_t i = 0; i < vpMapPointMatches.size(); i++) {MapPoint *pMP = vpMapPointMatches[i];if (pMP && !pMP->isBad()) {if (!pMP->IsInKeyFrame(mpCurrentKeyFrame)) {// step3.1. 该地图点是跟踪本关键帧时匹配得到的,在地图点中加入对当前关键帧的观测pMP->AddObservation(mpCurrentKeyFrame, i);pMP->UpdateNormalAndDepth();pMP->ComputeDistinctiveDescriptors();} else // this can only happen for new stereo points inserted by the Tracking{// step3.2. 该地图点是跟踪本关键帧时新生成的,将其加入容器mlpRecentAddedMapPoints待筛选mlpRecentAddedMapPoints.push_back(pMP);}}}// step4. 更新共视图关系mpCurrentKeyFrame->UpdateConnections();// step5. 将关键帧插入到地图中mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

剔除坏地图点: MapPointCulling()

冗余地图点的标准:满足以下其中之一就算是坏地图点

1. 召回率=实际观测到该地图点的帧数mnFound理论上应当观测到该地图点的帧数mnVisible<0.25=\frac{实际观测到该地图点的帧数mnFound}{理论上应当观测到该地图点的帧数mnVisible} < 0.25=理论上应当观测到该地图点的帧数mnVisible实际观测到该地图点的帧数mnFound​<0.25
2. 在创建的3帧内观测数目少于2(双目为3)

若地图点经过了连续3个关键帧仍未被剔除,则被认为是好的地图点

void LocalMapping::MapPointCulling() {list<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()) {// 标准0: 地图点在其他地方被删除了lit = mlpRecentAddedMapPoints.erase(lit);} else if (pMP->GetFoundRatio() < 0.25f) {// 标准1: 召回率<0.25pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);} else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs) {// 标准2: 从创建开始连续3个关键帧内观测数目少于cnThObspMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);} else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 3)// 通过了3个关键帧的考察,认为是好的地图点lit = mlpRecentAddedMapPoints.erase(lit);elselit++;}
}

MapPoint类中关于召回率的成员函数和变量如下:

这两个成员变量主要用于Tracking线程.

• 在函数Tracking::SearchLocalPoints()中,会对所有处于当前帧视锥内的地图点调用成员函数MapPoint::IncreaseVisible().(这些点未必真的被当前帧观测到了,只是地理位置上处于当前帧视锥范围内).

  void Tracking::SearchLocalPoints() {// 当前关键帧的地图点for (MapPoint *pMP : mCurrentFrame.mvpMapPoints) {pMP->IncreaseVisible();}}}// 局部关键帧中不属于当前帧,但在当前帧视锥范围内的地图点for (MapPoint *pMP = *vit : mvpLocalMapPoints.begin()) {if (mCurrentFrame.isInFrustum(pMP, 0.5)) {pMP->IncreaseVisible();}}// ...
  }
  

• 在函数Tracking::TrackLocalMap()中,会对所有当前帧观测到的地图点调用MaoPoint::IncreaseFound().

  bool Tracking::TrackLocalMap() {// ...for (int i = 0; i < mCurrentFrame.N; i++) {if (mCurrentFrame.mvpMapPoints[i]) {if (!mCurrentFrame.mvbOutlier[i]) {// 当前帧观测到的地图点mCurrentFrame.mvpMapPoints[i]->IncreaseFound();// ...}}}// ...
  }
  

创建新地图点: CreateNewMapPoints()

将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配,生成地图点.

对于双目相机的匹配特征点对,可以根据某帧特征点深度恢复地图点,也可以根据两帧间对极几何三角化地图点,这里取视差角最大的方式来生成地图点.

融合当前关键帧和其共视帧的地图点: SearchInNeighbors()

本函数将当前关键帧与其一级和二级共视关键帧做地图点融合,分两步:

1. 正向融合: 将当前关键帧的地图点融合到各共视关键帧中.
2. 反向融合: 将各共视关键帧的地图点融合到当前关键帧中.

void LocalMapping::SearchInNeighbors() {// step1. 取当前关键帧的一级共视关键帧const vector<KeyFrame *> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(10);// step2. 遍历一级关键帧,寻找二级关键帧vector<KeyFrame *> vpTargetKFs;for (KeyFrame *pKFi : vpNeighKFs) {if (pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi);pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;const vector<KeyFrame *> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);for (KeyFrame *pKFi2 : vpSecondNeighKFs) {if (pKFi2->isBad() || pKFi2->mnFuseTargetForKF == mpCurrentKeyFrame->mnId || pKFi2->mnId == mpCurrentKeyFrame->mnId)continue;vpTargetKFs.push_back(pKFi2);}}// step3. 正向融合: 将当前帧的地图点融合到各共视关键帧中vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();ORBmatcher matcher;for (KeyFrame *pKFi : vpTargetKFs) {matcher.Fuse(pKFi, vpMapPointMatches);}// step4. 反向融合: 将各共视关键帧的地图点融合到当前关键帧中// step4.1. 取出各共视关键帧的地图点存入vpFuseCandidatesvector<MapPoint *> vpFuseCandidates;for (KeyFrame *pKFi : vpTargetKFs) {vector<MapPoint *> vpMapPointsKFi = pKFi->GetMapPointMatches();for (MapPoint *pMP : vpMapPointsKFi.begin()) {if (!pMP || pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)continue;pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;vpFuseCandidates.push_back(pMP);}}// step 4.2. 进行反向融合matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates);// step5. 更新当前关键帧的地图点信息vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for (MapPoint *pMP : vpMapPointMatches) {if (pMP and !pMP->isBad()) {pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();}}// step6. 更新共视图mpCurrentKeyFrame->UpdateConnections();
}

ORBmatcher::Fuse()将地图点与帧中图像的特征点匹配,实现地图点融合.

在将地图点反投影到帧中的过程中,存在以下两种情况:

1. 若地图点反投影对应位置上不存在地图点,则直接添加观测.
2. 若地图点反投影位置上存在对应地图点,则将两个地图点合并到其中观测较多的那个.

int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th) {// 遍历所有的待投影地图点for(MapPoint* pMP : vpMapPoints) {// step1. 将地图点反投影到相机成像平面上const float invz = 1/p3Dc.at<float>(2);const float x = p3Dc.at<float>(0)*invz;const float y = p3Dc.at<float>(1)*invz;const float u = fx*x+cx;const float v = fy*y+cy;const float ur = u-bf*invz;const float maxDistance = pMP->GetMaxDistanceInvariance();const float minDistance = pMP->GetMinDistanceInvariance();cv::Mat PO = p3Dw-Ow;const float dist3D = cv::norm(PO);// step2. 地图点观测距离if(dist3D<minDistance || dist3D>maxDistance )continue;// step3. 地图点的观测距离和观测方向不能太离谱if (dist3D < minDistance || dist3D > maxDistance)continue;cv::Mat Pn = pMP->GetNormal();if (PO.dot(Pn) < 0.5 * dist3D)continue;// step4. 在投影位置寻找图像特征点int nPredictedLevel = pMP->PredictScale(dist3D, pKF);const float radius = th * pKF->mvScaleFactors[nPredictedLevel];const vector<size_t> vIndices = pKF->GetFeaturesInArea(u, v, radius);const cv::Mat dMP = pMP->GetDescriptor();int bestDist = 256;int bestIdx = -1;for (size_t idx : vIndices) {const size_t idx = *vit;const cv::KeyPoint &kp = pKF->mvKeysUn[idx];const int &kpLevel = kp.octave;// step4.1. 金字塔层级要接近if (kpLevel < nPredictedLevel - 1 || kpLevel > nPredictedLevel)continue;// step4.2. 使用卡方检验检查重投影误差,单目和双目的自由度不同if (pKF->mvuRight[idx] >= 0) {const float ex = u - kp.pt.x;const float ey = v - kp.pt.y;const float er = ur - pKF->mvuRight[idx];const float e2 = ex * ex + ey * ey + er * er;if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 7.8)continue;} else {const float ex = u - kp.pt.x;const float ey = v - kp.pt.y;const float e2 = ex * ex + ey * ey;if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 5.99)continue;}// step4.3. 检验描述子距离const cv::Mat &dKF = pKF->mDescriptors.row(idx);const int dist = DescriptorDistance(dMP, dKF);if (dist < bestDist) {bestDist = dist;bestIdx = idx;}}// step5. 与最近特征点的描述子距离足够小,就进行地图点融合if (bestDist <= TH_LOW) {MapPoint *pMPinKF = pKF->GetMapPoint(bestIdx);if (pMPinKF) {// step5.1. 地图点反投影位置上存在对应地图点,则将两个地图点合并到其中观测较多的那个则直接添加观测if (!pMPinKF->isBad()) {if (pMPinKF->Observations() > pMP->Observations())pMP->Replace(pMPinKF);elsepMPinKF->Replace(pMP);}} else {// step5.2. 地图点反投影对应位置上不存在地图点,pMP->AddObservation(pKF, bestIdx);pKF->AddMapPoint(pMP, bestIdx);}nFused++;}}return nFused;
}

局部BA优化: Optimizer::LocalBundleAdjustment()

局部BA优化当前帧的局部地图.

• 当前关键帧的一级共视关键帧位姿会被优化;二极共视关键帧会加入优化图,但其位姿不会被优化.

• 所有局部地图点位姿都会被优化.

Tracking线程中定义了局部地图成员变量mvpLocalKeyFrames和mvpLocalMapPoints,但是这些变量并没有被LocalMapping线程管理,因此在函数Optimizer::LocalBundleAdjustment()中还要重新构造局部地图变量,这种设计有些多此一举了.

剔除冗余关键帧: KeyFrameCulling()

冗余关键帧标准: 90%以上的地图点能被超过3个其他关键帧观测到.

void LocalMapping::KeyFrameCulling() {// step1. 遍历当前关键帧的所有共视关键帧vector<KeyFrame *> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();for (KeyFrame *pKF : vpLocalKeyFrames) {// step2. 遍历所有局部地图点const vector<MapPoint *> vpMapPoints = pKF->GetMapPointMatches();int nRedundantObservations = 0;int nMPs = 0;for (MapPoint *pMP : vpMapPoints) {if (pMP && !pMP->isBad()) {if (!mbMonocular) {// 双目相机只能看到不超过相机基线35倍的地图点if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)continue;}nMPs++;int nObs = 0;for (KeyFrame *pKFi : pMP->GetObservations()) {= mit->first;if (pKFi->mvKeysUn[mit->second].octave <= pKF->mvKeysUn[i].octave + 1) {nObs++;if (nObs >= 3)break;}}if (nObs >= 3) {nRedundantObservations++;}}}}// step3. 若关键帧超过90%的地图点能被超过3个其它关键帧观测到,则视为冗余关键帧if (nRedundantObservations > 0.9 * nMPs)pKF->SetBadFlag();
}

pdf版本笔记的下载地址: ORB-SLAM2代码详解08_局部建图线程LocalMapping,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

ORB-SLAM2代码详解08: 局部建图线程LocalMapping相关推荐

1. (01)ORB-SLAM2源码无死角解析-(63) BA优化(g2o)→局部建图线程:Optimizer::LocalBundleAdjustment→位姿与地图点优化

   讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (0 ...

2. ORB_SLAM2局部建图线程

     局部建图线程入口:可执行程序在初始化三个线程的时候,在System.cc的构造函数中进入局部建图线程 mpLocalMapper = new LocalMapping(mpMap, //指定使io ...

3. 【论文必用】模糊C均值聚类的简单介绍、复现及Python代码详解、聚类可视化图的绘制过程详解！

   详解模糊C均值聚类 一.聚类 二.模糊C均值聚类 三.模糊C均值聚类的Python实现 四.参考链接 一.聚类 聚类的定义: 将物理或抽象对象的集合分成由类似的对象组成的多个类的过程被称为聚类.由聚类 ...

4. orbslam2代码详解之tracking线程——局部地图跟踪

   目录 局部地图跟踪 TrackLocalMap() UpdateLocalMap() UpdateLocalKeyFrames() UpdateLocalPoints() SearchLocalPoi ...

5. 深入浅出吃透多线程、线程池核心原理及代码详解

   一.多线程详解 1.什么是线程 线程是一个操作系统概念.操作系统负责这个线程的创建.挂起.运行.阻塞和终结操作.而操作系统创建线程.切换线程状态.终结线程都要进行CPU调度--这是一个耗费时间和系统资 ...

6. DeepLearning tutorial（4）CNN卷积神经网络原理简介+代码详解

   FROM: http://blog.csdn.net/u012162613/article/details/43225445 DeepLearning tutorial(4)CNN卷积神经网络原理简介 ...

7. java编程数据溢出问题_Java数据溢出代码详解

   Java数据溢出代码详解 发布时间:2020-10-05 15:08:31 来源:脚本之家 阅读:103 作者:Pony小马 java是一门相对安全的语言,那么数据溢出时它是如何处理的呢? 看一段代码 ...

8. socket 获取回传信息_Luat系列官方教程5：Socket代码详解

   文章篇幅较长,代码部分建议横屏查看,或在PC端打开本文链接.文末依然为爱学习的你准备了专属福利~ TCP和UDP除了在Lua代码声明时有一些不同,其他地方完全一样,所以下面的代码将以TCP长连接的数据 ...

9. 来FAL学风控|风控策略分析师的日常是怎样的？（案例+代码详解篇）

   风控策略分析师的日常是怎样的?(案例+代码详解篇) FAL金科应用研究院 做了5年的金融,3年的数据分析工作,从17年6月才真正接触代码,算不到熟练,但在不断的学习和工作实践中目前是可以解决任何问题的 ...

最新文章

1. 全球最大资管公司押注人工智能！要做这些大事
2. 服务器块格式不正确的是什么,c#-服务器标签格式不正确.(databinder.eval)
3. tps是什么意思_系统了解精益生产系统TPS精益思想丛书介绍
4. c ++向量库_C ++中的2D向量–实用指南2D向量
5. C#面向对象编程的3个支柱
6. cocos2d-x 3.2线程安全的消息中心
7. python, c/c++去掉文本的换行符
8. java调用js中的方法样例
9. 南阳oj S + T
10. scratch编程小游戏黑白棋
11. Day08_vant实现_网易云音乐案例
12. 《禅与摩托车维修艺术》读后感
13. 数字后端入行门槛和条件？附入行进阶必读书籍丨建议收藏
14. python自动化系列之python操作pptx文件
15. 赵运泓:12：9黄金原油行情走势分析
16. 毕业论文设计：第二部分—激光雷达里程计研究
17. 【多线程】——深入理解线程中断方式（interrupt）
18. 数据库迁移工具（一）
19. JetsonNano内存卡
20. 天梯赛补题：L3-021 神坛 (30 分)

热门文章

1. 网络工程师的敲门砖，2022最新HCIA-Datacom题库H12-811首发分享
2. 写给新人的Python书籍推荐（必读）
3. JavaScript实现在线生成高强度随机密码工具-toolfk程序员在线工具网
4. 最受程序员欢迎的图书推荐
5. 传感器及ADAS技术相关
6. 自动控制原理（2）——自动控制的类型、基本要求
7. 网络爬虫的 “ 黑洞 ”
8. 单片机播放WAV格式音频的理解
9. 无参考图像质量评价之可察觉模糊程度方法（JNB）
10. python编写student类_Python艺术编程节——以趣味活动促进学生学习编程