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;
}
}
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
1）每一个候选回环帧都要跟待回环帧进行bow的词袋模型的匹配，并且得到匹配点。
（同一单词中两帧的关键点进行最佳匹配）
2）匹配点数大于等于20的时候进行sim3计算。
Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
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）数额足够 完成
1026
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
