ORB-SLAM2 从理论到代码实现（九）：LocalMapping 程序详解 1. 流程图LocalMapping线程负责对新加入的KeyFrames和MapPoints筛选融合剔除冗余的KeyFrames和MapPoints维护稳定的KeyFrame集合传给后续的LoopClosing线程。主要的功能点在处理新的关键帧ProcessNewKeyFrame()剔除不合格地图点MapPointCulling()三角化恢复新地图点CreateNewMapPoints()融合当前帧与相邻帧重复的地图点SearchInNeighbors()局部BA优化LocalBundleAdjustment()剔除冗余关键帧KeyFrameCulling()2. 入口函数LocalMapping::Run()入口函数是LocalMapping.cc中的Run()函数Run()函数也相当于LocalMapping的主函数。这个线程在系统运行起来的时候处于休眠或者等待状态。当有新的关键帧加入的时候线程就将自己设置为繁忙状态告诉Tracking线程我很忙暂时不接受新的关键帧并立刻处理新的关键帧当处理完一个关键帧后就会将自己设置为空闲状态告诉Tracking线程我可以接受新的关键帧了并进入睡眠状态3毫秒。当代码如下void LocalMapping::Run() { mbFinished false; while(1) { // Tracking will see that Local Mapping is busy // 告诉TrackingLocalMapping正处于繁忙状态 // LocalMapping线程处理的关键帧都是Tracking线程发过的 // 在LocalMapping线程还没有处理完关键帧之前Tracking线程最好不要发送太快 SetAcceptKeyFrames(false); // Check if there are keyframes in the queue // 等待处理的关键帧列表不为空 if(CheckNewKeyFrames()) { // BoW conversion and insertion in Map ProcessNewKeyFrame(); // Check recent MapPoints // 剔除ProcessNewKeyFrame函数中引入的不合格MapPoints MapPointCulling(); // Triangulate new MapPoints // 相机运动过程中与相邻关键帧通过三角化恢复出一些MapPoints CreateNewMapPoints(); // 已经处理完队列中的最后的一个关键帧 if(!CheckNewKeyFrames()) { // Find more matches in neighbor keyframes and fuse point duplications // 检查并融合当前关键帧与相邻帧两级相邻重复的MapPoints SearchInNeighbors(); } mbAbortBA false; if(!CheckNewKeyFrames() !stopRequested()) { // Local BA if(mpMap-KeyFramesInMap()2) Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,mbAbortBA, mpMap); // Check redundant local Keyframes // 检测并剔除当前帧相邻的关键帧中冗余的关键帧 // 剔除的标准是该关键帧的90%的MapPoints可以被其它关键帧观测到 // trick! // Tracking中先把关键帧交给LocalMapping线程 // 并且在Tracking中InsertKeyFrame函数的条件比较松交给LocalMapping线程的关键帧会比较密 // 在这里再删除冗余的关键帧 KeyFrameCulling(); } // 将当前帧加入到闭环检测队列中 mpLoopCloser-InsertKeyFrame(mpCurrentKeyFrame); } else if(Stop()) { // Safe area to stop while(isStopped() !CheckFinish()) { usleep(3000); } if(CheckFinish()) break; } ResetIfRequested(); // Tracking will see that Local Mapping is busy SetAcceptKeyFrames(true); if(CheckFinish()) break; usleep(3000); } SetFinish(); }3. void LocalMapping::SetAcceptKeyFrames()告诉Tracking线程LocalMapping是否处于繁忙状态。如果处于繁忙状态则不要再添加新的关键帧了否则可以添加关键帧。void LocalMapping::SetAcceptKeyFrames(bool flag) { unique_lockmutex lock(mMutexAccept); mbAcceptKeyFramesflag; }4. void LocalMapping::ProcessNewKeyFrame()作用是从队列取出第一个关键帧该关键帧时队列中按时间上最旧的关键帧计算该关键帧的Bow特征更新关键帧观测到的地图点的信息并且将该关键帧新生成的地图点添加进mlpRecentAddedMapPoints等待后续检测。然后将该关键帧插入地图。void LocalMapping::ProcessNewKeyFrame() { { unique_lockmutex lock(mMutexNewKFs); mpCurrentKeyFrame mlNewKeyFrames.front(); mlNewKeyFrames.pop_front(); } // Compute Bags of Words structures mpCurrentKeyFrame-ComputeBoW(); // Associate MapPoints to the new keyframe and update normal and descriptor const vectorMapPoint* vpMapPointMatches mpCurrentKeyFrame-GetMapPointMatches(); for(size_t i0; ivpMapPointMatches.size(); i) { MapPoint* pMP vpMapPointMatches[i]; if(pMP) { if(!pMP-isBad()) { // 如果该地图点没有记录该帧则添加上这个记录。 if(!pMP-IsInKeyFrame(mpCurrentKeyFrame)) { // i记录了地图点在关键帧中的索引 pMP-AddObservation(mpCurrentKeyFrame, i); // 因为地图点增加了新的观测而法向量是所有观测到该点的关键帧都求一个法徽号向量之后求均所以需要更新 pMP-UpdateNormalAndDepth(); // 从所有关键帧的观测点中选择一个作为该点的描述子 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. void LocalMapping::MapPointCulling()主要作用是筛选mlpRecentAddedMapPoints里的点对于不好的点标记为bad。已经是坏点的MapPoints直接从检查链表中删除跟踪到该MapPoint的Frame中被判定为内点的比例须大于25%注意不一定是关键帧。从该点建立开始到现在已经过了不小于2个关键帧但是观测到该点的关键帧数却不超过cnThObs帧那么该点检验不合格。从建立该点开始已经过了3个关键帧而没有被剔除则认为是质量高的点因此没有SetBadFlag()仅从队列中删除放弃继续对该MapPoint的检测。mlpRecentAddedMapPoints剩下的点需要继续经过以后的检测。void LocalMapping::MapPointCulling() { // Check Recent Added MapPoints listMapPoint*::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); } else if(pMP-GetFoundRatio()0.25f ) { pMP-SetBadFlag(); lit mlpRecentAddedMapPoints.erase(lit); } else if(((int)nCurrentKFid-(int)pMP-mnFirstKFid)2 pMP-Observations()cnThObs) { pMP-SetBadFlag(); lit mlpRecentAddedMapPoints.erase(lit); } else if(((int)nCurrentKFid-(int)pMP-mnFirstKFid)3) lit mlpRecentAddedMapPoints.erase(lit); else lit; } }5. void LocalMapping::CreateNewMapPoints()利用三角化新建一些地图点在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs遍历相邻关键帧vpNeighKFs得到基线向量vBaseline Ow2-Ow1。判断相机运动的基线是不是足够长邻接关键帧的场景深度中值medianDepthKF2baseline与景深的比例如果特别远(比例特别小)那么不考虑当前邻接的关键帧不生成3D点根据两个关键帧的位姿计算它们之间的基本矩阵F通过极线约束限制匹配时的搜索范围对满足对极约束的特征点进行特征点匹配。对每对匹配通过三角化生成3D点,和Triangulate函数差不多。接着分别检查新得到的点在两个平面上的重投影误差如果大于一定的值直接抛弃该点。检查尺度连续性如果满足对极约束则建立当前帧的地图点及其属性a.观测到该MapPoint的关键帧 b.该MapPoint的描述子 c.该MapPoint的平均观测方向和深度范围将地图点加入关键帧加入全局mapvoid LocalMapping::CreateNewMapPoints() { // Retrieve neighbor keyframes in covisibility graph int nn 10; if(mbMonocular) nn20; //在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs const vectorKeyFrame* vpNeighKFs mpCurrentKeyFrame-GetBestCovisibilityKeyFrames(nn); 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; const float ratioFactor 1.5f*mpCurrentKeyFrame-mfScaleFactor; int nnew0; // Search matches with epipolar restriction and triangulate for(size_t i0; ivpNeighKFs.size(); i) { // 基于实时性考虑如果已经处理了与新关键帧最邻近的一个关键帧但又来了更新的关键帧则停止立即返回去处理更新的关键帧。 if(i0 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); if(!mbMonocular) { if(baselinepKF2-mb) continue; } else { const float medianDepthKF2 pKF2-ComputeSceneMedianDepth(2); const float ratioBaselineDepth baseline/medianDepthKF2; if(ratioBaselineDepth0.01) continue; } // Compute Fundamental Matrix //根据两个关键帧的位姿计算它们之间的基本矩阵F cv::Mat F12 ComputeF12(mpCurrentKeyFrame,pKF2); // Search matches that fullfil epipolar constraint vectorpairsize_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 const int nmatches vMatchedIndices.size(); for(int ikp0; ikpnmatches; ikp) { const int idx1 vMatchedIndices[ikp].first; const int idx2 vMatchedIndices[ikp].second; const cv::KeyPoint kp1 mpCurrentKeyFrame-mvKeysUn[idx1]; const float kp1_urmpCurrentKeyFrame-mvuRight[idx1]; bool bStereo1 kp1_ur0; // 在上一帧中被双目观测到 const cv::KeyPoint kp2 pKF2-mvKeysUn[idx2]; const float kp2_ur pKF2-mvuRight[idx2]; bool bStereo2 kp2_ur0; // 在当前帧中被双目观测到 // Check parallax between rays 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)); float cosParallaxStereo cosParallaxRays1; float cosParallaxStereo1 cosParallaxStereo; float cosParallaxStereo2 cosParallaxStereo; 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); cv::Mat x3D; if(cosParallaxRayscosParallaxStereo cosParallaxRays0 (bStereo1 || bStereo2 || cosParallaxRays0.9998)) { // Linear Triangulation Method cv::Mat A(4,4,CV_32F); A.row(0) xn1.atfloat(0)*Tcw1.row(2)-Tcw1.row(0); A.row(1) xn1.atfloat(1)*Tcw1.row(2)-Tcw1.row(1); A.row(2) xn2.atfloat(0)*Tcw2.row(2)-Tcw2.row(0); A.row(3) xn2.atfloat(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.atfloat(3)0) continue; // Euclidean coordinates x3D x3D.rowRange(0,3)/x3D.atfloat(3); } else if(bStereo1 cosParallaxStereo1cosParallaxStereo2) { x3D mpCurrentKeyFrame-UnprojectStereo(idx1); } else if(bStereo2 cosParallaxStereo2cosParallaxStereo1) { x3D pKF2-UnprojectStereo(idx2); } else continue; //No stereo and very low parallax cv::Mat x3Dt x3D.t(); //Check triangulation in front of cameras float z1 Rcw1.row(2).dot(x3Dt)tcw1.atfloat(2); if(z10) continue; float z2 Rcw2.row(2).dot(x3Dt)tcw2.atfloat(2); if(z20) continue; //Check reprojection error in first keyframe const float sigmaSquare1 mpCurrentKeyFrame-mvLevelSigma2[kp1.octave]; const float x1 Rcw1.row(0).dot(x3Dt)tcw1.atfloat(0); const float y1 Rcw1.row(1).dot(x3Dt)tcw1.atfloat(1); const float invz1 1.0/z1; if(!bStereo1) { float u1 fx1*x1*invz1cx1; float v1 fy1*y1*invz1cy1; float errX1 u1 - kp1.pt.x; float errY1 v1 - kp1.pt.y; if((errX1*errX1errY1*errY1)5.991*sigmaSquare1) continue; } else { float u1 fx1*x1*invz1cx1; float u1_r u1 - mpCurrentKeyFrame-mbf*invz1; float v1 fy1*y1*invz1cy1; float errX1 u1 - kp1.pt.x; float errY1 v1 - kp1.pt.y; float errX1_r u1_r - kp1_ur; if((errX1*errX1errY1*errY1errX1_r*errX1_r)7.8*sigmaSquare1) continue; } //Check reprojection error in second keyframe const float sigmaSquare2 pKF2-mvLevelSigma2[kp2.octave]; const float x2 Rcw2.row(0).dot(x3Dt)tcw2.atfloat(0); const float y2 Rcw2.row(1).dot(x3Dt)tcw2.atfloat(1); const float invz2 1.0/z2; if(!bStereo2) { float u2 fx2*x2*invz2cx2; float v2 fy2*y2*invz2cy2; float errX2 u2 - kp2.pt.x; float errY2 v2 - kp2.pt.y; if((errX2*errX2errY2*errY2)5.991*sigmaSquare2) continue; } else { float u2 fx2*x2*invz2cx2; float u2_r u2 - mpCurrentKeyFrame-mbf*invz2; float v2 fy2*y2*invz2cy2; float errX2 u2 - kp2.pt.x; float errY2 v2 - kp2.pt.y; float errX2_r u2_r - kp2_ur; if((errX2*errX2errY2*errY2errX2_r*errX2_r)7.8*sigmaSquare2) continue; } //Check scale consistency cv::Mat normal1 x3D-Ow1; float dist1 cv::norm(normal1); cv::Mat normal2 x3D-Ow2; float dist2 cv::norm(normal2); if(dist10 || dist20) continue; const float ratioDist dist2/dist1; const float ratioOctave mpCurrentKeyFrame-mvScaleFactors[kp1.octave]/pKF2-mvScaleFactors[kp2.octave]; /*if(fabs(ratioDist-ratioOctave)ratioFactor) continue;*/ if(ratioDist*ratioFactorratioOctave || ratioDistratioOctave*ratioFactor) continue; // Triangulation is succesfull MapPoint* pMP new MapPoint(x3D,mpCurrentKeyFrame,mpMap); pMP-AddObservation(mpCurrentKeyFrame,idx1); pMP-AddObservation(pKF2,idx2); mpCurrentKeyFrame-AddMapPoint(pMP,idx1); pKF2-AddMapPoint(pMP,idx2); pMP-ComputeDistinctiveDescriptors(); pMP-UpdateNormalAndDepth(); mpMap-AddMapPoint(pMP); mlpRecentAddedMapPoints.push_back(pMP); nnew; } } }6. void LocalMapping::SearchInNeighbors().检查并融合当前关键帧与相邻帧两级相邻重复的MapPoints更新当前关键帧的连接关系。获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧找到当前帧一级相邻与二级相邻关键帧将当前帧的MapPoints分别与一级二级相邻帧的MapPoints进行融合matcher.Fuse(pKFi,vpMapPointMatches);投影当前帧的MapPoints到相邻关键帧pKFi中并判断是否有重复的MapPoints如果MapPoint能匹配关键帧的特征点并且该点有对应的MapPoint那么将两个MapPoint合并选择观测数多的如果MapPoint能匹配关键帧的特征点并且该点没有对应的MapPoint那么为该点添加MapPoint将一级二级相邻帧的MapPoints分别与当前帧的MapPoints进行融合更新当前帧MapPoints的描述子深度观测主方向等属性在这里插入代码片更新当前帧的MapPoints后更新与其它帧的连接关系void LocalMapping::SearchInNeighbors() { // Retrieve neighbor keyframes int nn 10; if(mbMonocular) nn20; const vectorKeyFrame* vpNeighKFs mpCurrentKeyFrame-GetBestCovisibilityKeyFrames(nn); vectorKeyFrame* vpTargetKFs; for(vectorKeyFrame*::const_iterator vitvpNeighKFs.begin(), vendvpNeighKFs.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 const vectorKeyFrame* vpSecondNeighKFs pKFi-GetBestCovisibilityKeyFrames(5); for(vectorKeyFrame*::const_iterator vit2vpSecondNeighKFs.begin(), vend2vpSecondNeighKFs.end(); vit2!vend2; vit2) { KeyFrame* pKFi2 *vit2; if(pKFi2-isBad() || pKFi2-mnFuseTargetForKFmpCurrentKeyFrame-mnId || pKFi2-mnIdmpCurrentKeyFrame-mnId) continue; vpTargetKFs.push_back(pKFi2); } } // Search matches by projection from current KF in target KFs ORBmatcher matcher; vectorMapPoint* vpMapPointMatches mpCurrentKeyFrame-GetMapPointMatches(); for(vectorKeyFrame*::iterator vitvpTargetKFs.begin(), vendvpTargetKFs.end(); vit!vend; vit) { KeyFrame* pKFi *vit; matcher.Fuse(pKFi,vpMapPointMatches); } // Search matches by projection from target KFs in current KF vectorMapPoint* vpFuseCandidates; vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size()); for(vectorKeyFrame*::iterator vitKFvpTargetKFs.begin(), vendKFvpTargetKFs.end(); vitKF!vendKF; vitKF) { KeyFrame* pKFi *vitKF; vectorMapPoint* vpMapPointsKFi pKFi-GetMapPointMatches(); for(vectorMapPoint*::iterator vitMPvpMapPointsKFi.begin(), vendMPvpMapPointsKFi.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 i0, iendvpMapPointMatches.size(); iiend; i) { MapPoint* pMPvpMapPointMatches[i]; if(pMP) { if(!pMP-isBad()) { pMP-ComputeDistinctiveDescriptors(); pMP-UpdateNormalAndDepth(); } } } // Update connections in covisibility graph mpCurrentKeyFrame-UpdateConnections(); }7. void Optimizer::LocalBundleAdjustment()优化的顶点是包括局部地图帧的位姿概念lLocalKeyFrames指的是当前关键帧和其相连接的关键帧组成的集合。还包括这些关键帧可以观测到的所有地图点地图点的位置也会优化。还有一些帧这些帧能够观测到这些地图点但却不是局部地图里这些帧的位姿也作为顶点添加进图中但是却固定不动不会被优化。void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap) { // Local KeyFrames: First Breath Search from Current Keyframe listKeyFrame* lLocalKeyFrames; lLocalKeyFrames.push_back(pKF); pKF-mnBALocalForKF pKF-mnId; const vectorKeyFrame* vNeighKFs pKF-GetVectorCovisibleKeyFrames(); for(int i0, iendvNeighKFs.size(); iiend; i) { KeyFrame* pKFi vNeighKFs[i]; pKFi-mnBALocalForKF pKF-mnId; if(!pKFi-isBad()) lLocalKeyFrames.push_back(pKFi); } // Local MapPoints seen in Local KeyFrames listMapPoint* lLocalMapPoints; for(listKeyFrame*::iterator litlLocalKeyFrames.begin() , lendlLocalKeyFrames.end(); lit!lend; lit) { vectorMapPoint* vpMPs (*lit)-GetMapPointMatches(); for(vectorMapPoint*::iterator vitvpMPs.begin(), vendvpMPs.end(); vit!vend; vit) { MapPoint* pMP *vit; if(pMP) if(!pMP-isBad()) if(pMP-mnBALocalForKF!pKF-mnId) { lLocalMapPoints.push_back(pMP); pMP-mnBALocalForKFpKF-mnId; } } } // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes listKeyFrame* lFixedCameras; for(listMapPoint*::iterator litlLocalMapPoints.begin(), lendlLocalMapPoints.end(); lit!lend; lit) { mapKeyFrame*,size_t observations (*lit)-GetObservations(); for(mapKeyFrame*,size_t::iterator mitobservations.begin(), mendobservations.end(); mit!mend; mit) { KeyFrame* pKFi mit-first; if(pKFi-mnBALocalForKF!pKF-mnId pKFi-mnBAFixedForKF!pKF-mnId) { pKFi-mnBAFixedForKFpKF-mnId; if(!pKFi-isBad()) lFixedCameras.push_back(pKFi); } } } // Setup optimizer g2o::SparseOptimizer optimizer; g2o::BlockSolver_6_3::LinearSolverType * linearSolver; linearSolver new g2o::LinearSolverEigeng2o::BlockSolver_6_3::PoseMatrixType(); g2o::BlockSolver_6_3 * solver_ptr new g2o::BlockSolver_6_3(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); if(pbStopFlag) optimizer.setForceStopFlag(pbStopFlag); unsigned long maxKFid 0; // Set Local KeyFrame vertices for(listKeyFrame*::iterator litlLocalKeyFrames.begin(), lendlLocalKeyFrames.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-mnId0); optimizer.addVertex(vSE3); if(pKFi-mnIdmaxKFid) maxKFidpKFi-mnId; } // Set Fixed KeyFrame vertices for(listKeyFrame*::iterator litlFixedCameras.begin(), lendlFixedCameras.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-mnIdmaxKFid) maxKFidpKFi-mnId; } // Set MapPoint vertices const int nExpectedSize (lLocalKeyFrames.size()lFixedCameras.size())*lLocalMapPoints.size(); vectorg2o::EdgeSE3ProjectXYZ* vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); vectorKeyFrame* vpEdgeKFMono; vpEdgeKFMono.reserve(nExpectedSize); vectorMapPoint* vpMapPointEdgeMono; vpMapPointEdgeMono.reserve(nExpectedSize); vectorg2o::EdgeStereoSE3ProjectXYZ* vpEdgesStereo; vpEdgesStereo.reserve(nExpectedSize); vectorKeyFrame* vpEdgeKFStereo; vpEdgeKFStereo.reserve(nExpectedSize); vectorMapPoint* vpMapPointEdgeStereo; vpMapPointEdgeStereo.reserve(nExpectedSize); const float thHuberMono sqrt(5.991); const float thHuberStereo sqrt(7.815); for(listMapPoint*::iterator litlLocalMapPoints.begin(), lendlLocalMapPoints.end(); lit!lend; lit) { MapPoint* pMP *lit; g2o::VertexSBAPointXYZ* vPoint new g2o::VertexSBAPointXYZ(); vPoint-setEstimate(Converter::toVector3d(pMP-GetWorldPos())); int id pMP-mnIdmaxKFid1; vPoint-setId(id); vPoint-setMarginalized(true); optimizer.addVertex(vPoint); const mapKeyFrame*,size_t observations pMP-GetObservations(); //Set edges for(mapKeyFrame*,size_t::const_iterator mitobservations.begin(), mendobservations.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::Matrixdouble,2,1 obs; obs kpUn.pt.x, kpUn.pt.y; g2o::EdgeSE3ProjectXYZ* e new g2o::EdgeSE3ProjectXYZ(); e-setVertex(0, dynamic_castg2o::OptimizableGraph::Vertex*(optimizer.vertex(id))); e-setVertex(1, dynamic_castg2o::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 { Eigen::Matrixdouble,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_castg2o::OptimizableGraph::Vertex*(optimizer.vertex(id))); e-setVertex(1, dynamic_castg2o::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); } } } } if(pbStopFlag) if(*pbStopFlag) return; optimizer.initializeOptimization(); optimizer.optimize(5); bool bDoMore true; if(pbStopFlag) if(*pbStopFlag) bDoMore false; if(bDoMore) { // Check inlier observations for(size_t i0, iendvpEdgesMono.size(); iiend;i) { g2o::EdgeSE3ProjectXYZ* e vpEdgesMono[i]; MapPoint* pMP vpMapPointEdgeMono[i]; if(pMP-isBad()) continue; if(e-chi2()5.991 || !e-isDepthPositive()) { e-setLevel(1); } e-setRobustKernel(0); } for(size_t i0, iendvpEdgesStereo.size(); iiend;i) { g2o::EdgeStereoSE3ProjectXYZ* e vpEdgesStereo[i]; MapPoint* pMP vpMapPointEdgeStereo[i]; if(pMP-isBad()) continue; if(e-chi2()7.815 || !e-isDepthPositive()) { e-setLevel(1); } e-setRobustKernel(0); } // Optimize again without the outliers optimizer.initializeOptimization(0); optimizer.optimize(10); } vectorpairKeyFrame*,MapPoint* vToErase; vToErase.reserve(vpEdgesMono.size()vpEdgesStereo.size()); // Check inlier observations for(size_t i0, iendvpEdgesMono.size(); iiend;i) { g2o::EdgeSE3ProjectXYZ* e vpEdgesMono[i]; MapPoint* pMP vpMapPointEdgeMono[i]; if(pMP-isBad()) continue; if(e-chi2()5.991 || !e-isDepthPositive()) { KeyFrame* pKFi vpEdgeKFMono[i]; vToErase.push_back(make_pair(pKFi,pMP)); } } for(size_t i0, iendvpEdgesStereo.size(); iiend;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 Mutex unique_lockmutex lock(pMap-mMutexMapUpdate); if(!vToErase.empty()) { for(size_t i0;ivToErase.size();i) { KeyFrame* pKFi vToErase[i].first; MapPoint* pMPi vToErase[i].second; pKFi-EraseMapPointMatch(pMPi); pMPi-EraseObservation(pKFi); } } // Recover optimized data //Keyframes for(listKeyFrame*::iterator litlLocalKeyFrames.begin(), lendlLocalKeyFrames.end(); lit!lend; lit) { KeyFrame* pKF *lit; g2o::VertexSE3Expmap* vSE3 static_castg2o::VertexSE3Expmap*(optimizer.vertex(pKF-mnId)); g2o::SE3Quat SE3quat vSE3-estimate(); pKF-SetPose(Converter::toCvMat(SE3quat)); } //Points for(listMapPoint*::iterator litlLocalMapPoints.begin(), lendlLocalMapPoints.end(); lit!lend; lit) { MapPoint* pMP *lit; g2o::VertexSBAPointXYZ* vPoint static_castg2o::VertexSBAPointXYZ*(optimizer.vertex(pMP-mnIdmaxKFid1)); pMP-SetWorldPos(Converter::toCvMat(vPoint-estimate())); pMP-UpdateNormalAndDepth(); } }8. void LocalMapping::KeyFrameCulling()在Covisibility Graph也就是局部地图中的关键帧一个关键帧的90%以上的MapPoints能被其他关键帧至少3个这里的其他关键帧不特指当前的局部地图关键帧观测到则认为该关键帧为冗余关键帧。根据Covisibility Graph提取当前帧的共视关键帧对所有的局部关键帧进行遍历提取每个共视关键帧的MapPoints遍历该局部关键帧的MapPoints判断是否90%以上的MapPoints能被其它关键帧至少3个观测到该局部关键帧90%以上的MapPoints能被其它关键帧至少3个观测到则认为是冗余关键帧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 // 检查冗余关键帧。一个关键帧的地图点中90%的地图点可以被至少其他3个相同或者更小等级这里的等级是图像金字塔的层数关键帧看到则认为这个关键帧是冗余的。 vectorKeyFrame* vpLocalKeyFrames mpCurrentKeyFrame-GetVectorCovisibleKeyFrames(); for(vectorKeyFrame*::iterator vitvpLocalKeyFrames.begin(), vendvpLocalKeyFrames.end(); vit!vend; vit) { KeyFrame* pKF *vit; if(pKF-mnId0) continue; const vectorMapPoint* vpMapPoints pKF-GetMapPointMatches(); int nObs 3; const int thObsnObs; int nRedundantObservations0; int nMPs0; for(size_t i0, iendvpMapPoints.size(); iiend; i) { MapPoint* pMP vpMapPoints[i]; if(pMP) { if(!pMP-isBad()) { if(!mbMonocular) { if(pKF-mvDepth[i]pKF-mThDepth || pKF-mvDepth[i]0) continue; } //MapPoint计数 nMPs; //该pMP是否可以被大于3个的关键帧看到 if(pMP-Observations()thObs) { const int scaleLevel pKF-mvKeysUn[i].octave; const mapKeyFrame*, size_t observations pMP-GetObservations(); int nObs0; for(mapKeyFrame*, size_t::const_iterator mitobservations.begin(), mendobservations.end(); mit!mend; mit) { KeyFrame* pKFi mit-first; if(pKFipKF) continue; // 获取关键点在金字塔图像中所处的层数 const int scaleLeveli pKFi-mvKeysUn[mit-second].octave; const int scaleLeveli pKFi-mvKeysUn[mit-second].octave; //pKFi的关键点所处的层数scaleLevel1 //为什么要用层数来判断呢还没有想明白 if(scaleLeveliscaleLevel1) { //共视关键帧计数 nObs; if(nObsthObs) break; } } //共视关键帧大于等于3判断 if(nObsthObs) { nRedundantObservations; } } } } } if(nRedundantObservations0.9*nMPs) //关键帧设置为bag即被认为是冗余关键帧 pKF-SetBadFlag(); } }参考文献主要内容来自下文重写了一说说明添加了一些注释读懂ORBSLAM2——LocalMapping线程_令狐少侠、的博客-CSDN博客