企业官网建设流程全解析

今天LZ的效率有些低下，为啥？说出来让大家娱乐娱乐吧，毕竟程序员需要自己找乐子。LZ今天用尾巴骨测试了学校楼梯的硬度。实验对象为食堂的楼梯和LZ的尾巴骨，实验共重复了5次，分别在相同的重力加速度下进行迅速撞击，最后的测试结果是楼梯硬！尾巴骨完败，不说了LZ趴着歇会去了。。。

#ifndef LOCALMAPPING_H #define LOCALMAPPING_H #include "KeyFrame.h" #include "Map.h" #include "LoopClosing.h" #include "Tracking.h" #include "KeyFrameDatabase.h" #include <mutex> namespace ORB_SLAM2 { class Tracking; class LoopClosing; class Map; class LocalMapping { public: LocalMapping(Map* pMap, const float bMonocular); void SetLoopCloser(LoopClosing* pLoopCloser); void SetTracker(Tracking* pTracker); // Main function void Run(); void InsertKeyFrame(KeyFrame* pKF); // Thread Synch void RequestStop(); void RequestReset(); bool Stop(); void Release(); bool isStopped(); bool stopRequested(); bool AcceptKeyFrames(); void SetAcceptKeyFrames(bool flag); bool SetNotStop(bool flag); void InterruptBA(); void RequestFinish(); bool isFinished(); int KeyframesInQueue(){ unique_lock<std::mutex> lock(mMutexNewKFs); return mlNewKeyFrames.size(); } protected: bool CheckNewKeyFrames(); void ProcessNewKeyFrame(); void CreateNewMapPoints(); void MapPointCulling(); void SearchInNeighbors(); void KeyFrameCulling(); cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2); cv::Mat SkewSymmetricMatrix(const cv::Mat &v); bool mbMonocular; void ResetIfRequested(); bool mbResetRequested; std::mutex mMutexReset; bool CheckFinish(); void SetFinish(); bool mbFinishRequested; bool mbFinished; std::mutex mMutexFinish; Map* mpMap; LoopClosing* mpLoopCloser; Tracking* mpTracker; std::list<KeyFrame*> mlNewKeyFrames; KeyFrame* mpCurrentKeyFrame; std::list<MapPoint*> mlpRecentAddedMapPoints; std::mutex mMutexNewKFs; bool mbAbortBA; bool mbStopped; bool mbStopRequested; bool mbNotStop; std::mutex mMutexStop; bool mbAcceptKeyFrames; std::mutex mMutexAccept; }; } //namespace ORB_SLAM #endif // LOCALMAPPING_H

#include "LocalMapping.h" #include "LoopClosing.h" #include "ORBmatcher.h" #include "Optimizer.h" #include<mutex> namespace ORB_SLAM2 { LocalMapping::LocalMapping(Map *pMap, const float bMonocular): mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap), mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true) { } void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser) { mpLoopCloser = pLoopCloser; } void LocalMapping::SetTracker(Tracking *pTracker) { mpTracker=pTracker; } void LocalMapping::Run() { mbFinished = false; while(1) { // Tracking will see that Local Mapping is busy //如果tracking线程发现Local Mapping线程还在处理状态，则不接受关键帧 SetAcceptKeyFrames(false); // Check if there are keyframes in the queue //检查队列中是否存在关键帧 ////对Tracking中送来的每一帧关键帧进行下列操作，直到mlNewKeyFrame为空 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()) { usleep(3000); } if(CheckFinish()) break; } ResetIfRequested(); // Tracking will see that Local Mapping is busy SetAcceptKeyFrames(true); if(CheckFinish()) break; usleep(3000); } SetFinish(); } //插入关键帧 void LocalMapping::InsertKeyFrame(KeyFrame *pKF) { unique_lock<mutex> lock(mMutexNewKFs); mlNewKeyFrames.push_back(pKF); mbAbortBA=true; } //检查新的关键帧，看关键帧是否为空 bool LocalMapping::CheckNewKeyFrames() { unique_lock<mutex> lock(mMutexNewKFs); return(!mlNewKeyFrames.empty()); } //处理新的关键帧 void LocalMapping::ProcessNewKeyFrame() { { unique_lock<mutex> lock(mMutexNewKFs); //取出新的关键帧存入当前帧 mpCurrentKeyFrame = mlNewKeyFrames.front(); // member,list,也就是说从存新的关键帧的成员函数中删除要处理当前的关键帧 mlNewKeyFrames.pop_front(); } // Compute Bags of Words structures //计算当关键帧BoW，便于后面三角化恢复新地图点 mpCurrentKeyFrame->ComputeBoW(); // Associate MapPoints to the new keyframe and update normal and descriptor //将TrackLocalMap中跟踪局部地图匹配上的地图点绑定到当前关键帧 //Tracking线程中只是通过匹配进行局部地图跟踪，优化当前关键帧姿态 //也就是在graph中加入当前关键帧作为node，并更新edge 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 { mlpRecentAddedMapPoints.push_back(pMP); } } } } // Update links in the Covisibility Graph //更新加入当前关键帧之后关键帧之间的连接关系 //包括更新Covisibility图和Essential图（最小生成树spanning tree） //共视关系好的边subset of edges from covisibility graph with high covisibility(theta = 100,闭环边) mpCurrentKeyFrame->UpdateConnections(); // Insert Keyframe in Map mpMap->AddKeyFrame(mpCurrentKeyFrame); } //对于ProcessNewKeyFrame和CreateNewMapPoints中最近添加的MapPoints进行检查剔除 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); } //跟踪(匹配上)到该地图点的普通帧数(increaseFound)<应该观测到该地图点的普通帧数量(25%IncreaseVisible) //该地图点虽然在视野范围内，但是很少被普通帧检测到 else if(pMP->GetFoundRatio()<0.25f ) { pMP->SetBadFlag(); lit = mlpRecentAddedMapPoints.erase(lit); } //从添加该地图点的关键帧算起的初始三个关键帧，第一帧不算，后面两帧看到该地图点的帧数 //对于单目<=2，对于双目和RGBD<=3;因此在地图点刚建立的阶段，要求比较严格，很容易被剔除 //而且单目的要求更严格，需要三帧都看到。若从添加该地图点的管家孩子呢算起，一共有了大于三个 //关键帧，还存在列表，则说明该地图点是高质量的，从检查列表中去掉 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++; } //一点经过了这样比较严格的筛选，地图点只有在观测到它的关键帧<3时才会被剔除，这主要发生在关键帧被 //剔除(90%以上匹配点可以被其他帧观测到)；或者局部捆集优化时，将地图点归为外点从观测中剔除了情况 //因此地图点中的外点是比较少的，所以整套ORB-SLAM中除了重定位见图和闭环很少去使用RANSAC，使用RANSAC会比较慢。 //ORB_SLAM中关键帧和地图点的加入和删除秉承第送入严出的标准，因此在提高了定位见图准确性的 //前提下又很好的限制了计算量，可以用于large scale的场景 } //通过关键帧，在局部地图中添加新的地图点 //根据当前关键帧恢复出一些新的地图点，不包括和当前关键帧匹配的局部地图点(已经在ProcessNewKeyFrame中处理) //注意理解与前面两步的先后关系，先处理新关键帧与局部地图点之间的关系，然后对局部地图点进行检查 //最后再通过新关键帧恢复新的地图点 void LocalMapping::CreateNewMapPoints() { // Retrieve neighbor keyframes in covisibility graph int nn = 10; if(mbMonocular) nn=20; //双目和RGBD需要十个与当前关键帧共视关系的最好的关键帧 //单目需要二十个 const vector<KeyFrame*> 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 nnew=0; // Search matches with epipolar restriction and triangulate for(size_t i=0; i<vpNeighKFs.size(); i++) { 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); if(!mbMonocular) { if(baseline<pKF2->mb) continue; } else { const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2); const float ratioBaselineDepth = baseline/medianDepthKF2; if(ratioBaselineDepth<0.01) continue; } // Compute Fundamental Matrix cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2); // Search matches that fullfil epipolar constraint 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 const int nmatches = vMatchedIndices.size(); for(int ikp=0; ikp<nmatches; ikp++) { const int &idx1 = vMatchedIndices[ikp].first; const int &idx2 = vMatchedIndices[ikp].second; const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1]; const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1]; bool bStereo1 = kp1_ur>=0; const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2]; const float kp2_ur = pKF2->mvuRight[idx2]; bool bStereo2 = kp2_ur>=0; // 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 = cosParallaxRays+1; 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(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)) { // Linear Triangulation Method 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; // Euclidean coordinates x3D = x3D.rowRange(0,3)/x3D.at<float>(3); } else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2) { x3D = mpCurrentKeyFrame->UnprojectStereo(idx1); } else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1) { 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.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 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; if((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; if((errX1*errX1+errY1*errY1+errX1_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.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 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; 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 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++; } } } //更新并融合当前关键帧以及两集相连(共视关键帧及其共视关键帧)的关键帧的地图点 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; 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(); } //计算基本矩阵 cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2) { cv::Mat R1w = pKF1->GetRotation(); cv::Mat t1w = pKF1->GetTranslation(); cv::Mat R2w = pKF2->GetRotation(); cv::Mat t2w = pKF2->GetTranslation(); cv::Mat R12 = R1w*R2w.t(); cv::Mat t12 = -R1w*R2w.t()*t2w+t1w; cv::Mat t12x = SkewSymmetricMatrix(t12); const cv::Mat &K1 = pKF1->mK; const cv::Mat &K2 = pKF2->mK; return K1.t().inv()*t12x*R12*K2.inv(); } //需求停止 void LocalMapping::RequestStop() { unique_lock<mutex> lock(mMutexStop); mbStopRequested = true; unique_lock<mutex> lock2(mMutexNewKFs); mbAbortBA = true; } //local mapping 停止 bool LocalMapping::Stop() { unique_lock<mutex> lock(mMutexStop); if(mbStopRequested && !mbNotStop) { mbStopped = true; cout << "Local Mapping STOP" << endl; return true; } return false; } //判断是否停止 bool LocalMapping::isStopped() { unique_lock<mutex> lock(mMutexStop); return mbStopped; } bool LocalMapping::stopRequested() { unique_lock<mutex> lock(mMutexStop); return mbStopRequested; } void LocalMapping::Release() { unique_lock<mutex> lock(mMutexStop); unique_lock<mutex> lock2(mMutexFinish); if(mbFinished) return; mbStopped = false; mbStopRequested = false; for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) delete *lit; mlNewKeyFrames.clear(); cout << "Local Mapping RELEASE" << endl; } bool LocalMapping::AcceptKeyFrames() { unique_lock<mutex> lock(mMutexAccept); return mbAcceptKeyFrames; } void LocalMapping::SetAcceptKeyFrames(bool flag) { unique_lock<mutex> lock(mMutexAccept); mbAcceptKeyFrames=flag; } bool LocalMapping::SetNotStop(bool flag) { unique_lock<mutex> lock(mMutexStop); if(flag && mbStopped) return false; mbNotStop = flag; return true; } //终止BA void LocalMapping::InterruptBA() { mbAbortBA = true; } //关键帧剔除 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 //候选的关键帧是LocalMapping中处理的关键帧的共视帧，不包括第一帧关键帧与当前关键帧 vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames(); for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++) { KeyFrame* pKF = *vit; 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) { 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; //观测尺度不大于或近似于在关键帧的尺度 if(scaleLeveli<=scaleLevel+1) { nObs++; //至少3帧观测到这个地图点 if(nObs>=thObs) break; } } if(nObs>=thObs) { nRedundantObservations++; } } } } } //若关键帧中90%的地图点能够被其他关键帧观测到，也就是说， //这个关键帧提供的信息有限，则将其删除 if(nRedundantObservations>0.9*nMPs) pKF->SetBadFlag(); } } //反对称矩阵构造 cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v) { return (cv::Mat_<float>(3,3) << 0, -v.at<float>(2), v.at<float>(1), v.at<float>(2), 0,-v.at<float>(0), -v.at<float>(1), v.at<float>(0), 0); } void LocalMapping::RequestReset() { { unique_lock<mutex> lock(mMutexReset); mbResetRequested = true; } while(1) { { unique_lock<mutex> lock2(mMutexReset); if(!mbResetRequested) break; } usleep(3000); } } void LocalMapping::ResetIfRequested() { unique_lock<mutex> lock(mMutexReset); if(mbResetRequested) { mlNewKeyFrames.clear(); mlpRecentAddedMapPoints.clear(); mbResetRequested=false; } } void LocalMapping::RequestFinish() { unique_lock<mutex> lock(mMutexFinish); mbFinishRequested = true; } bool LocalMapping::CheckFinish() { unique_lock<mutex> lock(mMutexFinish); return mbFinishRequested; } void LocalMapping::SetFinish() { unique_lock<mutex> lock(mMutexFinish); mbFinished = true; unique_lock<mutex> lock2(mMutexStop); mbStopped = true; } bool LocalMapping::isFinished() { unique_lock<mutex> lock(mMutexFinish); return mbFinished; } } //namespace ORB_SLAM

坐立难安，如坐针毡，LZ的尾巴骨哟O(∩_∩)O哈哈~加油啊，Felaim！