orb-slam系列 Tracking线程 重定位(六)
程序员文章站
2022-03-16 20:57:06
...
函数属性：类Tracking的成员函数Relocalization()
函数功能：
（1）计算当前帧的BoW映射
（2）找到与当前帧相似的候选关键帧
（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw
（4）优化相机位姿Tcw
首先进行优化，将优化的结果存到nGood中，
如果优化结果不理想进行映射匹配，然后再进行优化
如果优化结果还不理想缩小映射窗口后在进行匹配并优化
此时若还不理想就判定该候选关键帧不能与本帧形成匹配，继续进行下一关键帧的匹配;如果可以，则证明已经进行重定位或者找到回环，退出循环
函数参数介绍：NULL
备注：重定位函数，用于失帧重定位和回环检测函数中
bool Tracking::Relocalization()
{
// Compute Bag of Words Vector 计算当前帧的BoW向量和Feature向量
mCurrentFrame.ComputeBoW();
// Relocalization is performed when tracking is lost
// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
// 当追踪线程丢失时进行重定位
// 找到当前帧相似的候选关键帧
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
if(vpCandidateKFs.empty()) //如果当前帧找不到相似候选关键帧
return false;
const int nKFs = vpCandidateKFs.size(); //得到候选关键帧的数量
// We perform first an ORB matching with each candidate
// If enough matches are found we setup a PnP solver 对于没一个候选关键帧与当前帧进行匹配，如果发现匹配点数量足够的话，我们进行PnP求解相机位姿
ORBmatcher matcher(0.75,true);
vector<PnPsolver*> vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
vector<bool> vbDiscarded;
vbDiscarded.resize(nKFs);
// 候选关键帧数量
int nCandidates=0;
// 匹配当前帧与找到的候选关键帧，并将每一候选关键帧匹配到的地图点存储到vvpMapPointMatches容器中，并记录每一候选关键帧是否的到满意的匹配，储存到vbDiscarded容器中
// 根据匹配到的地图点和当前帧建立PNP解决器将其存储到vpPnPsolvers容器中
for(int i=0; i<nKFs; i++)
{
KeyFrame* pKF = vpCandidateKFs[i];
if(pKF->isBad())
vbDiscarded[i] = true;
else
{
//当前帧和候选关键帧通过BoW进行匹配 vvpMapPointMatches数组存储该候选关键帧与该帧匹配到的地图点
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
if(nmatches<15) //如果匹配点数量小于15 ，那么就说明该匹配是失败的
{
vbDiscarded[i] = true;
continue;
}
else //否则建立PNPsolver
{
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991); //设置PNP求解器的相关参数 iciccccccccccccc
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}
// Alternatively perform some iterations of P4P RANSAC
// Until we found a camera pose supported by enough inliers
bool bMatch = false; //该标志量表示是否完成重定位
ORBmatcher matcher2(0.9,true);
while(nCandidates>0 && !bMatch)
{
for(int i=0; i<nKFs; i++)
{
//检测是否在第一次匹配时已经标志为不符合要求的候选关键帧
if(vbDiscarded[i])
continue;
// Perform 5 Ransac Iterations 执行5次RANSAC迭代
vector<bool> vbInliers;
int nInliers;
bool bNoMore;
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers); //用PNP计算位姿 bNoMore是否达到最大迭代次数 vbInliers表示该点是否为内点 nInliers内点数量
// If Ransac reachs max. iterations discard keyframe 如果RANSAC迭代达到最大则取消迭代，认为该帧不能与该关键帧匹配
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize 如果相机位姿被计算出，进行优化
if(!Tcw.empty())
{
Tcw.copyTo(mCurrentFrame.mTcw);
set<MapPoint*> sFound;
const int np = vbInliers.size(); //该帧与当前匹配关键帧之间的匹配地图点数量
for(int j=0; j<np; j++) //循环所有的匹配地图点
{
if(vbInliers[j]) //如果该匹配地图点是内点 则将该地图点添加到当前帧的匹配地图点,否则为空
{
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
}
else
mCurrentFrame.mvpMapPoints[j]=NULL;
}
//对求出的位姿进行优化
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
if(nGood<10)
continue;
//剔除外点
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
// If few inliers, search by projection in a coarse window and optimize again 如果有很少的内点，那么通过仿射进行再次优化
if(nGood<50)
{
int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100); //通过投影的方式进行再次匹配,增加匹配点
if(nadditional+nGood>=50) //如果投影匹配和BOW匹配得到的内点大于50
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame); //进行再次优化
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
//如果有许多内点但是还不足够，那么就再次通过缩小窗口进行投影 进行再次优化
if(nGood>30 && nGood<50)
{
sFound.clear();
for(int ip =0; ip<mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
// Final optimization
if(nGood+nadditional>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame); //最终优化
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=NULL;
}
}
}
}
// If the pose is supported by enough inliers stop ransacs and continue 如果位姿有足够多的内点那么停止计算退出循环，证明已经成功重定位。
if(nGood>=50)
{
bMatch = true;
break;
}
}
}
}
if(!bMatch)
{
return false;
}
else
{
mnLastRelocFrameId = mCurrentFrame.mnId; //重定位成功，将上次重定位的ID设为本帧的ID
return true;
}
}
1）先找到该帧的候选关键帧
/******************************************
在所有关键帧中检测重定位候选关键帧
候选关键帧的选择标准:
1 首先查找与该帧存在相同词典节点的所有关键帧作为候选关键帧
2 然后根据关键帧与待重定位帧的相同节点数来删除相同节点数小的关键帧
3 之后计算每个关键帧的累计共视相似度得分 并且如果该关键帧的共视关键帧比该关键帧有更多的得分就用该共视关键帧代替该关键帧
累计共视相似度得分的计算方法: 根据两帧之间相同节点在词典中相应节点的得分来计算两帧的共视相似度
如果共视相似度大的证明有更多的可能进行重定位
重定位候选关键帧的存储容器(经过层层筛选的)
lKFsSharingWords--->lScoreAndMatch--->lAccScoreAndMatch--->vpRelocCandidates
*******************************************/
vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
{
//存储与该帧数据有相同节点的关键帧容器
list<KeyFrame*> lKFsSharingWords;
// Search all keyframes that share a word with current frame
{
unique_lock<mutex> lock(mMutex);
for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++) //遍历当前帧的所有BOW向量(每个特征点对应一个)
{
list<KeyFrame*> &lKFs = mvInvertedFile[vit->first]; //寻找与该特征点存在于同一词典节点上的其他关键帧
for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) //遍历这所有的关键帧
{
KeyFrame* pKFi=*lit;
if(pKFi->mnRelocQuery!=F->mnId) //将该关键帧的重定位单词数量初始化
{
pKFi->mnRelocWords=0;
pKFi->mnRelocQuery=F->mnId;
lKFsSharingWords.push_back(pKFi);
}
pKFi->mnRelocWords++; // 该关键帧与待重定位帧存在相同字典节点的数量
}
}
}
if(lKFsSharingWords.empty()) // 如果该帧没有与之存在相同节点关键帧 那么返回空
return vector<KeyFrame*>();
// Only compare against those keyframes that share enough words
// 所有候选关键帧和当前关键帧在相同节点下的特征点数量中的最大值
int maxCommonWords=0;
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) // 寻找最大共视单词数
{
if((*lit)->mnRelocWords>maxCommonWords)
maxCommonWords=(*lit)->mnRelocWords;
}
// 根据比例计算最小共视单词数
int minCommonWords = maxCommonWords*0.8f;
// 存储的是第一次筛选后的候选关键帧以及其重定位得分(候选关键帧与待重定位关键帧的BOW得分)
list<pair<float,KeyFrame*> > lScoreAndMatch;
int nscores=0;
// Compute similarity score. 计算与当前待重定位帧有相同词典节点的所有关键帧的相似度得分(根据词典的相同节点数来筛选)
for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
{
KeyFrame* pKFi = *lit;
if(pKFi->mnRelocWords>minCommonWords) // 如果该关键帧与待重定位帧的相同节点数大于最小相同节点数
{
nscores++;
// si存储的是在词典中待重定位帧与查询到的关键帧之间的得分
float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
pKFi->mRelocScore=si; // 将关键帧的重定位得分赋值为si
lScoreAndMatch.push_back(make_pair(si,pKFi));
}
}
if(lScoreAndMatch.empty())
return vector<KeyFrame*>();
//累计重定位得分(候选关键帧和当前待重定位关键帧的BOW得分) 及 最优的重定位关键帧
list<pair<float,KeyFrame*> > lAccScoreAndMatch;
float bestAccScore = 0;
// Lets now accumulate score by covisibility 通过共视相似度来获得得分
for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) //遍历lScoreAndMatch中所有关键帧
{
KeyFrame* pKFi = it->second;
// 存储与改关键帧*视程度最好的10帧关键帧
vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
//存储关键帧的最优得分
float bestScore = it->first;
// 所有共视关键帧的累计重定位得分
float accScore = bestScore;
// 存储最优重定位关键帧
KeyFrame* pBestKF = pKFi;
for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) //遍历这些共视关键帧
{
KeyFrame* pKF2 = *vit;
if(pKF2->mnRelocQuery!=F->mnId) //如果它的待重定位帧与该帧不同,则跳过该帧
continue;
accScore+=pKF2->mRelocScore; //如果相同 则将共视关键帧的得分也加到本次重定位得分中
if(pKF2->mRelocScore>bestScore) // 如果共视关键帧的重定位得分大于当前关键帧的重定位得分
{
pBestKF=pKF2; //最优重定位关键帧
bestScore = pKF2->mRelocScore; // 则将当前关键帧的最优重定位得分赋值为共视关键帧的重定位得分
}
}
lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
if(accScore>bestAccScore)
bestAccScore=accScore;
}
// Return all those keyframes with a score higher than 0.75*bestScore 返回所有重定位累计得分大于0.75倍最优重定位累计得分的所有关键帧
float minScoreToRetain = 0.75f*bestAccScore;
// 存储所有已经插入vpRelocCandidates的关键帧
set<KeyFrame*> spAlreadyAddedKF;
// 重定位候选帧
vector<KeyFrame*> vpRelocCandidates;
vpRelocCandidates.reserve(lAccScoreAndMatch.size());
for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
{
const float &si = it->first;
if(si>minScoreToRetain) // 如果该关键帧的累计重定位得分大于最优累计关键帧重定位得分的0.75倍
{
KeyFrame* pKFi = it->second;
if(!spAlreadyAddedKF.count(pKFi)) // 如果没有插入过该关键帧则插入该关键帧
{
vpRelocCandidates.push_back(pKFi);
spAlreadyAddedKF.insert(pKFi);
}
}
}
return vpRelocCandidates;
}
1）遍历bow向量，将关联的所有帧的重定位队列指向当前帧
2）累加该关键帧与待重定位帧存在相同字典节点的数量
3）找到最大单词数，*0.8赋值最小的单词数 进行筛选
4）lScoreAndMatch.push_back(make_pair(si,pKFi)); 得分 帧
5）lScoreAndMatch的共视关键帧最好的10帧 计算得分 一起加到lScoreAndMatch （最佳帧可能是词典帧的其中一个共视帧）
6）返回所有重定位累计得分大于0.75倍最优重定位累计得分的所有关键帧vpRelocCandidates
2）PNP求解
计算当前帧 pnp 求出位姿
/ PnP求解：已知世界坐标系下的3D点与图像坐标系对应的2D点，求解相机的外参(R t)，即从世界坐标系到相机坐标系的变换。
// 而EPnP的思想是：
// 将世界坐标系所有的3D点用四个虚拟的控制点来表示，将图像上对应的特征点转化为相机坐标系下的四个控制点
// 根据世界坐标系下的四个控制点与相机坐标系下对应的四个控制点（与世界坐标系下四个控制点有相同尺度）即可恢复出(R t)
// |x|
// |u| |fx r u0||r11 r12 r13 t1||y|
// s |v| = |0 fy v0||r21 r22 r23 t2||z|
// |1| |0 0 1 ||r32 r32 r33 t3||1|
// step1:用四个控制点来表达所有的3D点
// p_w = sigma(alphas_j * pctrl_w_j), j从0到4
// p_c = sigma(alphas_j * pctrl_c_j), j从0到4
// sigma(alphas_j) = 1, j从0到4
// step2:根据针孔投影模型
// s * u = K * sigma(alphas_j * pctrl_c_j), j从0到4
// step3:将step2的式子展开, 消去s
// sigma(alphas_j * fx * Xctrl_c_j) + alphas_j * (u0-u)*Zctrl_c_j = 0
// sigma(alphas_j * fy * Xctrl_c_j) + alphas_j * (v0-u)*Zctrl_c_j = 0
// step4:将step3中的12未知参数（4个控制点*3维参考点坐标）提成列向量
// Mx = 0,计算得到初始的解x后可以用Gauss-Newton来提纯得到四个相机坐标系的控制点
// step5:根据得到的p_w和对应的p_c，最小化重投影误差即可求解出R t
PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):
pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),
mnIterations(0), mnBestInliers(0), N(0)
{
mvpMapPointMatches = vpMapPointMatches;
mvP2D.reserve(F.mvpMapPoints.size());
mvSigma2.reserve(F.mvpMapPoints.size());
mvP3Dw.reserve(F.mvpMapPoints.size());
mvKeyPointIndices.reserve(F.mvpMapPoints.size());
mvAllIndices.reserve(F.mvpMapPoints.size());
int idx=0;
for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPointMatches[i]; //得到地图点
if(pMP) //如果该地图点存在
{
if(!pMP->isBad())
{
const cv::KeyPoint &kp = F.mvKeysUn[i]; //关键点坐标
mvP2D.push_back(kp.pt);
mvSigma2.push_back(F.mvLevelSigma2[kp.octave]); //存储每个特征点所对应的空间尺度
cv::Mat Pos = pMP->GetWorldPos(); //该地图点的世界坐标
mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0),Pos.at<float>(1), Pos.at<float>(2)));
mvKeyPointIndices.push_back(i); //关键点索引
mvAllIndices.push_back(idx); // 关键点而且也是地图点的索引
idx++;
}
}
}
// Set camera calibration parameters
fu = F.fx;
fv = F.fy;
uc = F.cx;
vc = F.cy;
SetRansacParameters(); //设置RANSAC的相关参数
}
候选帧还需要分别进行pnp计算位姿迭代5次
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
如果位姿被计算出
匹配地图点的内点 加入该帧地图点 外点为空
位姿优化
内点小于10 跳出 重新换候选帧
内点小于50 投影方式再次匹配
int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100); //通过投影的方式进行再次匹配,增加匹配点
if(nadditional+nGood>=50) //如果投影匹配和BOW匹配得到的内点大于50
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame); //进行再次优化
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
//如果有许多内点但是还不足够，那么就再次通过缩小窗口进行投影 进行再次优化
if(nGood>30 && nGood<50)
{
sFound.clear();
for(int ip =0; ip<mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
// Final optimization
if(nGood+nadditional>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame); //最终优化
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=NULL;
}
}
}
地图点选择策略
如果内点大于50 认为足够 退出循环 重定位成功
上一篇： div居中：最全的div居中方法总结