局部建图线程入口：可执行程序在初始化三个线程的时候，在System.cc的构造函数中进入局部建图线程

    mpLocalMapper = new LocalMapping(mpMap,                 //指定使iomanipmSensor==MONOCULAR);  // TODO 为什么这个要设置成为MONOCULAR？？？//运行这个局部建图线程mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //这个线程会调用的函数mpLocalMapper);             //这个调用函数的参数

插入关键帧

将关键帧插入到关键帧列表中

void LocalMapping::InsertKeyFrame(KeyFrame *pKF)

// 插入关键帧,由外部（Tracking）线程调用;这里只是插入到列表中,等待线程主函数对其进行处理
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{unique_lock<mutex> lock(mMutexNewKFs);// 将关键帧插入到列表中mlNewKeyFrames.push_back(pKF);mbAbortBA=true;
}

处理关键帧

void LocalMapping::ProcessNewKeyFrame()

Tracking线程将关键帧插入了队列中，该函数取出一个关键帧进行处理：建立路标点与关键帧的联系，更新共视图，将关键帧插入地图中。

    {unique_lock<mutex> lock(mMutexNewKFs);// 取出列表中最前面的关键帧，作为当前要处理的关键帧mpCurrentKeyFrame = mlNewKeyFrames.front();// 取出最前面的关键帧后，在原来的列表里删掉该关键帧mlNewKeyFrames.pop_front();}

计算关键帧的词袋信息

会更新关键帧数据库（存储所有关键帧的词典）。

mpCurrentKeyFrame->ComputeBoW();

建立当前帧与路标点的联系

在追踪局部地图时，只是建立了特征点和路标点间的匹配关系，并没有建立联系。

    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{// 如果当前帧中已经包含了这个地图点,但是这个地图点中却没有包含这个关键帧的信息// 这些地图点可能来自双目或RGBD跟踪过程中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生// 将上述地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验mlpRecentAddedMapPoints.push_back(pMP); }}}}

更新关键帧间的联系（共视图）

mpCurrentKeyFrame->UpdateConnections();

关键帧插入地图

mpMap->AddKeyFrame(mpCurrentKeyFrame);

剔除错误路标点

mlpRecentAddedMapPoints：当前关键帧中待检测的地图点集合。

void LocalMapping::MapPointCulling()

剔除条件1

** 地图点被追踪的次数/预计被看到的次数<0.25**
** 地图点被追踪的次数指的是：地图点投影到关键帧中，和多少关键帧的特征点形成了匹配。
预计被看到的次数指的是：该地图点在多少帧的视野范围内。**

        else if(pMP->GetFoundRatio()<0.25f){// Step 2.2：跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例小于25%，删除// (mnFound/mnVisible） < 25%// mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好// mnVisible：地图点应该被看到的次数// (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);}

剔除条件2

该路标点可以被3个以上的关键帧观测到，否则剔除

        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs){// Step 2.3：从该点建立开始，到现在已经过了不小于2个关键帧// 但是观测到该点的关键帧数却不超过cnThObs帧，那么删除该点pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);}else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)// Step 2.4：从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点// 因此没有SetBadFlag()，仅从队列中删除，放弃继续对该MapPoint的检测lit = mlpRecentAddedMapPoints.erase(lit);elselit++;

检查关键帧列表是否为空

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

创建新地图点

获得数据

（1）取出与当前帧共视程度最高的几个关键帧。

 const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);

（2）获取当前关键帧的变换矩阵:由旋转矩阵和平移矩阵组成

    cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();cv::Mat Rwc1 = Rcw1.t();cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();cv::Mat Tcw1(3,4,CV_32F);Rcw1.copyTo(Tcw1.colRange(0,3));tcw1.copyTo(Tcw1.col(3));

（3）获得相机光心坐标、内参

    cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();const float &fx1 = mpCurrentKeyFrame->fx;const float &fy1 = mpCurrentKeyFrame->fy;const float &cx1 = mpCurrentKeyFrame->cx;const float &cy1 = mpCurrentKeyFrame->cy;const float &invfx1 = mpCurrentKeyFrame->invfx;const float &invfy1 = mpCurrentKeyFrame->invfy;

遍历共视程度最高的关键帧

（1）判断是否需要对每个相邻的关键进行三角化,需要满足一定的稀疏性。单目应满足的条件：
两帧间基线的长度要大于路标点平均深度的一个比例。

        else    {// 单目相机情况// 邻接关键帧的场景深度中值const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);// baseline与景深的比例const float ratioBaselineDepth = baseline/medianDepthKF2;// 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点if(ratioBaselineDepth<0.01)continue;}

（2）根据两个关键帧的位姿计算它们之间的基础矩阵

cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);

（3）通过BoW对两关键帧的未匹配的特征点快速匹配，并使用极线约束

        vector<pair<size_t,size_t> > vMatchedIndices;matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

对每对匹配通过三角化生成3D点

（1）获得视差角

            // 归一化坐标cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);// 由相机坐标系转到世界坐标系// 是一个相机光心---->路标点的方向向量cv::Mat ray1 = Rwc1*xn1;cv::Mat ray2 = Rwc2*xn2;// 这个就是求向量之间角度公式const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));

（2）如果视差角满足约束，则使用SVD分解进行三角化，求出路标点的世界坐标

            if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)){// Linear Triangulation Method// 见Initializer.cc的 Triangulate 函数,实现是一样的,顶多就是把投影矩阵换成了变换矩阵cv::Mat A(4,4,CV_32F);A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);cv::Mat w,u,vt;cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);x3D = vt.row(3).t();// 归一化之前的检查if(x3D.at<float>(3)==0)continue;// 归一化成为齐次坐标,然后提取前面三个维度作为欧式坐标x3D = x3D.rowRange(0,3)/x3D.at<float>(3);}

（3）判断三角化后的路标点是否满足约束
1）深度大于0
2）重投影误差小于阈值

            // Step 6.5：检测生成的3D点是否在相机前方,不在的话就放弃这个点float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);if(z1<=0)continue;float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);if(z2<=0)continue;//Check reprojection error in first keyframe// Step 6.6：计算3D点在当前关键帧下的重投影误差const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);const float invz1 = 1.0/z1;if(!bStereo1){// 单目情况下float u1 = fx1*x1*invz1+cx1;float v1 = fy1*y1*invz1+cy1;float errX1 = u1 - kp1.pt.x;float errY1 = v1 - kp1.pt.y;// 假设测量有一个像素的偏差，2自由度卡方检验阈值是5.991if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)continue;}

建立新的地图点与帧、地图间的联系

地图点放入队列中，等待检测

融合相邻帧中的路标点

因为在局部建图线程中，创建了很多新的路标点，因此需要进行融合。

获得一阶、二阶共视关键帧

    // 开始之前先定义几个概念// 当前关键帧的邻接关键帧，称为一级相邻关键帧，也就是邻居// 与一级相邻关键帧相邻的关键帧，称为二级相邻关键帧，也就是邻居的邻居// 单目情况要10个邻接关键帧，双目或者RGBD则要20个int nn = 10;if(mbMonocular)nn=20;// 和当前关键帧相邻的关键帧，也就是一级相邻关键帧const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);// Step 2：存储一级相邻关键帧及其二级相邻关键帧vector<KeyFrame*> vpTargetKFs;// 开始对所有候选的一级关键帧展开遍历：for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;// 没有和当前帧进行过融合的操作if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)continue;// 加入一级相邻关键帧    vpTargetKFs.push_back(pKFi);// 标记已经加入pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;// Extend to some second neighbors// 以一级相邻关键帧的共视关系最好的5个相邻关键帧 作为二级相邻关键帧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);}}

进行路标点融合

 // Step 3：将当前帧的地图点分别与一级二级相邻关键帧地图点进行融合 -- 正向vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;// 将地图点投影到关键帧中进行匹配和融合；融合策略如下// 1.如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点// 2.如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点// 注意这个时候对地图点融合的操作是立即生效的matcher.Fuse(pKFi,vpMapPointMatches);}// Search matches by projection from target KFs in current KF// Step 4：将一级二级相邻关键帧地图点分别与当前关键帧地图点进行融合 -- 反向// 用于进行存储要融合的一级邻接和二级邻接关键帧所有MapPoints的集合vector<MapPoint*> vpFuseCandidates;vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());//  Step 4.1：遍历每一个一级邻接和二级邻接关键帧，收集他们的地图点存储到 vpFuseCandidatesfor(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++){KeyFrame* pKFi = *vitKF;vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();// 遍历当前一级邻接和二级邻接关键帧中所有的MapPoints,找出需要进行融合的并且加入到集合中for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++){MapPoint* pMP = *vitMP;if(!pMP)continue;// 如果地图点是坏点，或者已经加进集合vpFuseCandidates，跳过if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)continue;// 加入集合，并标记已经加入pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;vpFuseCandidates.push_back(pMP);}}// Step 4.2：进行地图点投影融合,和正向融合操作是完全相同的// 不同的是正向操作是"每个关键帧和当前关键帧的地图点进行融合",而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合"matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);

更新当前帧路标点的特性

    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++){MapPoint* pMP=vpMapPointMatches[i];if(pMP){if(!pMP->isBad()){// 在所有找到pMP的关键帧中，获得最佳的描述子pMP->ComputeDistinctiveDescriptors();// 更新平均观测方向和观测距离pMP->UpdateNormalAndDepth();}}}

更新共视图

mpCurrentKeyFrame->UpdateConnections();

局部地图BA优化

Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

对局部地图进行全局优化。
优化变量：当前关键帧以及与当前关键帧具有一定共视关系的关键帧的位姿、路标点坐标。
那些能观测到路标点，但无法达到公式关系阈值的关键帧，会提供约束，但不会进行优化。

检测冗余的关键帧

检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧。
冗余关键帧的判定：90%以上的地图点能被其他关键帧（至少3个）观测到。

void LocalMapping::KeyFrameCulling()

获得公式关键帧

vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();

遍历共视关键帧

（1）遍历某一共视关键帧的所有地图点
（2）如果该共视关键帧中的路标点的90%以上被3个关键帧观测到，则该公式关键帧视为冗余关键帧，删除该关键帧

总结

l 局部建图线程在可执行程序初始化时进入，直接调用LocalMapping::Run函数。
流程为：

（1）先将标志位置为FALSE，使得Tracking线程产生关键帧时，不再送入局部建图线程中。

SetAcceptKeyFrames(false);

（2）在队列mlNewKeyFrames中取出一个关键帧，进行处理

ProcessNewKeyFrame();

1）计算关键帧的词袋，更新关键帧数据库。
2）在追踪局部地图时，匹配了局部地图中的路标点和关键帧的特征点，但未建立联系。因此建立关键帧的路标点与该关键帧的联系。同时将路标点加入到待检测变量中()
3）更新共视图（只是更新当前帧及其有共视关系的关键帧间的权重）。
4）将关键帧插入到地图中。

（3）去除错误路标点

MapPointCulling();

遍历待检测的地图点，去除错误地图点的条件为：
1）地图点被追踪的次数/预计被看到的次数<0.25
2）该路标点可以被3个以上的关键帧观测到，否则剔除
（4）创建新的地图点

CreateNewMapPoints();

如果不创建新的地图点的话，已知深度的地图点会越来越少，以至于后面无法继续追踪。
1）找到与当前帧共视程度最高的n个关键帧
2）遍历所有候选关键帧，使用词袋加速算法+极线约束寻找匹配的特征点
3）对每对特征点进行三角化测距
4）建立路标点与关键帧、局部地图的联系
（5）重复特征点的融合
创建了新的路标点之后，与这个路标点1匹配的特征点之前可能有匹配的路标点2。这是需要在路标点1、路标点2中进行选择。

SearchInNeighbors();

1）寻找当前关键帧的候选关键帧（共视点多）
2）将当前帧的路标点与每个候选关键帧的特征点进行重新匹配。
2）将所有候选关键帧的路标点与当前帧的特征点进行重新匹配。
（6）全局BA优化

Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

优化的是当前关键帧及其共视关键帧的位姿与路标点的坐标。这里的共视关键帧指的是共视路标点的数目超过一个阈值。对于那些共视路标点没有超过阈值的关键帧，会为BA优化提供约束，但并不进行优化。
（7）删除冗余关键帧

KeyFrameCulling();

冗余关键帧指的是路标点的90%以上被3个以上的关键帧观察到。

ORB_SLAM2局部建图线程相关推荐

(01)ORB-SLAM2源码无死角解析-(63) BA优化(g2o)→局部建图线程:Optimizer::LocalBundleAdjustment→位姿与地图点优化
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (0 ...
ORB-SLAM2代码详解08: 局部建图线程LocalMapping
pdf版本笔记的下载地址: ORB-SLAM2代码详解08_局部建图线程LocalMapping,排版更美观一点,这个网站的默认排版太丑了(访问密码:3834) ORB-SLAM2代码详解08: 局部 ...
ORB-SLAM2添加稠密建图线程
注:本篇文章只是对高翔博士稠密点云代码进行的简述,内容主要包括的是在ORB-SLAM2基础上怎么添加稠密建图线程,并未对高翔博士代码进行改动.本文章仅用于自己学习记录,若有侵权麻烦私聊联系删除. 目录 ...
ORB_SLAM2稠密建图（ORBSLAM2_with_pointcloud_map）,并保存地图
本文基于高翔博士的ORB_SLAM2稠密建图代码改进,并且添加了稠密地图保存功能. 本文的代码已上传github,可用以下代码下载: git clone https://github.com/FAST ...
ubuntu从零到一跑通ORB_SLAM2及其ORBSLAM2_with_pointcloud稠密建图
目录前言 1 安装ubuntu18.04.6 1.1 问题 ①安装成功ubuntu后会出现窗口界面不适配,以及Ubuntu和windows之间无法复制粘贴问题 ②swapon: /swapfile ...
orb特征稠密特征_一种基于ORB-SLAM2的双目三维稠密建图方法技术
本发明专利技术公开了一种基于ORB‑SLAM2的双目稠密建图方法,涉及机器人同步定位与地图创建领域,该方法主要由跟踪线程.局部地图线程.闭环检测线程和稠密建图线程组成.其中稠密建图线程包含以下步骤:1 ...
使用ORBSLAM2进行kineticV2稠密建图，实时转octomap建图以及导航
决定总结最近一个月的工作,这个月在orbslam2的基础上,使用kineticV2完成了稠密点云地图的重建,实现了点云的回环,并使用octomap转换成实时的八叉树地图,导航部分已经有了思路,打算下个 ...
se2lam翻译：基于 SE (2)-XYZ 约束的地面车辆视觉-里程计定位与建图
摘要-本文关注的是地面车辆使用里程计和单目视觉传感器的SLAM问题,为了改进基于视觉的地面车辆估计精度,研究人员利用了近似平面运动的约束,通常将其作为 SE (3)位姿的随机约束来实现,在本文中提出了 ...
ORB_SLAM2新增稠密建图代码笔记
接上一篇博客"ORB_SLAM2+kinect稠密建图实战项目总结" ORB_SLAM2+kinect稠密建图实战项目总结_好好仔仔的博客-CSDN博客本篇在此基础上对整个项目的 ...

ORB_SLAM2局部建图线程