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) 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; vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches(); 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()) { nMPs++; if(pMP->Observations()>3) { int scaleLevel = pKF->GetKeyPointUn(i).octave; map<KeyFrame*, size_t> observations = pMP->GetObservations(); int nObs=0; for(map<KeyFrame*, size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { KeyFrame* pKFi = mit->first; if(pKFi==pKF) continue; int scaleLeveli = pKFi->GetKeyPointUn(mit->second).octave; if(scaleLeveli<=scaleLevel+1) { nObs++; if(nObs>=3) break; } } if(nObs>=3) { nRedundantObservations++; } } } } } if(nRedundantObservations>0.9*nMPs) pKF->SetBadFlag(); } }
void KeyFrame::UpdateConnections() { map<KeyFrame*,int> KFcounter; vector<MapPoint*> vpMP; { boost::mutex::scoped_lock 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]++; } } 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); } { boost::mutex::scoped_lock 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; } } }