2021SC@SDUSC
LoopClosing分析（3）
接着来分析代码很多的LoopClosing——因为有篇幅较长的函数，我们依然把分析注释直接写在代码里了
- 首先是DetectCommonRegionsFromBoW函数，这个函数通过根据bow相似度来提取候选闭环帧，然后进行匹配、优化一系列操作，判断是否闭环。过程有些复杂↓
bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
int nBoWMatches = 20;
int nBoWInliers = 15;
int nSim3Inliers = 20;
int nProjMatches = 50;
int nProjOptMatches = 80;
// 当前帧共视关键帧集合
set<KeyFrame*> spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames();
int nNumCovisibles = 5;
ORBmatcher matcherBoW(0.9, true);
ORBmatcher matcher(0.75, true);
int nNumGuidedMatching = 0;
// Varibles to select the best numbe
KeyFrame* pBestMatchedKF;
int nBestMatchesReproj = 0;
int nBestNumCoindicendes = 0;
g2o::Sim3 g2oBestScw;
std::vector<MapPoint*> vpBestMapPoints;
std::vector<MapPoint*> vpBestMatchedMapPoints;
int numCandidates = vpBowCand.size();
vector<int> vnStage(numCandidates, 0);
vector<int> vnMatchesStage(numCandidates, 0);
int index = 0;
// 候选闭环关键帧，来自相似bow
for(KeyFrame* pKFi : vpBowCand)
{
//cout << endl << "-------------------------------" << endl;
if(!pKFi || pKFi->isBad())
continue;
// Current KF against KF with covisibles version
// 候选闭环帧的共视帧集合
std::vector<KeyFrame*> vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
vpCovKFi.push_back(vpCovKFi[0]);
vpCovKFi[0] = pKFi;
std::vector<std::vector<MapPoint*> > vvpMatchedMPs;
vvpMatchedMPs.resize(vpCovKFi.size());
std::set<MapPoint*> spMatchedMPi;
int numBoWMatches = 0;
KeyFrame* pMostBoWMatchesKF = pKFi;
int nMostBoWNumMatches = 0;
std::vector<MapPoint*> vpMatchedPoints = std::vector<MapPoint*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
std::vector<KeyFrame*> vpKeyFrameMatchedMP = std::vector<KeyFrame*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));
int nIndexMostBoWMatchesKF=0;
// 遍历候选闭环帧的共视帧集合
for(int j=0; j<vpCovKFi.size(); ++j)
{
if(!vpCovKFi[j] || vpCovKFi[j]->isBad())
continue;
/**
* 3d,2d都已知，计算匹配关系；通过bow，对两个关键帧的点进行匹配，用于闭环检测
* 1、两个关键帧的特征点都划分到了词袋树中不同节点中去了
* 2、遍历节点集合，相同的节点才计算匹配点
* 3、在同一节点中，遍历两个关键帧的特征点，计算描述子距离
* 4、描述子距离小于阈值，且次佳与最佳有一定差距，认为匹配上了
* 5、根据特征点angle差值构造的直方图，删除非前三的离群匹配点
* 6、vpMatches12保存数据，当pKF1的特征点 - pKF2的MP
*/
int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]);
//cout << "BoW: " << num << " putative matches with KF " << vpCovKFi[j]->mnId << endl;
// 记录最佳匹配
if (num > nMostBoWNumMatches)
{
nMostBoWNumMatches = num;
nIndexMostBoWMatchesKF = j;
}
}
bool bAbortByNearKF = false;
// 遍历候选闭环帧的共视帧集合
for(int j=0; j<vpCovKFi.size(); ++j)
{
// 在当前帧的共视帧集合中，不是闭环
if(spConnectedKeyFrames.find(vpCovKFi[j]) != spConnectedKeyFrames.end())
{
bAbortByNearKF = true;
//cout << "BoW: Candidate KF aborted by proximity" << endl;
break;
}
//cout << "Matches: " << num << endl;
// 遍历匹配点
for(int k=0; k < vvpMatchedMPs[j].size(); ++k)
{
MapPoint* pMPi_j = vvpMatchedMPs[j][k];
if(!pMPi_j || pMPi_j->isBad())
continue;
if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end())
{
spMatchedMPi.insert(pMPi_j);
numBoWMatches++;
vpMatchedPoints[k]= pMPi_j;
vpKeyFrameMatchedMP[k] = vpCovKFi[j];
}
}
}
//cout <<"BoW: " << numBoWMatches << " independent putative matches" << endl;
// 匹配点数大于阈值
if(!bAbortByNearKF && numBoWMatches >= nBoWMatches) // TODO pick a good threshold
{
// Geometric validation
bool bFixedScale = mbFixScale;
if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
// 已知匹配点，两帧优化
Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP);
solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers
bool bNoMore = false;
vector<bool> vbInliers;
int nInliers;
bool bConverge = false;
cv::Mat mTcm;
while(!bConverge && !bNoMore)
{
mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge);
}
//cout << "Num inliers: " << nInliers << endl;
if(bConverge)
{
// Match by reprojection
//int nNumCovisibles = 5;
vpCovKFi.clear();
vpCovKFi = pMostBoWMatchesKF->GetBestCovisibilityKeyFrames(nNumCovisibles);
int nInitialCov = vpCovKFi.size();
vpCovKFi.push_back(pMostBoWMatchesKF);
set<KeyFrame*> spCheckKFs(vpCovKFi.begin(), vpCovKFi.end());
set<MapPoint*> spMapPoints;
vector<MapPoint*> vpMapPoints;
vector<KeyFrame*> vpKeyFrames;
// 遍历匹配闭环帧的共视帧集合
for(KeyFrame* pCovKFi : vpCovKFi)
{
for(MapPoint* pCovMPij : pCovKFi->GetMapPointMatches())
{
if(!pCovMPij || pCovMPij->isBad())
continue;
if(spMapPoints.find(pCovMPij) == spMapPoints.end())
{
spMapPoints.insert(pCovMPij);
vpMapPoints.push_back(pCovMPij);
vpKeyFrames.push_back(pCovKFi);
}
}
}
//cout << "Point cloud: " << vpMapPoints.size() << endl;
g2o::Sim3 gScm(Converter::toMatrix3d(solver.GetEstimatedRotation()),Converter::toVector3d(solver.GetEstimatedTranslation()),solver.GetEstimatedScale());
g2o::Sim3 gSmw(Converter::toMatrix3d(pMostBoWMatchesKF->GetRotation()),Converter::toVector3d(pMostBoWMatchesKF->GetTranslation()),1.0);
g2o::Sim3 gScw = gScm*gSmw; // Similarity matrix of current from the world position
cv::Mat mScw = Converter::toCvMat(gScw);
vector<MapPoint*> vpMatchedMP;
vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
vector<KeyFrame*> vpMatchedKF;
vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));
// 3d-2d，闭环关键帧与其共视关键帧的MP，投影到当前关键帧中，计算描述子距离，寻找当前帧匹配特征点
int numProjMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP, vpMatchedKF, 8, 1.5);
cout <<"BoW: " << numProjMatches << " matches between " << vpMapPoints.size() << " points with coarse Sim3" << endl;
if(numProjMatches >= nProjMatches)
{
// Optimize Sim3 transformation with every matches
Eigen::Matrix<double, 7, 7> mHessian7x7;
bool bFixedScale = mbFixScale;
if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pKFi, vpMatchedMP, gScm, 10, mbFixScale, mHessian7x7, true);
if(numOptMatches >= nSim3Inliers)
{
//cout <<"BoW: " << numOptMatches << " inliers in Sim3 optimization" << endl;
g2o::Sim3 gSmw(Converter::toMatrix3d(pMostBoWMatchesKF->GetRotation()),Converter::toVector3d(pMostBoWMatchesKF->GetTranslation()),1.0);
g2o::Sim3 gScw = gScm*gSmw; // Similarity matrix of current from the world position
cv::Mat mScw = Converter::toCvMat(gScw);
vector<MapPoint*> vpMatchedMP;
vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
int numProjOptMatches = matcher.SearchByProjection(mpCurrentKF, mScw, vpMapPoints, vpMatchedMP, 5, 1.0);
//cout <<"BoW: " << numProjOptMatches << " matches after of the Sim3 optimization" << endl;
if(numProjOptMatches >= nProjOptMatches)
{
cout << "BoW: Current KF " << mpCurrentKF->mnId << "; candidate KF " << pKFi->mnId << endl;
cout << "BoW: There are " << numProjOptMatches << " matches between them with the optimized Sim3" << endl;
int max_x = -1, min_x = 1000000;
int max_y = -1, min_y = 1000000;
for(MapPoint* pMPi : vpMatchedMP)
{
if(!pMPi || pMPi->isBad())
{
continue;
}
tuple<size_t,size_t> indexes = pMPi->GetIndexInKeyFrame(pKFi);
int index = get<0>(indexes);
if(index >= 0)
{
int coord_x = pKFi->mvKeysUn[index].pt.x;
if(coord_x < min_x)
{
min_x = coord_x;
}
if(coord_x > max_x)
{
max_x = coord_x;
}
int coord_y = pKFi->mvKeysUn[index].pt.y;
if(coord_y < min_y)
{
min_y = coord_y;
}
if(coord_y > max_y)
{
max_y = coord_y;
}
}
}
//cout << "BoW: Coord in X -> " << min_x << ", " << max_x << "; and Y -> " << min_y << ", " << max_y << endl;
//cout << "BoW: features area in X -> " << (max_x - min_x) << " and Y -> " << (max_y - min_y) << endl;
int nNumKFs = 0;
//vpMatchedMPs = vpMatchedMP;
//vpMPs = vpMapPoints;
// Check the Sim3 transformation with the current KeyFrame covisibles
vector<KeyFrame*> vpCurrentCovKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(nNumCovisibles);
//cout << "---" << endl;
//cout << "BoW: Geometrical validation" << endl;
int j = 0;
while(nNumKFs < 3 && j<vpCurrentCovKFs.size())
//for(int j=0; j<vpCurrentCovKFs.size(); ++j)
{
KeyFrame* pKFj = vpCurrentCovKFs[j];
cv::Mat mTjc = pKFj->GetPose() * mpCurrentKF->GetPoseInverse();
g2o::Sim3 gSjc(Converter::toMatrix3d(mTjc.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTjc.rowRange(0, 3).col(3)),1.0);
g2o::Sim3 gSjw = gSjc * gScw;
int numProjMatches_j = 0;
vector<MapPoint*> vpMatchedMPs_j;
bool bValid = DetectCommonRegionsFromLastKF(pKFj,pMostBoWMatchesKF, gSjw,numProjMatches_j, vpMapPoints, vpMatchedMPs_j);
if(bValid)
{
//cout << "BoW: KF " << pKFj->mnId << " has " << numProjMatches_j << " matches" << endl;
cv::Mat Tc_w = mpCurrentKF->GetPose();
cv::Mat Tw_cj = pKFj->GetPoseInverse();
cv::Mat Tc_cj = Tc_w * Tw_cj;
cv::Vec3d vector_dist = Tc_cj.rowRange(0, 3).col(3);
cv::Mat Rc_cj = Tc_cj.rowRange(0, 3).colRange(0, 3);
double dist = cv::norm(vector_dist);
cout << "BoW: KF " << pKFi->mnId << " to KF " << pKFj->mnId << " is separated by " << dist << " meters" << endl;
cout << "BoW: Rotation between KF -> " << Rc_cj << endl;
vector<float> v_euler = Converter::toEuler(Rc_cj);
v_euler[0] *= 180 /3.1415;
v_euler[1] *= 180 /3.1415;
v_euler[2] *= 180 /3.1415;
cout << "BoW: Rotation in angles (x, y, z) -> (" << v_euler[0] << ", " << v_euler[1] << ", " << v_euler[2] << ")" << endl;
nNumKFs++;
}
j++;
}
if(nNumKFs < 3)
{
vnStage[index] = 8;
vnMatchesStage[index] = nNumKFs;
}
if(nBestMatchesReproj < numProjOptMatches)
{
nBestMatchesReproj = numProjOptMatches;
nBestNumCoindicendes = nNumKFs;
pBestMatchedKF = pMostBoWMatchesKF;
g2oBestScw = gScw;
vpBestMapPoints = vpMapPoints;
vpBestMatchedMapPoints = vpMatchedMP;
}
}
}
}
}
}
index++;
}
if(nBestMatchesReproj > 0)
{
pLastCurrentKF = mpCurrentKF;
nNumCoincidences = nBestNumCoindicendes;
pMatchedKF2 = pBestMatchedKF;
pMatchedKF2->SetNotErase();
g2oScw = g2oBestScw;
vpMPs = vpBestMapPoints;
vpMatchedMPs = vpBestMatchedMapPoints;
return nNumCoincidences >= 3;
}
else
{
int maxStage = -1;
int maxMatched;
for(int i=0; i<vnStage.size(); ++i)
{
if(vnStage[i] > maxStage)
{
maxStage = vnStage[i];
maxMatched = vnMatchesStage[i];
}
}
}
return false;
}
- DetectCommonRegionsFromLastKF函数相对很简洁，它会提取候选匹配帧的共视帧集合，以及二级共视帧集合，形成一个大的map。然后在当前帧中寻找2d匹配点，并返回匹配点数量，超过阈值则认为是闭环
bool LoopClosing::DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
set<MapPoint*> spAlreadyMatchedMPs(vpMatchedMPs.begin(), vpMatchedMPs.end());
nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
int nProjMatches = 30;
if(nNumProjMatches >= nProjMatches)
{
return true;
}
return false;
}
- 这篇博客最后来分析一个很长的函数MergeLocal↓
void LoopClosing::MergeLocal()
{
Verbose::PrintMess("MERGE: Merge Visual detected!!!!", Verbose::VERBOSITY_NORMAL);
int numTemporalKFs = 15;
//Relationship to rebuild the essential graph, it is used two times, first in the local window and later in the rest of the map
KeyFrame* pNewChild;
KeyFrame* pNewParent;
vector<KeyFrame*> vpLocalCurrentWindowKFs;
vector<KeyFrame*> vpMergeConnectedKFs;
// Flag that is true only when we stopped a running BA, in this case we need relaunch at the end of the merge
bool bRelaunchBA = false;
Verbose::PrintMess("MERGE: Check Full Bundle Adjustment", Verbose::VERBOSITY_DEBUG);
// If a Global Bundle Adjustment is running, abort it
if(isRunningGBA())
{
unique_lock<mutex> lock(mMutexGBA);
mbStopGBA = true;
mnFullBAIdx++;
if(mpThreadGBA)
{
mpThreadGBA->detach();
delete mpThreadGBA;
}
bRelaunchBA = true;
}
Verbose::PrintMess("MERGE: Request Stop Local Mapping", Verbose::VERBOSITY_DEBUG);
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
Verbose::PrintMess("MERGE: Local Map stopped", Verbose::VERBOSITY_DEBUG);
mpLocalMapper->EmptyQueue();
// Merge map will become in the new active map with the local window of KFs and MPs from the current map.
// Later, the elements of the current map will be transform to the new active map reference, in order to keep real time tracking
Map* pCurrentMap = mpCurrentKF->GetMap();
Map* pMergeMap = mpMergeMatchedKF->GetMap();
// Ensure current keyframe is updated
mpCurrentKF->UpdateConnections();
//Get the current KF and its neighbors(visual->covisibles; inertial->temporal+covisibles)
set<KeyFrame*> spLocalWindowKFs;
//Get MPs in the welding area from the current map
set<MapPoint*> spLocalWindowMPs;
if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization
{
KeyFrame* pKFi = mpCurrentKF;
int nInserted = 0;
while(pKFi && nInserted < numTemporalKFs)
{
spLocalWindowKFs.insert(pKFi);
pKFi = mpCurrentKF->mPrevKF;
nInserted++;
set<MapPoint*> spMPi = pKFi->GetMapPoints();
spLocalWindowMPs.insert(spMPi.begin(), spMPi.end());
}
pKFi = mpCurrentKF->mNextKF;
while(pKFi)
{
spLocalWindowKFs.insert(pKFi);
set<MapPoint*> spMPi = pKFi->GetMapPoints();
spLocalWindowMPs.insert(spMPi.begin(), spMPi.end());
}
}
else
{
spLocalWindowKFs.insert(mpCurrentKF);
}
vector<KeyFrame*> vpCovisibleKFs = mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
const int nMaxTries = 3;
int nNumTries = 0;
while(spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
{
vector<KeyFrame*> vpNewCovKFs;
vpNewCovKFs.empty();
for(KeyFrame* pKFi : spLocalWindowKFs)
{
vector<KeyFrame*> vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2);
for(KeyFrame* pKFcov : vpKFiCov)
{
if(pKFcov && !pKFcov->isBad() && spLocalWindowKFs.find(pKFcov) == spLocalWindowKFs.end())
{
vpNewCovKFs.push_back(pKFcov);
}
}
}
spLocalWindowKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end());
nNumTries++;
}
//TODO TEST
//vector<KeyFrame*> vpTestKFs = pCurrentMap->GetAllKeyFrames();
//spLocalWindowKFs.insert(vpTestKFs.begin(), vpTestKFs.end());
for(KeyFrame* pKFi : spLocalWindowKFs)
{
if(!pKFi || pKFi->isBad())
continue;
set<MapPoint*> spMPs = pKFi->GetMapPoints();
spLocalWindowMPs.insert(spMPs.begin(), spMPs.end());
}
set<KeyFrame*> spMergeConnectedKFs;
if(pCurrentMap->IsInertial() && pMergeMap->IsInertial()) //TODO Check the correct initialization
{
KeyFrame* pKFi = mpMergeMatchedKF;
int nInserted = 0;
while(pKFi && nInserted < numTemporalKFs)
{
spMergeConnectedKFs.insert(pKFi);
pKFi = mpCurrentKF->mPrevKF;
nInserted++;
}
pKFi = mpMergeMatchedKF->mNextKF;
while(pKFi)
{
spMergeConnectedKFs.insert(pKFi);
}
}
else
{
spMergeConnectedKFs.insert(mpMergeMatchedKF);
}
vpCovisibleKFs = mpMergeMatchedKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
spMergeConnectedKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
nNumTries = 0;
while(spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries)
{
vector<KeyFrame*> vpNewCovKFs;
for(KeyFrame* pKFi : spMergeConnectedKFs)
{
vector<KeyFrame*> vpKFiCov = pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs/2);
for(KeyFrame* pKFcov : vpKFiCov)
{
if(pKFcov && !pKFcov->isBad() && spMergeConnectedKFs.find(pKFcov) == spMergeConnectedKFs.end())
{
vpNewCovKFs.push_back(pKFcov);
}
}
}
spMergeConnectedKFs.insert(vpNewCovKFs.begin(), vpNewCovKFs.end());
nNumTries++;
}
set<MapPoint*> spMapPointMerge;
for(KeyFrame* pKFi : spMergeConnectedKFs)
{
set<MapPoint*> vpMPs = pKFi->GetMapPoints();
spMapPointMerge.insert(vpMPs.begin(),vpMPs.end());
}
vector<MapPoint*> vpCheckFuseMapPoint;
vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
cv::Mat twc = Twc.rowRange(0,3).col(3);
g2o::Sim3 g2oNonCorrectedSwc(Converter::toMatrix3d(Rwc),Converter::toVector3d(twc),1.0);
g2o::Sim3 g2oNonCorrectedScw = g2oNonCorrectedSwc.inverse();
g2o::Sim3 g2oCorrectedScw = mg2oMergeScw;
KeyFrameAndPose vCorrectedSim3, vNonCorrectedSim3;
vCorrectedSim3[mpCurrentKF]=g2oCorrectedScw;
vNonCorrectedSim3[mpCurrentKF]=g2oNonCorrectedScw;
for(KeyFrame* pKFi : spLocalWindowKFs)
{
if(!pKFi || pKFi->isBad())
{
continue;
}
g2o::Sim3 g2oCorrectedSiw;
if(pKFi!=mpCurrentKF)
{
cv::Mat Tiw = pKFi->GetPose();
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
vNonCorrectedSim3[pKFi]=g2oSiw;
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);
g2oCorrectedSiw = g2oSic*mg2oMergeScw;
vCorrectedSim3[pKFi]=g2oCorrectedSiw;
}
else
{
g2oCorrectedSiw = g2oCorrectedScw;
}
pKFi->mTcwMerge = pKFi->GetPose();
// 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();
pKFi->mfScale = s;
eigt *=(1./s); //[R t/s;0 1]
cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
pKFi->mTcwMerge = correctedTiw;
// Make sure connections are updated
if(pCurrentMap->isImuInitialized())
{
Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix();
pKFi->mVwbMerge = Converter::toCvMat(Rcor)*pKFi->GetVelocity();
}
//TODO DEBUG to know which are the KFs that had been moved to the other map
//pKFi->mnOriginMapId = 5;
}
for(MapPoint* pMPi : spLocalWindowMPs)
{
if(!pMPi || pMPi->isBad())
continue;
KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];
// 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(g2oNonCorrectedSiw.map(eigP3Dw));
Eigen::Matrix3d eigR = g2oCorrectedSwi.rotation().toRotationMatrix();
Eigen::Matrix3d Rcor = eigR * g2oNonCorrectedSiw.rotation().toRotationMatrix();
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->mPosMerge = cvCorrectedP3Dw;
pMPi->mNormalVectorMerge = Converter::toCvMat(Rcor) * pMPi->GetNormal();
}
{
unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
unique_lock<mutex> mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map
for(KeyFrame* pKFi : spLocalWindowKFs)
{
if(!pKFi || pKFi->isBad())
{
continue;
}
pKFi->mTcwBefMerge = pKFi->GetPose();
pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
pKFi->SetPose(pKFi->mTcwMerge);
// Make sure connections are updated
pKFi->UpdateMap(pMergeMap);
pKFi->mnMergeCorrectedForKF = mpCurrentKF->mnId;
pMergeMap->AddKeyFrame(pKFi);
pCurrentMap->EraseKeyFrame(pKFi);
if(pCurrentMap->isImuInitialized())
{
pKFi->SetVelocity(pKFi->mVwbMerge);
}
}
for(MapPoint* pMPi : spLocalWindowMPs)
{
if(!pMPi || pMPi->isBad())
continue;
pMPi->SetWorldPos(pMPi->mPosMerge);
pMPi->SetNormalVector(pMPi->mNormalVectorMerge);
pMPi->UpdateMap(pMergeMap);
pMergeMap->AddMapPoint(pMPi);
pCurrentMap->EraseMapPoint(pMPi);
}
mpAtlas->ChangeMap(pMergeMap);
mpAtlas->SetMapBad(pCurrentMap);
pMergeMap->IncreaseChangeIndex();
}
//Rebuild the essential graph in the local window
pCurrentMap->GetOriginKF()->SetFirstConnection(false);
pNewChild = mpCurrentKF->GetParent(); // Old parent, it will be the new child of this KF
pNewParent = mpCurrentKF; // Old child, now it will be the parent of its own parent(we need eliminate this KF from children list in its old parent)
mpCurrentKF->ChangeParent(mpMergeMatchedKF);
while(pNewChild )
{
pNewChild->EraseChild(pNewParent); // We remove the relation between the old parent and the new for avoid loop
KeyFrame * pOldParent = pNewChild->GetParent();
pNewChild->ChangeParent(pNewParent);
pNewParent = pNewChild;
pNewChild = pOldParent;
}
//Update the connections between the local window
mpMergeMatchedKF->UpdateConnections();
vpMergeConnectedKFs = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
vpMergeConnectedKFs.push_back(mpMergeMatchedKF);
vpCheckFuseMapPoint.reserve(spMapPointMerge.size());
std::copy(spMapPointMerge.begin(), spMapPointMerge.end(), std::back_inserter(vpCheckFuseMapPoint));
// Project MapPoints observed in the neighborhood of the merge keyframe
// into the current keyframe and neighbors using corrected poses.
// Fuse duplications.
SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint);
// Update connectivity
for(KeyFrame* pKFi : spLocalWindowKFs)
{
if(!pKFi || pKFi->isBad())
continue;
pKFi->UpdateConnections();
}
for(KeyFrame* pKFi : spMergeConnectedKFs)
{
if(!pKFi || pKFi->isBad())
continue;
pKFi->UpdateConnections();
}
bool bStop = false;
Verbose::PrintMess("MERGE: Start local BA ", Verbose::VERBOSITY_DEBUG);
vpLocalCurrentWindowKFs.clear();
vpMergeConnectedKFs.clear();
std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs));
std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs));
if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO)
{
Optimizer::MergeInertialBA(mpLocalMapper->GetCurrKF(),mpMergeMatchedKF,&bStop, mpCurrentKF->GetMap(),vCorrectedSim3);
}
else
{
Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop);
}
// Loop closed. Release Local Mapping.
mpLocalMapper->Release();
Verbose::PrintMess("MERGE: Finish the LBA", Verbose::VERBOSITY_DEBUG);
//Update the non critical area from the current map to the merged map
vector<KeyFrame*> vpCurrentMapKFs = pCurrentMap->GetAllKeyFrames();
vector<MapPoint*> vpCurrentMapMPs = pCurrentMap->GetAllMapPoints();
if(vpCurrentMapKFs.size() == 0)
{
Verbose::PrintMess("MERGE: There are not KFs outside of the welding area", Verbose::VERBOSITY_DEBUG);
}
else
{
Verbose::PrintMess("MERGE: Calculate the new position of the elements outside of the window", Verbose::VERBOSITY_DEBUG);
//Apply the transformation
{
if(mpTracker->mSensor == System::MONOCULAR)
{
unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
for(KeyFrame* pKFi : vpCurrentMapKFs)
{
if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
{
continue;
}
g2o::Sim3 g2oCorrectedSiw;
cv::Mat Tiw = pKFi->GetPose();
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
vNonCorrectedSim3[pKFi]=g2oSiw;
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 g2oSim(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
g2oCorrectedSiw = g2oSim*mg2oMergeScw;
vCorrectedSim3[pKFi]=g2oCorrectedSiw;
// 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();
pKFi->mfScale = s;
eigt *=(1./s); //[R t/s;0 1]
cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
pKFi->mTcwBefMerge = pKFi->GetPose();
pKFi->mTwcBefMerge = pKFi->GetPoseInverse();
pKFi->SetPose(correctedTiw);
if(pCurrentMap->isImuInitialized())
{
Eigen::Matrix3d Rcor = eigR.transpose()*vNonCorrectedSim3[pKFi].rotation().toRotationMatrix();
pKFi->SetVelocity(Converter::toCvMat(Rcor)*pKFi->GetVelocity()); // TODO: should add here scale s
}
}
for(MapPoint* pMPi : vpCurrentMapMPs)
{
if(!pMPi || pMPi->isBad()|| pMPi->GetMap() != pCurrentMap)
continue;
KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];
// 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(g2oNonCorrectedSiw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->UpdateNormalAndDepth();
}
}
}
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
// Optimize graph (and update the loop position for each element form the begining to the end)
if(mpTracker->mSensor != System::MONOCULAR)
{
Optimizer::OptimizeEssentialGraph(mpCurrentKF, vpMergeConnectedKFs, vpLocalCurrentWindowKFs, vpCurrentMapKFs, vpCurrentMapMPs);
}
{
// Get Merge Map Mutex
unique_lock<mutex> currentLock(pCurrentMap->mMutexMapUpdate); // We update the current map with the Merge information
unique_lock<mutex> mergeLock(pMergeMap->mMutexMapUpdate); // We remove the Kfs and MPs in the merged area from the old map
for(KeyFrame* pKFi : vpCurrentMapKFs)
{
if(!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap)
{
continue;
}
// Make sure connections are updated
pKFi->UpdateMap(pMergeMap);
pMergeMap->AddKeyFrame(pKFi);
pCurrentMap->EraseKeyFrame(pKFi);
}
for(MapPoint* pMPi : vpCurrentMapMPs)
{
if(!pMPi || pMPi->isBad())
continue;
pMPi->UpdateMap(pMergeMap);
pMergeMap->AddMapPoint(pMPi);
pCurrentMap->EraseMapPoint(pMPi);
}
}
}
mpLocalMapper->Release();
Verbose::PrintMess("MERGE:Completed!!!!!", Verbose::VERBOSITY_DEBUG);
if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)))
{
// Launch a new thread to perform Global Bundle Adjustment
Verbose::PrintMess("Relaunch Global BA", Verbose::VERBOSITY_DEBUG);
mbRunningGBA = true;
mbFinishedGBA = false;
mbStopGBA = false;
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this, pMergeMap, mpCurrentKF->mnId);
}
mpMergeMatchedKF->AddMergeEdge(mpCurrentKF);
mpCurrentKF->AddMergeEdge(mpMergeMatchedKF);
pCurrentMap->IncreaseChangeIndex();
pMergeMap->IncreaseChangeIndex();
mpAtlas->RemoveBadMaps();
}
那么这一篇就是这三个函数（因为一三太长了…）,下一篇博客接着分析
3180
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
