话不多说，直接上代码了。。。
#ifndef KEYFRAME_H #define KEYFRAME_H #include "MapPoint.h" #include "Thirdparty/DBoW2/DBoW2/BowVector.h" #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h" #include "ORBVocabulary.h" #include "ORBextractor.h" #include "Frame.h" #include "KeyFrameDatabase.h" #include <mutex> namespace ORB_SLAM2 { class Map; class MapPoint; class Frame; class KeyFrameDatabase; //KeyFrame类利用Frame类来构造，对于什么样的Frame可以认为是关键帧以及何时需要 //加入关键帧的判断都是在Tracking的线程中实现的 class KeyFrame { public: KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB); // Pose functions //相机坐标Z朝北，Z朝东，Y朝地 //给出了get函数获取参数 void SetPose(const cv::Mat &Tcw); cv::Mat GetPose(); cv::Mat GetPoseInverse(); cv::Mat GetCameraCenter(); cv::Mat GetStereoCenter(); cv::Mat GetRotation(); cv::Mat GetTranslation(); // Bag of Words Representation void ComputeBoW(); // Covisibility graph functions void AddConnection(KeyFrame* pKF, const int &weight); void EraseConnection(KeyFrame* pKF); void UpdateConnections(); void UpdateBestCovisibles(); std::set<KeyFrame *> GetConnectedKeyFrames(); std::vector<KeyFrame* > GetVectorCovisibleKeyFrames(); std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int &N); std::vector<KeyFrame*> GetCovisiblesByWeight(const int &w); int GetWeight(KeyFrame* pKF); // Spanning tree functions void AddChild(KeyFrame* pKF); void EraseChild(KeyFrame* pKF); void ChangeParent(KeyFrame* pKF); std::set<KeyFrame*> GetChilds(); KeyFrame* GetParent(); bool hasChild(KeyFrame* pKF); // Loop Edges void AddLoopEdge(KeyFrame* pKF); std::set<KeyFrame*> GetLoopEdges(); // MapPoint observation functions void AddMapPoint(MapPoint* pMP, const size_t &idx); void EraseMapPointMatch(const size_t &idx); void EraseMapPointMatch(MapPoint* pMP); void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP); std::set<MapPoint*> GetMapPoints(); std::vector<MapPoint*> GetMapPointMatches(); int TrackedMapPoints(const int &minObs); MapPoint* GetMapPoint(const size_t &idx); // KeyPoint functions std::vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r) const; cv::Mat UnprojectStereo(int i); // Image bool IsInImage(const float &x, const float &y) const; // Enable/Disable bad flag changes void SetNotErase(); void SetErase(); // Set/check bad flag void SetBadFlag(); bool isBad(); // Compute Scene Depth (q=2 median). Used in monocular. float ComputeSceneMedianDepth(const int q); static bool weightComp( int a, int b){ return a>b; } static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){ return pKF1->mnId<pKF2->mnId; } // The following variables are accesed from only 1 thread or never change (no mutex needed). public: static long unsigned int nNextId; long unsigned int mnId; const long unsigned int mnFrameId; const double mTimeStamp; // Grid (to speed up feature matching) const int mnGridCols; const int mnGridRows; const float mfGridElementWidthInv; const float mfGridElementHeightInv; // Variables used by the tracking long unsigned int mnTrackReferenceForFrame; long unsigned int mnFuseTargetForKF; // Variables used by the local mapping long unsigned int mnBALocalForKF; long unsigned int mnBAFixedForKF; // Variables used by the keyframe database long unsigned int mnLoopQuery; int mnLoopWords; float mLoopScore; long unsigned int mnRelocQuery; int mnRelocWords; float mRelocScore; // Variables used by loop closing cv::Mat mTcwGBA; cv::Mat mTcwBefGBA; long unsigned int mnBAGlobalForKF; // Calibration parameters const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth; // Number of KeyPoints const int N; // KeyPoints, stereo coordinate and descriptors (all associated by an index) const std::vector<cv::KeyPoint> mvKeys; const std::vector<cv::KeyPoint> mvKeysUn; const std::vector<float> mvuRight; // negative value for monocular points const std::vector<float> mvDepth; // negative value for monocular points const cv::Mat mDescriptors; //BoW //mBowVec对应的BowVector的定义是std::map<WordId, WordValue> DBoW2::BowVector mBowVec; DBoW2::FeatureVector mFeatVec; // Pose relative to parent (this is computed when bad flag is activated) cv::Mat mTcp; // Scale const int mnScaleLevels; const float mfScaleFactor; const float mfLogScaleFactor; const std::vector<float> mvScaleFactors; const std::vector<float> mvLevelSigma2; const std::vector<float> mvInvLevelSigma2; // Image bounds and calibration const int mnMinX; const int mnMinY; const int mnMaxX; const int mnMaxY; const cv::Mat mK; // The following variables need to be accessed trough a mutex to be thread safe. protected: // SE3 Pose and camera center //设置KeyFrame中成员变量mTcw,mTwc,Ow(左目相机的中心坐标，以左为准) cv::Mat Tcw; cv::Mat Twc; cv::Mat Ow; //在双目相机中baseline中点坐标 cv::Mat Cw; // Stereo middel point. Only for visualization // MapPoints associated to keypoints std::vector<MapPoint*> mvpMapPoints; // BoW KeyFrameDatabase* mpKeyFrameDB; ORBVocabulary* mpORBvocabulary; // Grid over the image to speed up feature matching std::vector< std::vector <std::vector<size_t> > > mGrid; std::map<KeyFrame*,int> mConnectedKeyFrameWeights; std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames; std::vector<int> mvOrderedWeights; // Spanning Tree and Loop Edges bool mbFirstConnection; KeyFrame* mpParent; std::set<KeyFrame*> mspChildrens; std::set<KeyFrame*> mspLoopEdges; // Bad flags bool mbNotErase; bool mbToBeErased; bool mbBad; float mHalfBaseline; // Only for visualization Map* mpMap; //由于KeyFrame中一部分数据会被多个线程访问修改，因此需要在这些成员中加线程锁， //保证同一时间只有一个线程有访问权。涉及线程安全的如下： //关键帧位姿的设置 std::mutex mMutexPose; //关键帧间连接关系的设置 std::mutex mMutexConnections; //关键帧对应地图点的操作，包括地图点计算相连关键帧之间的权重 std::mutex mMutexFeatures; }; } //namespace ORB_SLAM #endif // KEYFRAME_H#include "KeyFrame.h" #include "Converter.h" #include "ORBmatcher.h" #include<mutex> namespace ORB_SLAM2 { long unsigned int KeyFrame::nNextId=0; //mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL)); mvpMapPoints在Frame.cpp的定义是这样的 //其中有N个空指针，因此有的位置上的MapPoint并没有指向实际的地图点(虽然有对应的idx，但是是要进行判断的) KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB): mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS), mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv), mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy), mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn), mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()), mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor), mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2), mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX), mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB), mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap) { mnId=nNextId++; mGrid.resize(mnGridCols); for(int i=0; i<mnGridCols;i++) { mGrid[i].resize(mnGridRows); for(int j=0; j<mnGridRows; j++) mGrid[i][j] = F.mGrid[i][j]; } SetPose(F.mTcw); } void KeyFrame::ComputeBoW() { if(mBowVec.empty() || mFeatVec.empty()) { vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors); // Feature vector associate features with nodes in the 4th level (from leaves up) // We assume the vocabulary tree has 6 levels, change the 4 otherwise mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4); } } //Tcw直接求逆计算量比较大？从博客上看到的解释。。。实时系统为了加速也是很拼啊 //对于现在的计算机，4*4的矩阵求逆的计算量应该不是很大的吧 //Ow是对应Twc的twc部分，Ow就对应Tcw^-1中的平移向量-R^T*t void KeyFrame::SetPose(const cv::Mat &Tcw_) { unique_lock<mutex> lock(mMutexPose); Tcw_.copyTo(Tcw); cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3); cv::Mat tcw = Tcw.rowRange(0,3).col(3); cv::Mat Rwc = Rcw.t(); Ow = -Rwc*tcw; Twc = cv::Mat::eye(4,4,Tcw.type()); Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3)); Ow.copyTo(Twc.rowRange(0,3).col(3)); cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1); Cw = Twc*center; } //得到位姿 cv::Mat KeyFrame::GetPose() { unique_lock<mutex> lock(mMutexPose); return Tcw.clone(); } //得到位姿的逆 cv::Mat KeyFrame::GetPoseInverse() { unique_lock<mutex> lock(mMutexPose); return Twc.clone(); } //这里的Ow其实是世界坐标系(第一帧)原点(相机光心)在当前帧参考系(相机坐标系)中的坐标 //等价于twc,运行ORB界面上有个Follow Camera选项，选上后，相机在界面中的位置固定， //这是就需要这个Ow来计算第一帧的坐标，而不能错误地理解为当前相机在世界参考系下的坐标 cv::Mat KeyFrame::GetCameraCenter() { unique_lock<mutex> lock(mMutexPose); return Ow.clone(); } cv::Mat KeyFrame::GetStereoCenter() { unique_lock<mutex> lock(mMutexPose); return Cw.clone(); } //获得旋转矩阵 cv::Mat KeyFrame::GetRotation() { unique_lock<mutex> lock(mMutexPose); return Tcw.rowRange(0,3).colRange(0,3).clone(); } //获得平移 cv::Mat KeyFrame::GetTranslation() { unique_lock<mutex> lock(mMutexPose); return Tcw.rowRange(0,3).col(3).clone(); } //增加连接，如果该关键帧不存在，增加该关键帧和其对应的权重 //如果该关键帧已经存在，但权重不符，修改权重 //如果都符合，直接返回不用继续添加连接 void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) { { unique_lock<mutex> lock(mMutexConnections); if(!mConnectedKeyFrameWeights.count(pKF)) mConnectedKeyFrameWeights[pKF]=weight; else if(mConnectedKeyFrameWeights[pKF]!=weight) mConnectedKeyFrameWeights[pKF]=weight; else return; } UpdateBestCovisibles(); } //每一个关键帧都会维护自己的map，其中记录了与其他关键帧之间的weight //每次为当前关键帧添加新的连接关键帧后，都需要根据weight对map结构重新排序 void KeyFrame::UpdateBestCovisibles() { unique_lock<mutex> lock(mMutexConnections); vector<pair<int,KeyFrame*> > vPairs; vPairs.reserve(mConnectedKeyFrameWeights.size()); //由于map结构中没有sort函数，需要将元素取出放入一个pair组成的vector中，排序后放入vPairs for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) vPairs.push_back(make_pair(mit->second,mit->first)); //进行排序，默认按升序 sort(vPairs.begin(),vPairs.end()); list<KeyFrame*> lKFs; list<int> lWs; for(size_t i=0, iend=vPairs.size(); i<iend;i++) { //所以定义的链表中权重由大到小排列要用push_front lKFs.push_front(vPairs[i].second);//构造关键帧链表 lWs.push_front(vPairs[i].first);//构造权重链表 } //更新排序好的连接关键帧及其对应的权重 mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); } //返回相邻关键帧，定义成set类型，无顺序 set<KeyFrame*> KeyFrame::GetConnectedKeyFrames() { unique_lock<mutex> lock(mMutexConnections); set<KeyFrame*> s; for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++) s.insert(mit->first); return s; } //返回相邻关键帧，看之前的定义就可知道是有顺序的，按照权重递减排列 vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames() { unique_lock<mutex> lock(mMutexConnections); return mvpOrderedConnectedKeyFrames; } //获取前N个与当前关键帧共视关系最好的关键帧，如果小于N个，则返回全部当前相邻关键帧 vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N) { unique_lock<mutex> lock(mMutexConnections); if((int)mvpOrderedConnectedKeyFrames.size()<N) return mvpOrderedConnectedKeyFrames; else return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N); } //设定权重的阈值为w，选取阈值大于等于w的关键帧 vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w) { unique_lock<mutex> lock(mMutexConnections); if(mvpOrderedConnectedKeyFrames.empty()) return vector<KeyFrame*>(); vector<int>::iterator it = upper_bound(mvOrderedWeights.begin(),mvOrderedWeights.end(),w,KeyFrame::weightComp); if(it==mvOrderedWeights.end()) return vector<KeyFrame*>(); else { int n = it-mvOrderedWeights.begin(); return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n); } } //返回指定帧和当前帧之间的权重 int KeyFrame::GetWeight(KeyFrame *pKF) { unique_lock<mutex> lock(mMutexConnections); if(mConnectedKeyFrameWeights.count(pKF)) return mConnectedKeyFrameWeights[pKF]; else return 0; } //当前帧对应的地图点的指针均存放在mvpMapPoints(mvp代表：member、vector、pointer)向量中 //通过对mvpMapPoints操作封装，可以得到以下的操作： //增加地图点 void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx) { unique_lock<mutex> lock(mMutexFeatures); mvpMapPoints[idx]=pMP; } //删除对应关键帧中的附图点以及在地图中该地图点的内存 void KeyFrame::EraseMapPointMatch(const size_t &idx) { unique_lock<mutex> lock(mMutexFeatures); mvpMapPoints[idx]=static_cast<MapPoint*>(NULL); } //删除在关键帧中对应的地图点的index void KeyFrame::EraseMapPointMatch(MapPoint* pMP) { int idx = pMP->GetIndexInKeyFrame(this); if(idx>=0) mvpMapPoints[idx]=static_cast<MapPoint*>(NULL); } //替换对应idx的地图点 void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP) { mvpMapPoints[idx]=pMP; } //得到当前关键帧中的地图点 set<MapPoint*> KeyFrame::GetMapPoints() { unique_lock<mutex> lock(mMutexFeatures); set<MapPoint*> s; for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++) { //判断vector中对应idx的地图点是否存在 if(!mvpMapPoints[i]) continue; //存在，取出，判断是否为坏点，不是，则存入set中 MapPoint* pMP = mvpMapPoints[i]; if(!pMP->isBad()) s.insert(pMP); } return s; } //返回高质量MapPoints(感觉也就是设定了一个阈值)的数量 //这个阈值设定是该地图点最少被多少相机观测到 int KeyFrame::TrackedMapPoints(const int &minObs) { unique_lock<mutex> lock(mMutexFeatures); int nPoints=0; const bool bCheckObs = minObs>0; for(int i=0; i<N; i++) { MapPoint* pMP = mvpMapPoints[i]; if(pMP) { if(!pMP->isBad()) { if(bCheckObs) { if(mvpMapPoints[i]->Observations()>=minObs) nPoints++; } else nPoints++; } } } return nPoints; } //获得对应地图点的匹配关系 vector<MapPoint*> KeyFrame::GetMapPointMatches() { unique_lock<mutex> lock(mMutexFeatures); return mvpMapPoints; } //获得指定的地图点 MapPoint* KeyFrame::GetMapPoint(const size_t &idx) { unique_lock<mutex> lock(mMutexFeatures); return mvpMapPoints[idx]; } //为关键帧之间添加连接，通过关键帧之间的weight连接，wight指的是两个关键帧之间共同观测到的地图点 void KeyFrame::UpdateConnections() { map<KeyFrame*,int> KFcounter; vector<MapPoint*> vpMP; { unique_lock<mutex> lockMPs(mMutexFeatures); vpMP = mvpMapPoints; } //For all map points in keyframe check in which other keyframes are they seen //Increase counter for those keyframes for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++) { MapPoint* pMP = *vit; if(!pMP) continue; if(pMP->isBad()) continue; map<KeyFrame*,size_t> observations = pMP->GetObservations(); for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { if(mit->first->mnId==mnId) continue; KFcounter[mit->first]++; } } // This should not happen if(KFcounter.empty()) return; //If the counter is greater than threshold add connection //In case no keyframe counter is over threshold add the one with maximum counter int nmax=0; KeyFrame* pKFmax=NULL; int th = 15; vector<pair<int,KeyFrame*> > vPairs; vPairs.reserve(KFcounter.size()); for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { if(mit->second>nmax) { nmax=mit->second; pKFmax=mit->first; } if(mit->second>=th) { vPairs.push_back(make_pair(mit->second,mit->first)); (mit->first)->AddConnection(this,mit->second); } } if(vPairs.empty()) { vPairs.push_back(make_pair(nmax,pKFmax)); pKFmax->AddConnection(this,nmax); } sort(vPairs.begin(),vPairs.end()); list<KeyFrame*> lKFs; list<int> lWs; for(size_t i=0; i<vPairs.size();i++) { lKFs.push_front(vPairs[i].second); lWs.push_front(vPairs[i].first); } { unique_lock<mutex> lockCon(mMutexConnections); // mspConnectedKeyFrames = spConnectedKeyFrames; mConnectedKeyFrameWeights = KFcounter; mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); if(mbFirstConnection && mnId!=0) { mpParent = mvpOrderedConnectedKeyFrames.front(); mpParent->AddChild(this); mbFirstConnection = false; } } } //增加孩子节点 void KeyFrame::AddChild(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); mspChildrens.insert(pKF); } //删除孩子节点 void KeyFrame::EraseChild(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); mspChildrens.erase(pKF); } //修改父亲节点 void KeyFrame::ChangeParent(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); mpParent = pKF; pKF->AddChild(this); } //获得孩子节点 set<KeyFrame*> KeyFrame::GetChilds() { unique_lock<mutex> lockCon(mMutexConnections); return mspChildrens; } //得到父亲节点 KeyFrame* KeyFrame::GetParent() { unique_lock<mutex> lockCon(mMutexConnections); return mpParent; } //判断是否有孩子节点 bool KeyFrame::hasChild(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); return mspChildrens.count(pKF); } //增加LoopEdges void KeyFrame::AddLoopEdge(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); mbNotErase = true; mspLoopEdges.insert(pKF); } //获得LoopEdges set<KeyFrame*> KeyFrame::GetLoopEdges() { unique_lock<mutex> lockCon(mMutexConnections); return mspLoopEdges; } //设置不被删除的标志 void KeyFrame::SetNotErase() { unique_lock<mutex> lock(mMutexConnections); mbNotErase = true; } //设置可删除 void KeyFrame::SetErase() { { unique_lock<mutex> lock(mMutexConnections); //删除关键帧是，首先要检测mspLoopEdges是否是空的 //因为如果当前帧维护了一个回环，删了这个关键帧回环没有了 //所以通常情况系下是空的，就可以吧mbNotErase设置成false //也就是说当前帧是可删除的 if(mspLoopEdges.empty()) { mbNotErase = false; } } if(mbToBeErased) { SetBadFlag(); } } //设置坏的标志 //KeyFrame中比较难理解的是setFlag()函数，真实删除当前关键帧之前， //需要处理好父亲和儿子关键帧关系，不然会造成整个关键帧维护图的断裂 //或者混乱，不能够为后端提供较好的初值 //理解起来就是当前帧(父亲)挂了，儿子节点需要找新的父亲节点， //在候选的父亲节点中，当前帧的父亲节点（父亲的父亲）肯定在候选节点中 //说先设置成坏帧，且不是回环帧，则可以删除当前帧，如果是回环帧，那么则无法删除该帧 void KeyFrame::SetBadFlag() { { unique_lock<mutex> lock(mMutexConnections); if(mnId==0) return; //初始mbNotErase状态是true，调用SetBadFlag后，将mbNotErase状态设置为true， //然后return，并没有执行后续的操作 else if(mbNotErase) { mbToBeErased = true; return; } } for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++) mit->first->EraseConnection(this); for(size_t i=0; i<mvpMapPoints.size(); i++) if(mvpMapPoints[i]) mvpMapPoints[i]->EraseObservation(this); { unique_lock<mutex> lock(mMutexConnections); unique_lock<mutex> lock1(mMutexFeatures); mConnectedKeyFrameWeights.clear(); mvpOrderedConnectedKeyFrames.clear(); // Update Spanning Tree set<KeyFrame*> sParentCandidates; //首先将当前帧的父亲放入候选父亲中 sParentCandidates.insert(mpParent); // Assign at each iteration one children with a parent (the pair with highest covisibility weight) // Include that children as new parent candidate for the rest //遍历当前帧的所有儿子 while(!mspChildrens.empty()) { bool bContinue = false; int max = -1; KeyFrame* pC; KeyFrame* pP; for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++) { KeyFrame* pKF = *sit; if(pKF->isBad()) continue; // Check if a parent candidate is connected to the keyframe //遍历每个儿子的每个共视帧 vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames(); for(size_t i=0, iend=vpConnected.size(); i<iend; i++) { //如果共视帧有候选父亲，则将该共视帧的候选父亲 更新为该候选父亲 for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++) { if(vpConnected[i]->mnId == (*spcit)->mnId) { int w = pKF->GetWeight(vpConnected[i]); if(w>max) { pC = pKF; pP = vpConnected[i]; max = w; bContinue = true; } } } } } //如果找到，则修改该儿子节点的父节点为所有共视帧父亲节点权重值最高的为父亲节点 if(bContinue) { pC->ChangeParent(pP);//认别的帧做父亲节点了 sParentCandidates.insert(pC);//父亲节点的候选节点中加入该节点 mspChildrens.erase(pC);//该子节点也就和当前帧没有关系了 } //如果没有，break else break; } // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF //如果当前子节点没有任何候选父节点，分配初始的父节点 //不存在共视关系可能是因为(速度太快，旋转太急，匹配跟丢) //直接分配当前帧的父节点(也就是爷爷辈的节点来管) if(!mspChildrens.empty()) for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++) { (*sit)->ChangeParent(mpParent); } mpParent->EraseChild(this); mTcp = Tcw*mpParent->GetPoseInverse(); mbBad = true; } mpMap->EraseKeyFrame(this); mpKeyFrameDB->erase(this); } //判断是否为Bad bool KeyFrame::isBad() { unique_lock<mutex> lock(mMutexConnections); return mbBad; } //擦除连接 void KeyFrame::EraseConnection(KeyFrame* pKF) { bool bUpdate = false; { unique_lock<mutex> lock(mMutexConnections); if(mConnectedKeyFrameWeights.count(pKF)) { mConnectedKeyFrameWeights.erase(pKF); bUpdate=true; } } if(bUpdate) UpdateBestCovisibles(); } //在指定的区域内获取特征 vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const { vector<size_t> vIndices; vIndices.reserve(N); const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); if(nMinCellX>=mnGridCols) return vIndices; const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv)); if(nMaxCellX<0) return vIndices; const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv)); if(nMinCellY>=mnGridRows) return vIndices; const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv)); if(nMaxCellY<0) return vIndices; for(int ix = nMinCellX; ix<=nMaxCellX; ix++) { for(int iy = nMinCellY; iy<=nMaxCellY; iy++) { const vector<size_t> vCell = mGrid[ix][iy]; for(size_t j=0, jend=vCell.size(); j<jend; j++) { const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]]; const float distx = kpUn.pt.x-x; const float disty = kpUn.pt.y-y; if(fabs(distx)<r && fabs(disty)<r) vIndices.push_back(vCell[j]); } } } return vIndices; } //判断点是否在图像中 bool KeyFrame::IsInImage(const float &x, const float &y) const { return (x>=mnMinX && x<mnMaxX && y>=mnMinY && y<mnMaxY); } //反投影到空间中，并返回T cv::Mat KeyFrame::UnprojectStereo(int i) { const float z = mvDepth[i]; if(z>0) { const float u = mvKeys[i].pt.x; const float v = mvKeys[i].pt.y; const float x = (u-cx)*z*invfx; const float y = (v-cy)*z*invfy; cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z); unique_lock<mutex> lock(mMutexPose); return Twc.rowRange(0,3).colRange(0,3)*x3Dc+Twc.rowRange(0,3).col(3); } else return cv::Mat(); } //计算场景中中间的深度 float KeyFrame::ComputeSceneMedianDepth(const int q) { vector<MapPoint*> vpMapPoints; cv::Mat Tcw_; { unique_lock<mutex> lock(mMutexFeatures); unique_lock<mutex> lock2(mMutexPose); vpMapPoints = mvpMapPoints; Tcw_ = Tcw.clone(); } vector<float> vDepths; vDepths.reserve(N); cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3); Rcw2 = Rcw2.t(); float zcw = Tcw_.at<float>(2,3); for(int i=0; i<N; i++) { if(mvpMapPoints[i]) { MapPoint* pMP = mvpMapPoints[i]; cv::Mat x3Dw = pMP->GetWorldPos(); float z = Rcw2.dot(x3Dw)+zcw; vDepths.push_back(z); } } sort(vDepths.begin(),vDepths.end()); return vDepths[(vDepths.size()-1)/q]; } } //namespace ORB_SLAM自己鼓励自己下，恭喜你，又看完了一小段ORB-SLAM2的代码O(∩_∩)O哈哈~
