ComputeSim3
/********************************
* 計算每一個回環候選關鍵幀與目前關鍵幀之間的相似矩陣
* 1. 首先通過BOW向量将回環候選關鍵幀和目前關鍵幀進行比對,得到比對地圖點,通過比對地圖點初始化相似矩陣求解器
* 2. 周遊所有的回環候選關鍵幀和目前關鍵幀計算sim矩陣,并優化sim矩陣,根據優化sim矩陣确定比對内點數量,進而确定此sim矩陣的準确性,以及是否可以判定為一回環.
* 3. 在找到的回環關鍵幀周圍查找共視關鍵幀 并比對共視關鍵幀的地圖點和目前關鍵幀 相當與是比對covisual graph和目前關鍵幀 根據比對點數量确定目前幀是否發生了回環
**************************************/
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[i][j] 候選回環幀i 對應于 待回環幀第j地圖點 對應的地圖點
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],mbFixScale);
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
// RANSAC疊代每一個回環候選關鍵幀和目前待回環關鍵幀
// 通過SearchByBoW比對得到初步比對點,根據此比對計算兩關鍵幀之間的sim矩陣
//
while(nCandidates>0 && !bMatch)
{
for(int i=0; i<nInitialCandidates; i++)
{
if(vbDiscarded[i]) //小于20
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
// 如果RANSAC求取了一個合适的相似變換矩陣sim 則通過sim矩陣重新進行地圖點比對,并優化sim矩陣并确定内點 根據内點數量判定sim矩陣是否符合要求
if(!Scm.empty())
{
vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL)); //比對 待回環幀i幀 對應的兩地圖點 =>> vpMapPointMatches[i] = loopkfi.map[j] 此幀i點 對應于 回環幀的j點
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
if(vbInliers[j])
vpMapPointMatches[j]=vvpMapPointMatches[i][j]; //第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); //sim3 current點到 回環幀點的sim3
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
// 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
// 在找到的回環關鍵幀周圍查找共視關鍵幀 并比對共視關鍵幀的地圖點和目前關鍵幀 相當于是比對covisual graph和目前關鍵幀 改動關聯
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) //回環幀是 mpMatchedKF
mvpEnoughConsistentCandidates[i]->SetErase();
return true;
}
else
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
}
1）每一個候選回環幀都要跟待回環幀進行bow的詞袋模型的比對，并且得到比對點。
（同一單詞中兩幀的關鍵點進行最佳比對）
2）比對點數大于等于20的時候進行sim3計算。
3）通過pSolver->iterate(5,bNoMore,vbInliers,nInliers); 計算出Scm
在坐标系 幀一幀二中任意選3個對點 計算sim3算法（也可以icp計算）得到位姿變化 T12
4）matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5); 進行sim3驗證
雙向比對:
根據相似矩陣将關鍵幀1中的地圖點向關鍵幀2中投影,确定投影區域,并在投影區域内尋找關鍵幀1中地圖點的比對
根據相似矩陣将關鍵幀2中的地圖點向關鍵幀1中投影,确定投影區域,并在投影區域内尋找關鍵幀2中地圖點的比對
根據雙向比對結果,如果兩次比對都能成功,則确定該對比對是有效的.将其存入vpMatches12容器
最終傳回比對點對個數
幀一的地圖三維點，在相機一的坐标，用優化的T12轉化到相機二的坐标。驗證幀二是否有這個像素點，幀二的門檻值範圍内找特征點比對，找到最佳比對。
雙向 同理。
都成功是比對點。
5）const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale)
優化的政策
關鍵點1的uv值- K1 * t12 * p3dc2
同理 關鍵點2的uv值- K2 * t21 * p3dc1
優化得到sim3
6）修正
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
mg2oScw = gScm*gSmw;
因為gSmw是待回環幀在世界坐标系的位姿，計算出來目前幀 在正确世界坐标系的位姿
7）mvpCurrentMatchedPoints是目前幀和 回環幀的比對點。mpMatchedKF是回環幀
8）mvpLoopMapPoints 回環幀及其共視幀的地圖點 vpLoopConnectedKFs回環幀及其共視幀
9）matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
注意 裡面3d點的來源是 mvpLoopMapPoints 也就是正确的位置3d點
p3Dc得到了在目前相機坐标系下的坐标，門檻值範圍内找最佳比對 傳回比對點數目
10）數額足夠 完成
![WiFi SLAM包配置記錄WiFi SLAM包配置記錄然後，按照前面編譯順序，依次往下編譯即可，編譯完成之後就可以使用wifislam了[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)