0 引言

ORB-SLAM2算法7详细了解了System主类和多线程、ORB-SLAM2学习笔记8详细了解了图像特征点提取和描述子的生成、ORB-SLAM2算法9详细了解了图像帧、ORB-SLAM2算法10详细了解了图像关键帧、ORB-SLAM2算法11详细了解了地图点及ORB-SLAM2算法12详细了解了单目初始化，ORB-SLAM2算法13详细了解了ORB-SLAM2中的三大线程中的跟踪线程Tracking。本文主要学习ORB-SLAM2中的三大线程中的局部建图线程Local Mapping。

1 概述

ORB-SLAM2算法中，局部建图Local Mapping作为一个单独的线程，从系统启动后就开始运行，只是在上一篇Tracking线程生成新的关键帧，并插入到Local Mapping线程中的成员变量mlNewKeyFrames列表后，Local Mapping线程检测到该列表中有关键帧后才开始处理这些新插入的关键帧，如果没有关键帧插入的时候，Local Mapping线程是处于空闲的睡眠状态。

Local Mapping线程的主要流程：

1. 首先请求停止建图，如果不停止继续下一步；
2. 告诉Tracking线程，Local Mapping线程正处于繁忙状态，暂停接收关键帧SetAcceptKeyFrames(false)；
3. 处理列表中的关键帧，包括计算BOW、更新观测、描述子、共视图，插入到地图等；
4. 根据地图点的观测情况剔除质量不好的地图点；
5. 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳定；
6. 如果已经处理完最后的一个关键帧，检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点；
7. 如果已经处理完最后一个关键帧，且闭环检测线程没有请求停止建图，当局部地图中的关键帧大于2个的时候进行局部地图的BA，并检测并剔除当前帧相邻的关键帧中冗余的关键帧；
8. 设置继续接收当前帧SetAcceptKeyFrames(true)，并暂停当前线程3秒，然后回归第1步。

主要流程图如下：

// 线程主函数void LocalMapping::Run()
{// 标记状态，表示当前run函数正在运行，尚未结束mbFinished = false;// 主循环while(1){// Tracking will see that Local Mapping is busy// Step 1 告诉Tracking，LocalMapping正处于繁忙状态，请不要给我发送关键帧打扰我// LocalMapping线程处理的关键帧都是Tracking线程发来的SetAcceptKeyFrames(false);// Check if there are keyframes in the queue// 等待处理的关键帧列表不为空if(CheckNewKeyFrames()){// BoW conversion and insertion in Map// Step 2 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等ProcessNewKeyFrame();// Check recent MapPoints// Step 3 根据地图点的观测情况剔除质量不好的地图点MapPointCulling();// Triangulate new MapPoints// Step 4 当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳CreateNewMapPoints();// 已经处理完队列中的最后的一个关键帧if(!CheckNewKeyFrames()){// Find more matches in neighbor keyframes and fuse point duplications//  Step 5 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点SearchInNeighbors();}// 终止BA的标志mbAbortBA = false;// 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalMappingif(!CheckNewKeyFrames() && !stopRequested()){// Local BA// Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BAif(mpMap->KeyFramesInMap()>2)// 注意这里的第二个参数是按地址传递的,当这里的 mbAbortBA 状态发生变化时，能够及时执行/停止BAOptimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);// Check redundant local Keyframes// Step 7 检测并剔除当前帧相邻的关键帧中冗余的关键帧// 冗余的判定：该关键帧的90%的地图点可以被其它关键帧观测到KeyFrameCulling();}// Step 8 将当前帧加入到闭环检测队列中// 注意这里的关键帧被设置成为了bad的情况,这个需要注意mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);}else if(Stop())     // 当要终止当前线程的时候{// Safe area to stopwhile(isStopped() && !CheckFinish()){// 如果还没有结束利索,那么等// usleep(3000);std::this_thread::sleep_for(std::chrono::milliseconds(3));}// 然后确定终止了就跳出这个线程的主循环if(CheckFinish())break;}// 查看是否有复位线程的请求ResetIfRequested();// Tracking will see that Local Mapping is not busySetAcceptKeyFrames(true);// 如果当前线程已经结束了就跳出主循环if(CheckFinish())break;//usleep(3000);std::this_thread::sleep_for(std::chrono::milliseconds(3));}// 设置线程已经终止SetFinish();
}

以下是针对上述流程中的主要步骤详细阐述，比如如何处理队列中的关键帧、剔除坏的地图点及局部BA优化等等。

2 处理队列中的关键帧

该步骤主要是通过成员函数void LocalMapping::ProcessNewKeyFrame()实现的，包括计算词袋BOW、更新观测、描述子、共视图及插入到地图等。主要实现步骤如下：

1. 从缓冲队列中取出一帧关键帧mpCurrentKeyFrame；
2. 计算该关键帧mpCurrentKeyFrame的词袋向量;
3. 处理关键帧的地图点：更新地图点平均观测方向、更新该点的平均观测方向和观测距离范围、更新地图点的最佳描述子;
4. 更新关键帧间的连接关系（共视图），主要更新下列变量：
   • mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息
   • mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧
   • mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度
   • mpParent：共视程度最高的那个关键帧
   • mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧
5. 将该关键帧mpCurrentKeyFrame插入地图。

// 处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等void LocalMapping::ProcessNewKeyFrame()
{// Step 1：从缓冲队列中取出一帧关键帧// 该关键帧队列是Tracking线程向LocalMapping中插入的关键帧组成{unique_lock<mutex> lock(mMutexNewKFs);// 取出列表中最前面的关键帧，作为当前要处理的关键帧mpCurrentKeyFrame = mlNewKeyFrames.front();// 取出最前面的关键帧后，在原来的列表里删掉该关键帧mlNewKeyFrames.pop_front();}// Compute Bags of Words structures// Step 2：计算该关键帧特征点的词袋向量mpCurrentKeyFrame->ComputeBoW();// Associate MapPoints to the new keyframe and update normal and descriptor// Step 3：当前处理关键帧中有效的地图点，更新normal，描述子等信息// TrackLocalMap中和当前帧新匹配上的地图点和当前关键帧进行关联绑定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); }}}}    // Update links in the Covisibility Graph// Step 4：更新关键帧间的连接关系（共视图）mpCurrentKeyFrame->UpdateConnections();// Insert Keyframe in Map// Step 5：将该关键帧插入到地图中mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

3 剔除坏的地图点

该步骤主要通过成员函数void LocalMapping::MapPointCulling()实现的，主要实现步骤如下：

1. 根据相机类型设置不同的观测阈值cnThObs，单目设置为2，双目/RGBD设置成3；
2. 遍历检查新添加的地图点；
   2.1 已经是坏的地图点，直接从队列中删除；
   2.2 跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除；
   2.3 从该点建立开始，到现在已经过了不小于2个关键帧，并且观测到该点的相机数却不超过阈值cnThObs，从地图中删除；
   2.4 从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点。

其中2.2和2.3中可知质量不好的地图点的判断标准：

1. 召回率$=\frac{mnFound}{mnVisible}<0.25$
2. 创建的3帧内观测数目少于设定阈值cnThObs

而质量好的地图点的判断标准就是：

1. 连续3个关键帧仍未被剔除。

// 检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点void LocalMapping::MapPointCulling()
{// Check Recent Added MapPointslist<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;// Step 1：根据相机类型设置不同的观测阈值int nThObs;if(mbMonocular)nThObs = 2;elsenThObs = 3;const int cnThObs = nThObs;// Step 2：遍历检查新添加的地图点while(lit!=mlpRecentAddedMapPoints.end()){MapPoint* pMP = *lit;if(pMP->isBad()){// Step 2.1：已经是坏点的地图点仅从队列中删除lit = mlpRecentAddedMapPoints.erase(lit);}else if(pMP->GetFoundRatio()<0.25f){// Step 2.2：跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除// (mnFound/mnVisible） < 25%// mnFound ：地图点被多少帧（包括普通帧）看到，次数越多越好// mnVisible：地图点应该被看到的次数// (mnFound/mnVisible）：对于大FOV镜头这个比例会高，对于窄FOV镜头这个比例会低pMP->SetBadFlag();lit = mlpRecentAddedMapPoints.erase(lit);}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()，仅从队列中删除lit = mlpRecentAddedMapPoints.erase(lit);elselit++;}
}

4 创建新地图点

该步骤主要是成员函数void LocalMapping::CreateNewMapPoints()实现的，主要实现的步骤如下：

1. 根据相机类型设置不同的搜索最佳共视关键帧的数目nn，单目设置为20，双目/RGBD设置10；
2. 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧；
3. 遍历相邻关键帧，搜索匹配并用极线约束剔除误匹配，最终三角化；
4. 判断相机运动的基线是不是足够长；
5. 根据两个关键帧的位姿计算它们之间的基础矩阵；
6. 通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对；
7. 对每对匹配通过三角化生成3D点,和Initializer::Triangulate函数差不多；

// 用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳void LocalMapping::CreateNewMapPoints()
{// Retrieve neighbor keyframes in covisibility graph// nn表示搜索最佳共视关键帧的数目// 不同传感器下要求不一样,单目的时候需要有更多的具有较好共视关系的关键帧来建立地图int nn = 10;if(mbMonocular)nn=20;// Step 1：在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);// 特征点匹配配置 最佳距离 < 0.6*次佳距离，比较苛刻了。不检查旋转ORBmatcher matcher(0.6,false); // 取出当前帧从世界坐标系到相机坐标系的变换矩阵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));// 得到当前关键帧（左目）光心在世界坐标系中的坐标、内参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.5是经验值// mfScaleFactor = 1.2const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;// 记录三角化成功的地图点数目int nnew=0;// Search matches with epipolar restriction and triangulate// Step 2：遍历相邻关键帧，搜索匹配并用极线约束剔除误匹配，最终三角化for(size_t i=0; i<vpNeighKFs.size(); i++){// ! 疑似bug，正确应该是 if(i>0 && !CheckNewKeyFrames())if(i>0 && CheckNewKeyFrames())return;KeyFrame* pKF2 = vpNeighKFs[i];// Check first that baseline is not too short// 相邻的关键帧光心在世界坐标系中的坐标cv::Mat Ow2 = pKF2->GetCameraCenter();// 基线向量，两个关键帧间的相机位移cv::Mat vBaseline = Ow2-Ow1;// 基线长度const float baseline = cv::norm(vBaseline);// Step 3：判断相机运动的基线是不是足够长if(!mbMonocular){// 如果是双目相机，关键帧间距小于本身的基线时不生成3D点// 因为太短的基线下能够恢复的地图点不稳定if(baseline<pKF2->mb)continue;}else    {// 单目相机情况// 相邻关键帧的场景深度中值const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);// 基线与景深的比例const float ratioBaselineDepth = baseline/medianDepthKF2;// 如果比例特别小，基线太短恢复3D点不准，那么跳过当前邻接的关键帧，不生成3D点if(ratioBaselineDepth<0.01)continue;}// Compute Fundamental Matrix// Step 4：根据两个关键帧的位姿计算它们之间的基础矩阵cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);// Search matches that fullfil epipolar constraint// Step 5：通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对vector<pair<size_t,size_t> > vMatchedIndices;matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);cv::Mat Rcw2 = pKF2->GetRotation();cv::Mat Rwc2 = Rcw2.t();cv::Mat tcw2 = pKF2->GetTranslation();cv::Mat Tcw2(3,4,CV_32F);Rcw2.copyTo(Tcw2.colRange(0,3));tcw2.copyTo(Tcw2.col(3));const float &fx2 = pKF2->fx;const float &fy2 = pKF2->fy;const float &cx2 = pKF2->cx;const float &cy2 = pKF2->cy;const float &invfx2 = pKF2->invfx;const float &invfy2 = pKF2->invfy;// Triangulate each match// Step 6：对每对匹配通过三角化生成3D点,和 Triangulate函数差不多const int nmatches = vMatchedIndices.size();for(int ikp=0; ikp<nmatches; ikp++) {// Step 6.1：取出匹配特征点// 当前匹配对在当前关键帧中的索引const int &idx1 = vMatchedIndices[ikp].first;// 当前匹配对在邻接关键帧中的索引const int &idx2 = vMatchedIndices[ikp].second;// 当前匹配在当前关键帧中的特征点const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];// mvuRight中存放着双目的深度值，如果不是双目，其值将为-1const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];bool bStereo1 = kp1_ur>=0;// 当前匹配在邻接关键帧中的特征点const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];// mvuRight中存放着双目的深度值，如果不是双目，其值将为-1const float kp2_ur = pKF2->mvuRight[idx2];bool bStereo2 = kp2_ur>=0;// Check parallax between rays// Step 6.2：利用匹配点反投影得到视差角// 特征点反投影,其实得到的是在各自相机坐标系下的一个非归一化的方向向量,和这个点的反投影射线重合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));// 加1是为了让cosParallaxStereo随便初始化为一个很大的值float cosParallaxStereo = cosParallaxRays+1;  // cosParallaxStereo1、cosParallaxStereo2在后面可能不存在，需要初始化为较大的值float cosParallaxStereo1 = cosParallaxStereo;float cosParallaxStereo2 = cosParallaxStereo;// Step 6.3：对于双目，利用双目得到视差角；单目相机没有特殊操作if(bStereo1)// 传感器是双目相机,并且当前的关键帧的这个点有对应的深度// 假设是平行的双目相机，计算出双目相机观察这个点的时候的视差角余弦cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));else if(bStereo2)// 传感器是双目相机,并且邻接的关键帧的这个点有对应的深度，和上面一样操作cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));// 得到双目观测的视差角中最小的那个cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);// Step 6.4：三角化恢复3D点cv::Mat x3D;// cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)表明视差角正常,0.9998 对应1°// cosParallaxRays < cosParallaxStereo 表明匹配点对夹角大于双目本身观察三维点夹角// 匹配点对夹角大，用三角法恢复3D点// 参考：https://github.com/raulmur/ORB_SLAM2/issues/345if(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);}// 匹配点对夹角小，用双目恢复3D点else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)  {// 如果是双目，用视差角更大的那个双目信息来恢复，直接用已知3D点反投影了x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);                }else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)  {x3D = pKF2->UnprojectStereo(idx2);}elsecontinue; //No stereo and very low parallax, 放弃// 为方便后续计算，转换成为了行向量cv::Mat x3Dt = x3D.t();//Check triangulation in front of cameras// 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;}else{// 双目情况float u1 = fx1*x1*invz1+cx1;// 根据视差公式计算假想的右目坐标float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;     float v1 = fy1*y1*invz1+cy1;float errX1 = u1 - kp1.pt.x;float errY1 = v1 - kp1.pt.y;float errX1_r = u1_r - kp1_ur;// 自由度为3，卡方检验阈值是7.8if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)continue;}//Check reprojection error in second keyframe// 计算3D点在另一个关键帧下的重投影误差，操作同上const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);const float invz2 = 1.0/z2;if(!bStereo2){float u2 = fx2*x2*invz2+cx2;float v2 = fy2*y2*invz2+cy2;float errX2 = u2 - kp2.pt.x;float errY2 = v2 - kp2.pt.y;if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)continue;}else{float u2 = fx2*x2*invz2+cx2;float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;float v2 = fy2*y2*invz2+cy2;float errX2 = u2 - kp2.pt.x;float errY2 = v2 - kp2.pt.y;float errX2_r = u2_r - kp2_ur;if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)continue;}//Check scale consistency// Step 6.7：检查尺度连续性// 世界坐标系下，3D点与相机间的向量，方向由相机指向3D点cv::Mat normal1 = x3D-Ow1;float dist1 = cv::norm(normal1);cv::Mat normal2 = x3D-Ow2;float dist2 = cv::norm(normal2);if(dist1==0 || dist2==0)continue;// ratioDist是不考虑金字塔尺度下的距离比例const float ratioDist = dist2/dist1;// 金字塔尺度因子的比例const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];/*if(fabs(ratioDist-ratioOctave)>ratioFactor)continue;*/// 距离的比例和图像金字塔的比例不应该差太多，否则就跳过if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)continue;// Triangulation is succesfull// Step 6.8：三角化生成3D点成功，构造成MapPointMapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);// Step 6.9：为该MapPoint添加属性：// a.观测到该MapPoint的关键帧pMP->AddObservation(mpCurrentKeyFrame,idx1);            pMP->AddObservation(pKF2,idx2);mpCurrentKeyFrame->AddMapPoint(pMP,idx1);pKF2->AddMapPoint(pMP,idx2);// b.该MapPoint的描述子pMP->ComputeDistinctiveDescriptors();// c.该MapPoint的平均观测方向和深度范围pMP->UpdateNormalAndDepth();mpMap->AddMapPoint(pMP);// Step 6.10：将新产生的点放入检测队列// 这些MapPoints都会经过MapPointCulling函数的检验mlpRecentAddedMapPoints.push_back(pMP);nnew++;}}
}

5 融合当前关键帧和其共视帧的地图点

该步骤主要是通过成员函数void LocalMapping::SearchInNeighbors()实现的，这里有两个小概念需要先熟悉下：

1. 一级相邻关键帧：当前关键帧的邻接关键帧，即当前关键帧的邻居
2. 二级相邻关键帧：一级相邻关键帧相邻的关键帧，即当前关键帧的邻居的邻居

该函数主要实现的步骤如下：

1. 根据相机类型设置不同的邻接关键帧数目nn，单目设置为20，双目/RGBD设置10；
2. 获得当前关键帧在共视图中权重排名前nn的邻接关键帧；
3. 存储一级相邻关键帧及其二级相邻关键帧；
4. 将当前帧的地图点分别投影到两级相邻关键帧，寻找匹配点对应的地图点进行融合，称为正向投影融合；
5. 将两级相邻关键帧地图点分别投影到当前关键帧，寻找匹配点对应的地图点进行融合，称为反向投影融合；
6. 更新当前帧地图点的描述子、深度、平均观测方向等属性；
7. 最后，更新当前帧与其它帧的共视连接关系；

// 检查并融合当前关键帧与相邻帧（两级相邻）重复的地图点void LocalMapping::SearchInNeighbors()
{// Retrieve neighbor keyframes// Step 1：获得当前关键帧在共视图中权重排名前nn的邻接关键帧// 开始之前先定义几个概念// 当前关键帧的邻接关键帧，称为一级相邻关键帧，也就是邻居// 与一级相邻关键帧相邻的关键帧，称为二级相邻关键帧，也就是邻居的邻居// 单目情况要20个邻接关键帧，双目或者RGBD则要10个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);}}// Search matches by projection from current KF in target KFs// 使用默认参数, 最优和次优比例0.6,匹配时检查特征点的旋转ORBmatcher matcher;// 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);// Update points// Step 5：更新当前帧地图点的描述子、深度、平均观测方向等属性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();}}}// Update connections in covisibility graph// Step 6：更新当前帧与其它帧的共视连接关系mpCurrentKeyFrame->UpdateConnections();
}

6 局部BA优化

该步骤主要是通过成员函数void Optimizer::LocalBundleAdjustment()实现的，该函数不是在LocalMapping类中实现的，是Optimizer类中实现的（Optimizer.cc），主要是构造g2o优化器来实现的，详细的实现步骤如下：

1. 关键帧选择：ORB-SLAM2中，局部BA优化是在一组关键帧上执行的。首先，根据一些准则（例如，距离、关键点数量等），选择一组具有较高质量和代表性的关键帧。
2. 重投影误差计算：对于每个选择的关键帧，计算其观测到的地图点在其图像上的重投影位置，并计算重投影误差。重投影误差即观测到的图像点与通过当前估计的相机位姿和地图点位置计算得到的重投影点之间的差异。
3. 优化问题建模：将局部BA问题建模为一个非线性优化问题。优化的目标是最小化重投影误差，通过调整关键帧的位姿和地图点的位置来优化。
4. 约束设置：将关键帧的位姿和地图点的位置作为优化变量，并设置约束来限制它们的调整范围。约束通常包括相机位姿的刚性运动模型和地图点的三维空间几何关系。
5. 优化求解：使用非线性优化方法（例如，高斯-牛顿法或Levenberg-Marquardt算法）对建立的优化问题进行求解。通过迭代优化变量的值，以最小化重投影误差。
6. 优化结果应用：将优化后的相机位姿和地图点位置应用于系统中的关键帧和地图更新。这样可以提高相机位姿的准确性，优化地图点的位置，并进一步改善系统的精度和稳定性。

// 局部BA优化void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{// 该优化函数用于LocalMapping线程的局部BA优化// Local KeyFrames: First Breadth Search from Current Keyframe// 局部关键帧list<KeyFrame*> lLocalKeyFrames;// Step 1 将当前关键帧及其共视关键帧加入局部关键帧lLocalKeyFrames.push_back(pKF);pKF->mnBALocalForKF = pKF->mnId;// 找到关键帧连接的共视关键帧（一级相连），加入局部关键帧中const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();for(int i=0, iend=vNeighKFs.size(); i<iend; i++){KeyFrame* pKFi = vNeighKFs[i];// 把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId，防止重复添加pKFi->mnBALocalForKF = pKF->mnId;// 保证该关键帧有效才能加入if(!pKFi->isBad())      lLocalKeyFrames.push_back(pKFi);}// Local MapPoints seen in Local KeyFrames// Step 2 遍历局部关键帧中(一级相连)关键帧，将它们观测的地图点加入到局部地图点list<MapPoint*> lLocalMapPoints;// 遍历局部关键帧中的每一个关键帧for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++){// 取出该关键帧对应的地图点vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();// 遍历这个关键帧观测到的每一个地图点，加入到局部地图点for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++){MapPoint* pMP = *vit;if(pMP){if(!pMP->isBad())   //保证地图点有效// 把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId// mnBALocalForKF 是为了防止重复添加if(pMP->mnBALocalForKF!=pKF->mnId){lLocalMapPoints.push_back(pMP);pMP->mnBALocalForKF=pKF->mnId;}}   // 判断这个地图点是否靠谱} // 遍历这个关键帧观测到的每一个地图点} // 遍历 lLocalKeyFrames 中的每一个关键帧// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes// Step 3 得到能被局部地图点观测到，但不属于局部关键帧的关键帧(二级相连)，这些二级相连关键帧在局部BA优化时不优化list<KeyFrame*> lFixedCameras;// 遍历局部地图中的每个地图点for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++){// 观测到该地图点的KF和该地图点在KF中的索引map<KeyFrame*,size_t> observations = (*lit)->GetObservations();// 遍历所有观测到该地图点的关键帧for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){KeyFrame* pKFi = mit->first;// pKFi->mnBALocalForKF!=pKF->mnId 表示不属于局部关键帧，// pKFi->mnBAFixedForKF!=pKF->mnId 表示还未标记为fixed（固定的）关键帧if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId){                // 将局部地图点能观测到的、但是不属于局部BA范围的关键帧的mnBAFixedForKF标记为pKF（触发局部BA的当前关键帧）的mnIdpKFi->mnBAFixedForKF=pKF->mnId;if(!pKFi->isBad())lFixedCameras.push_back(pKFi);}}}// Setup optimizer// Step 4 构造g2o优化器g2o::SparseOptimizer optimizer;g2o::BlockSolver_6_3::LinearSolverType * linearSolver;linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);// LM大法好g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);optimizer.setAlgorithm(solver);// 外界设置的停止优化标志// 可能在 Tracking::NeedNewKeyFrame() 里置位if(pbStopFlag)optimizer.setForceStopFlag(pbStopFlag);// 记录参与局部BA的最大关键帧mnIdunsigned long maxKFid = 0;// Set Local KeyFrame vertices// Step 5 添加待优化的位姿顶点：局部关键帧的位姿for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++){KeyFrame* pKFi = *lit;g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();// 设置初始优化位姿vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));vSE3->setId(pKFi->mnId);// 如果是初始关键帧，要锁住位姿不优化vSE3->setFixed(pKFi->mnId==0);optimizer.addVertex(vSE3);if(pKFi->mnId>maxKFid)maxKFid=pKFi->mnId;}// Set Fixed KeyFrame vertices// Step  6 添加不优化的位姿顶点：固定关键帧的位姿，注意这里调用了vSE3->setFixed(true)// 不优化为啥也要添加？回答：为了增加约束信息for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++){KeyFrame* pKFi = *lit;g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));vSE3->setId(pKFi->mnId);// 所有的这些顶点的位姿都不优化，只是为了增加约束项vSE3->setFixed(true);  optimizer.addVertex(vSE3);if(pKFi->mnId>maxKFid)maxKFid=pKFi->mnId;}// Set MapPoint vertices// Step  7 添加待优化的局部地图点顶点// 边的最大数目 = 位姿数目 * 地图点数目const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;vpEdgesMono.reserve(nExpectedSize);vector<KeyFrame*> vpEdgeKFMono;vpEdgeKFMono.reserve(nExpectedSize);vector<MapPoint*> vpMapPointEdgeMono;vpMapPointEdgeMono.reserve(nExpectedSize);vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;vpEdgesStereo.reserve(nExpectedSize);vector<KeyFrame*> vpEdgeKFStereo;vpEdgeKFStereo.reserve(nExpectedSize);vector<MapPoint*> vpMapPointEdgeStereo;vpMapPointEdgeStereo.reserve(nExpectedSize);// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991const float thHuberMono = sqrt(5.991);// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815const float thHuberStereo = sqrt(7.815);// 遍历所有的局部地图点for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++){// 添加顶点：MapPointMapPoint* pMP = *lit;g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));// 前面记录maxKFid的作用在这里体现int id = pMP->mnId+maxKFid+1;vPoint->setId(id);// 因为使用了LinearSolverType，所以需要将所有的三维点边缘化掉vPoint->setMarginalized(true);  optimizer.addVertex(vPoint);// 观测到该地图点的KF和该地图点在KF中的索引const map<KeyFrame*,size_t> observations = pMP->GetObservations();// Set edges// Step 8 在添加完了一个地图点之后, 对每一对关联的地图点和关键帧构建边// 遍历所有观测到当前地图点的关键帧for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++){KeyFrame* pKFi = mit->first;if(!pKFi->isBad()){                const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];// 根据单目/双目两种不同的输入构造不同的误差边// Monocular observation// 单目模式下if(pKFi->mvuRight[mit->second]<0){Eigen::Matrix<double,2,1> obs;obs << kpUn.pt.x, kpUn.pt.y;g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();// 边的第一个顶点是地图点e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));// 边的第一个顶点是观测到该地图点的关键帧e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));e->setMeasurement(obs);// 权重为特征点所在图像金字塔的层数的倒数const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);// 使用鲁棒核函数抑制外点g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberMono);e->fx = pKFi->fx;e->fy = pKFi->fy;e->cx = pKFi->cx;e->cy = pKFi->cy;// 将边添加到优化器，记录边、边连接的关键帧、边连接的地图点信息optimizer.addEdge(e);vpEdgesMono.push_back(e);vpEdgeKFMono.push_back(pKFi);vpMapPointEdgeMono.push_back(pMP);}else // Stereo observation{// 双目或RGB-D模式和单目模式类似Eigen::Matrix<double,3,1> obs;const float kp_ur = pKFi->mvuRight[mit->second];obs << kpUn.pt.x, kpUn.pt.y, kp_ur;g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));e->setMeasurement(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;e->setInformation(Info);g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;e->setRobustKernel(rk);rk->setDelta(thHuberStereo);e->fx = pKFi->fx;e->fy = pKFi->fy;e->cx = pKFi->cx;e->cy = pKFi->cy;e->bf = pKFi->mbf;optimizer.addEdge(e);vpEdgesStereo.push_back(e);vpEdgeKFStereo.push_back(pKFi);vpMapPointEdgeStereo.push_back(pMP);} } } // 遍历所有观测到当前地图点的关键帧} // 遍历所有的局部地图中的地图点// 开始BA前再次确认是否有外部请求停止优化，因为这个变量是引用传递，会随外部变化// 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 里置位if(pbStopFlag)if(*pbStopFlag)return;// Step 9 分成两个阶段开始优化。// 第一阶段优化optimizer.initializeOptimization();// 迭代5次optimizer.optimize(5);  bool bDoMore= true;// 检查是否外部请求停止if(pbStopFlag)if(*pbStopFlag)bDoMore = false;// 如果有外部请求停止,那么就不在进行第二阶段的优化if(bDoMore){// Check inlier observations// Step 10 检测outlier，并设置下次不优化// 遍历所有的单目误差边for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++){g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];MapPoint* pMP = vpMapPointEdgeMono[i];if(pMP->isBad())continue;// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991// 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，不优化了。if(e->chi2()>5.991 || !e->isDepthPositive()){// 不优化e->setLevel(1);}// 第二阶段优化的时候就属于精求解了,所以就不使用核函数e->setRobustKernel(0);}// 对于所有的双目的误差边也都进行类似的操作for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++){g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];MapPoint* pMP = vpMapPointEdgeStereo[i];if(pMP->isBad())continue;// 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815if(e->chi2()>7.815 || !e->isDepthPositive()){e->setLevel(1);}e->setRobustKernel(0);}// Optimize again without the outliers// Step 11：排除误差较大的outlier后再次优化 -- 第二阶段优化optimizer.initializeOptimization(0);optimizer.optimize(10);}vector<pair<KeyFrame*,MapPoint*> > vToErase;vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());// Check inlier observations// Step 12：在优化后重新计算误差，剔除连接误差比较大的关键帧和地图点// 对于单目误差边for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++){g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];MapPoint* pMP = vpMapPointEdgeMono[i];if(pMP->isBad())continue;// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991// 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，要删掉了if(e->chi2()>5.991 || !e->isDepthPositive()){// outlierKeyFrame* pKFi = vpEdgeKFMono[i];vToErase.push_back(make_pair(pKFi,pMP));}}// 双目误差边for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++){g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];MapPoint* pMP = vpMapPointEdgeStereo[i];if(pMP->isBad())continue;if(e->chi2()>7.815 || !e->isDepthPositive()){KeyFrame* pKFi = vpEdgeKFStereo[i];vToErase.push_back(make_pair(pKFi,pMP));}}// Get Map Mutexunique_lock<mutex> lock(pMap->mMutexMapUpdate);// 删除点// 连接偏差比较大，在关键帧中剔除对该地图点的观测// 连接偏差比较大，在地图点中剔除对该关键帧的观测if(!vToErase.empty()){for(size_t i=0;i<vToErase.size();i++){KeyFrame* pKFi = vToErase[i].first;MapPoint* pMPi = vToErase[i].second;pKFi->EraseMapPointMatch(pMPi);pMPi->EraseObservation(pKFi);}}// Recover optimized data// Step 13：优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性//Keyframesfor(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++){KeyFrame* pKF = *lit;g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));g2o::SE3Quat SE3quat = vSE3->estimate();pKF->SetPose(Converter::toCvMat(SE3quat));}//Pointsfor(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));pMP->UpdateNormalAndDepth();}
}

7 剔除冗余关键帧

该步骤主要是通过成员函数void LocalMapping::KeyFrameCulling()实现的，该函数的主要变量如下表：

成员变量	定义
mpCurrentKeyFrame	当前关键帧，本程序就是判断它是否需要删除
pKF	mpCurrentKeyFrame的某一个共视关键帧
vpMapPoints	pKF对应的所有地图点
pMP	vpMapPoints中的某个地图点
observations	所有能观测到pMP的关键帧
pKFi	observations中的某个关键帧
scaleLeveli	pKFi的金字塔尺度
scaleLevel	pKF的金字塔尺度

冗余关键帧的判定标准是：

• 90%以上的地图点能被其他关键帧（至少3个）观测到。

详细的实现步骤如下：

1. 根据共视图提取当前关键帧的所有共视关键帧；
2. 提取每个共视关键帧的地图点；
3. 遍历该共视关键帧的所有地图点，其中能被其它至少3个关键帧观测到的地图点为冗余地图点；
4. 如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧。

// 检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧void LocalMapping::KeyFrameCulling()
{// Step 1：根据共视图提取当前关键帧的所有共视关键帧vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();// 对所有的共视关键帧进行遍历for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++){KeyFrame* pKF = *vit;// 第1个关键帧不能删除，跳过if(pKF->mnId==0)continue;// Step 2：提取每个共视关键帧的地图点const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();// 记录某个点被观测次数，后面并未使用int nObs = 3;                     // 观测次数阈值，默认为3const int thObs=nObs;               // 记录冗余观测点的数目int nRedundantObservations=0;     int nMPs=0;            // Step 3：遍历该共视关键帧的所有地图点，其中能被其它至少3个关键帧观测到的地图点为冗余地图点for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++){MapPoint* pMP = vpMapPoints[i];if(pMP){if(!pMP->isBad()){if(!mbMonocular){// 对于双目或RGB-D，仅考虑近处（不超过基线的40倍 ）的地图点if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)continue;}nMPs++;// pMP->Observations() 是观测到该地图点的相机总数目（单目1，双目2）if(pMP->Observations()>thObs){const int &scaleLevel = pKF->mvKeysUn[i].octave;// Observation存储的是可以看到该地图点的所有关键帧的集合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;// 尺度约束：为什么pKF 尺度+1 要大于等于 pKFi 尺度？// 回答：因为同样或更低金字塔层级的地图点更准确if(scaleLeveli<=scaleLevel+1){nObs++;// 已经找到3个满足条件的关键帧，就停止不找了if(nObs>=thObs)break;}}// 地图点至少被3个关键帧观测到，就记录为冗余点，更新冗余点计数数目if(nObs>=thObs){nRedundantObservations++;}}}}}// Step 4：如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧if(nRedundantObservations>0.9*nMPs)pKF->SetBadFlag();}
}

Reference:

1. https://github.com/raulmur/ORB_SLAM2
2. https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔