void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints
list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
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((nCurrentKFid-pMP->mnFirstKFid)>=2 && pMP->Observations()<=2)
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if((nCurrentKFid-pMP->mnFirstKFid)>=3)
lit = mlpRecentAddedMapPoints.erase(lit);
else
lit++;
}
}
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 LocalMapping::ProcessNewKeyFrame()
{
{
boost::mutex::scoped_lock lock(mMutexNewKFs);
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();
}
// Compute Bags of Words structures
mpCurrentKeyFrame->ComputeBoW();
if(mpCurrentKeyFrame->mnId==0)
return;
// Associate MapPoints to the new keyframe and update normal and descriptor
vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
if(mpCurrentKeyFrame->mnId>1) //This operations are already done in the tracking for the first two keyframes
{
for(size_t i=0; i<vpMapPointMatches.size(); i++)
{
MapPoint* pMP = vpMapPointMatches[i];
if(pMP)
{
if(!pMP->isBad())
{
pMP->AddObservation(mpCurrentKeyFrame, i);
pMP->UpdateNormalAndDepth();
pMP->ComputeDistinctiveDescriptors();
}
}
}
}
if(mpCurrentKeyFrame->mnId==1)
{
for(size_t i=0; i<vpMapPointMatches.size(); i++)
{
MapPoint* pMP = vpMapPointMatches[i];
if(pMP)
{
mlpRecentAddedMapPoints.push_back(pMP);
}
}
}
// Update links in the Covisibility Graph
mpCurrentKeyFrame->UpdateConnections();
// Insert Keyframe in Map
mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
set<MapPoint*> KeyFrame::GetMapPoints()
{
boost::mutex::scoped_lock lock(mMutexFeatures);
set<MapPoint*> s;
for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
{
if(!mvpMapPoints[i])
continue;
MapPoint* pMP = mvpMapPoints[i];
if(!pMP->isBad())
s.insert(pMP);
}
return s;
}
void LoopClosing::CorrectLoop()
{
// Send a stop signal to Local Mapping
// Avoid new keyframes are inserted while correcting the loop
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
//ros::Rate r(1e4);
//while(ros::ok() && !mpLocalMapper->isStopped())
while(!mpLocalMapper->isStopped())
{
//r.sleep();
boost::this_thread::sleep(boost::posix_time::milliseconds(10000));
}
// Ensure current keyframe is updated
mpCurrentKF->UpdateConnections();
// Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);
KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
CorrectedSim3[mpCurrentKF]=mg2oScw;
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
cv::Mat Tiw = pKFi->GetPose();
if(pKFi!=mpCurrentKF)
{
cv::Mat Tic = Tiw*Twc;
cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
cv::Mat tic = Tic.rowRange(0,3).col(3);
g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
//Pose corrected with the Sim3 of the loop closure
CorrectedSim3[pKFi]=g2oCorrectedSiw;
}
cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
cv::Mat tiw = Tiw.rowRange(0,3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
//Pose without correction
NonCorrectedSim3[pKFi]=g2oSiw;
}
// Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
g2o::Sim3 g2oCorrectedSiw = mit->second;
g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
{
MapPoint* pMPi = vpMPsi[iMP];
if(!pMPi)
continue;
if(pMPi->isBad())
continue;
if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId)
continue;
// Project with non-corrected pose and project back with corrected pose
cv::Mat P3Dw = pMPi->GetWorldPos();
Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
pMPi->mnCorrectedReference = pKFi->mnId;
pMPi->UpdateNormalAndDepth();
}
// Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
double s = g2oCorrectedSiw.scale();
eigt *=(1./s); //[R t/s;0 1]
cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
pKFi->SetPose(correctedTiw);
// Make sure connections are updated
pKFi->UpdateConnections();
}
// Start Loop Fusion
// Update matched map points and replace if duplicated
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
{
MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
if(pCurMP)
pCurMP->Replace(pLoopMP);
else
{
mpCurrentKF->AddMapPoint(pLoopMP,i);
pLoopMP->AddObservation(mpCurrentKF,i);
pLoopMP->ComputeDistinctiveDescriptors();
}
}
}
// Project MapPoints observed in the neighborhood of the loop keyframe
// into the current keyframe and neighbors using corrected poses.
// Fuse duplications.
SearchAndFuse(CorrectedSim3);
// After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
map<KeyFrame*, set<KeyFrame*> > LoopConnections;
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
// Update connections. Detect new links.
pKFi->UpdateConnections();
LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
{
LoopConnections[pKFi].erase(*vit_prev);
}
for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
{
LoopConnections[pKFi].erase(*vit2);
}
}
mpTracker->ForceRelocalisation();
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, mg2oScw, NonCorrectedSim3, CorrectedSim3, LoopConnections);
//Add edge
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
std::cout << "Loop Closed!" << std::endl;
// Loop closed. Release Local Mapping.
mpLocalMapper->Release();
mpMap->SetFlagAfterBA();
mLastLoopKFid = mpCurrentKF->mnId;
}
bool LoopClosing::ComputeSim3()
{
// For each consistent loop candidate we try to compute a Sim3
const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
// We compute first ORB matches for each candidate
// If enough matches are found, we setup a Sim3Solver
ORBmatcher matcher(0.75,true);
vector<Sim3Solver*> vpSim3Solvers;
vpSim3Solvers.resize(nInitialCandidates);
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nInitialCandidates);
vector<bool> vbDiscarded;
vbDiscarded.resize(nInitialCandidates);
int nCandidates=0; //candidates with enough matches
for(int i=0; i<nInitialCandidates; i++)
{
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// avoid that local mapping erase it while it is being processed in this thread
pKF->SetNotErase();
if(pKF->isBad())
{
vbDiscarded[i] = true;
continue;
}
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
if(nmatches<20)
{
vbDiscarded[i] = true;
continue;
}
else
{
Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,20,300);
vpSim3Solvers[i] = pSolver;
}
nCandidates++;
}
bool bMatch = false;
// Perform alternatively RANSAC iterations for each candidate
// until one is succesful or all fail
while(nCandidates>0 && !bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
{
if(vbDiscarded[i])
continue;
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// Perform 5 Ransac Iterations
vector<bool> vbInliers;
int nInliers;
bool bNoMore;
Sim3Solver* pSolver = vpSim3Solvers[i];
cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
if(!Scm.empty())
{
vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
if(vbInliers[j])
vpMapPointMatches[j]=vvpMapPointMatches[i][j];
}
cv::Mat R = pSolver->GetEstimatedRotation();
cv::Mat t = pSolver->GetEstimatedTranslation();
const float s = pSolver->GetEstimatedScale();
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10);
// If optimization is succesful stop ransacs and continue
if(nInliers>=20)
{
bMatch = true;
mpMatchedKF = pKF;
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
mg2oScw = gScm*gSmw;
mScw = Converter::toCvMat(mg2oScw);
mvpCurrentMatchedPoints = vpMapPointMatches;
break;
}
}
}
}
if(!bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
// Retrieve MapPoints seen in Loop Keyframe and neighbors
vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
vpLoopConnectedKFs.push_back(mpMatchedKF);
mvpLoopMapPoints.clear();
for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
{
KeyFrame* pKF = *vit;
vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
{
mvpLoopMapPoints.push_back(pMP);
pMP->mnLoopPointForKF=mpCurrentKF->mnId;
}
}
}
}
// Find more matches projecting with the computed Sim3
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
// If enough matches accept Loop
int nTotalMatches = 0;
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
nTotalMatches++;
}
if(nTotalMatches>=40)
{
for(int i=0; i<nInitialCandidates; i++)
if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
mvpEnoughConsistentCandidates[i]->SetErase();
return true;
}
else
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
}
void LocalMapping::SearchInNeighbors()
{
// Retrieve neighbor keyframes
vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(20);
vector<KeyFrame*> vpTargetKFs;
for(vector<KeyFrame*>::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
vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
for(vector<KeyFrame*>::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(0.6);
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();
}
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;
}
}
}
