其中void KeyFrame::UpdateConnections()
以及void KeyFrame::SetBadFlag()
比较难以理解。需要好好琢磨。
关键帧(共视图)——更新连接关系(和流程相关)
更新连接,创建共视图:
实现过程:
(1)首先获得该关键帧的所有MapPoint点,对于每一个MapPoint点,统计所有能观测到它的关键帧,然后统计这些关键帧与本关键帧的共视程度(即共视关键点个数)
(2)对每一个找到的关键帧(共视程度大于15),建立一条边,边的权重是该关键帧与当前关键帧公共3d点的个数
(3)对这些连接按照权重从大到小进行排序,以方便将来的处理,存储到mvpOrderedConnectedKeyFrames和mvOrderedWeights中
(4)更新父关键帧为关联关键帧权重最大帧
/**
* @brief 更新图的连接
*
* 1. 首先获得该关键帧的所有MapPoint点,统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度
* 对每一个找到的关键帧,建立一条边,边的权重是该关键帧与当前关键帧公共3d点的个数。
* 2. 并且该权重必须大于一个阈值,如果没有超过该阈值的权重,那么就只保留权重最大的边(与其它关键帧的共视程度比较高)
* 3. 对这些连接按照权重从大到小进行排序,以方便将来的处理
* 更新完covisibility图之后,如果没有初始化过,则初始化为连接权重最大的边(与其它关键帧共视程度最高的那个关键帧),类似于最大生成树
*/
void KeyFrame::UpdateConnections()
{
// 在没有执行这个函数前,关键帧只和MapPoints之间有连接关系,这个函数可以更新关键帧之间的连接关系
//===============1==================================
map<KeyFrame*,int> KFcounter; // 关键帧-权重,权重为其它关键帧与当前关键帧共视3d点的个数
vector<MapPoint*> vpMP;
{
// 获得该关键帧的所有3D点
unique_lock<mutex> lockMPs(mMutexFeatures);
vpMP = mvpMapPoints;
}
//For all map points in keyframe check in which other keyframes are they seen
//Increase counter for those keyframes
// 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
// 即统计每一个关键帧都有多少关键帧与它存在共视关系,统计结果放在KFcounter
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(!pMP)
continue;
if(pMP->isBad())
continue;
// 对于每一个MapPoint点,observations记录了可以观测到该MapPoint的所有关键帧
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]++;
}
}
// This should not happen
if(KFcounter.empty())
return;
//===============2==================================
// 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;
// vPairs记录与其它关键帧共视帧数大于th的关键帧
// pair<int,KeyFrame*>将关键帧的权重写在前面,关键帧写在后面方便后面排序
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));
// 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
// 更新其它KeyFrame的mConnectedKeyFrameWeights,更新其它关键帧与当前帧的连接权重
(mit->first)->AddConnection(this,mit->second);
}
}
// 如果没有超过阈值的权重,则对权重最大的关键帧建立连接
if(vPairs.empty())
{
// 如果每个关键帧与它共视的关键帧的个数都少于th,
// 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
// 这是对之前th这个阈值可能过高的一个补丁
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
// vPairs里存的都是相互共视程度比较高的关键帧和共视权重,由大到小
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);
}
//===============3==================================
{
unique_lock<mutex> lockCon(mMutexConnections);
// mspConnectedKeyFrames = spConnectedKeyFrames;
// 更新图的连接(权重)
mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights,更新当前帧与其它关键帧的连接权重
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;
}
}
}
关键帧(共视图 边)——增加回环的边
将某一关键帧加入回环边集合
void KeyFrame::AddLoopEdge(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mbNotErase = true;
mspLoopEdges.insert(pKF);
}
关键帧(共视图 边)——获得回环关键帧集合
set<KeyFrame*> KeyFrame::GetLoopEdges()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspLoopEdges;
}
关键帧(共视图)——设置不可移除
void KeyFrame::SetNotErase()
{
unique_lock<mutex> lock(mMutexConnections);
mbNotErase = true;
}
关键帧(共视图)——设置需要移除
void KeyFrame::SetErase()
{
{
unique_lock<mutex> lock(mMutexConnections);
if(mspLoopEdges.empty())
{
mbNotErase = false;
}
}
// 这个地方是不是应该:(!mbToBeErased),(wubo???) 这个地方确实需要探讨(求问)
// SetBadFlag函数就是将mbToBeErased置为true,mbToBeErased就表示该KeyFrame被擦除了
if(mbToBeErased)
{
SetBadFlag();
}
}
关键帧(共视图)——删除关键帧,修改父子连接关系
void KeyFrame::SetBadFlag()
{
{
unique_lock<mutex> lock(mMutexConnections);
if(mnId==0)
return;
else if(mbNotErase)// mbNotErase表示不应该擦除该KeyFrame,于是把mbToBeErased置为true,表示已经擦除了,其实没有擦除
{
mbToBeErased = true;
return;
}
}
for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
mit->first->EraseConnection(this);// 让其它的KeyFrame删除与自己的联系
for(size_t i=0; i<mvpMapPoints.size(); i++)
if(mvpMapPoints[i])
mvpMapPoints[i]->EraseObservation(this);// 让与自己有联系的MapPoint删除与自己的联系
{
unique_lock<mutex> lock(mMutexConnections);
unique_lock<mutex> lock1(mMutexFeatures);
//清空自己与其它关键帧之间的联系
mConnectedKeyFrameWeights.clear();
mvpOrderedConnectedKeyFrames.clear();
// Update Spanning Tree
set<KeyFrame*> sParentCandidates;
sParentCandidates.insert(mpParent);
// Assign at each iteration one children with a parent (the pair with highest covisibility weight)
// Include that children as new parent candidate for the rest
// 如果这个关键帧有自己的孩子关键帧,告诉这些子关键帧,它们的父关键帧不行了,赶紧找新的父关键帧
while(!mspChildrens.empty())
{
bool bContinue = false;
int max = -1;
KeyFrame* pC;
KeyFrame* pP;
// 遍历每一个子关键帧,让它们更新它们指向的父关键帧
for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++)
{
KeyFrame* pKF = *sit;
if(pKF->isBad())
continue;
// Check if a parent candidate is connected to the keyframe
// 子关键帧遍历每一个与它相连的关键帧(共视关键帧)
vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();
for(size_t i=0, iend=vpConnected.size(); i<iend; i++)
{
for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
{
// 如果该帧的子节点和父节点(祖孙节点)之间存在连接关系(共视)
// 举例:B-->A(B的父节点是A) C-->B(C的父节点是B) D--C(D与C相连) E--C(E与C相连) F--C(F与C相连) D-->A(D的父节点是A) E-->A(E的父节点是A)
// 现在B挂了,于是C在与自己相连的D、E、F节点中找到父节点指向A的D
// 此过程就是为了找到可以替换B的那个节点。
// 上面例子中,B为当前要设置为SetBadFlag的关键帧
// A为spcit,也即sParentCandidates
// C为pKF,pC,也即mspChildrens中的一个
// D、E、F为vpConnected中的变量,由于C与D间的权重 比 C与E间的权重大,因此D为pP
if(vpConnected[i]->mnId == (*spcit)->mnId)
{
int w = pKF->GetWeight(vpConnected[i]);
if(w>max)
{
pC = pKF;
pP = vpConnected[i];
max = w;
bContinue = true;
}
}
}
}
}
if(bContinue)
{
// 因为父节点死了,并且子节点找到了新的父节点,子节点更新自己的父节点
pC->ChangeParent(pP);
// 因为子节点找到了新的父节点并更新了父节点,那么该子节点升级,作为其它子节点的备选父节点
sParentCandidates.insert(pC);
// 该子节点处理完毕
mspChildrens.erase(pC);
}
else
break;
}
// If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
// 如果还有子节点没有找到新的父节点
if(!mspChildrens.empty())
for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
{
// 直接把父节点的父节点作为自己的父节点
(*sit)->ChangeParent(mpParent);
}
mpParent->EraseChild(this);
mTcp = Tcw*mpParent->GetPoseInverse();
mbBad = true;
}
mpMap->EraseKeyFrame(this);
mpKeyFrameDB->erase(this);
}
关键帧(共视图)——判断是个需要移除
判断一帧图像是否是坏帧,即是否已经被删除
bool KeyFrame::isBad()
{
unique_lock<mutex> lock(mMutexConnections);
return mbBad;
}
关键帧(共视图)——擦除连接关系
删除与某一帧的联系,并没有删除该关键帧,同时更新共视关系。
void KeyFrame::EraseConnection(KeyFrame* pKF)
{
bool bUpdate = false;
{
unique_lock<mutex> lock(mMutexConnections);
if(mConnectedKeyFrameWeights.count(pKF))
{
mConnectedKeyFrameWeights.erase(pKF);
bUpdate=true;
}
}
if(bUpdate)
UpdateBestCovisibles();
}
关键帧——得到特定区域内所有特征点
找到在以x,y为中心,边长为2r的方形内特征点,先遍历该圆域区间所属的边长为2r的格子区间内的所有特征点,再判断该点的尺度信息是否满足要求,再判断该特征点离中心点的距离是否满足半径要求,返回该区间的所有的特征点,参考普通帧的GetFeaturesInArea函数
// r为边长(半径)
vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const
{
vector<size_t> vIndices;
vIndices.reserve(N);
// floor向下取整,mfGridElementWidthInv为每个像素占多少个格子
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=mnGridCols)
return vIndices;
// ceil向上取整
const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=mnGridRows)
return vIndices;
const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
const vector<size_t> vCell = mGrid[ix][iy];
for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
if(fabs(distx)<r && fabs(disty)<r)
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
关键帧——判断坐标是否在图像中
查看x, y
与图像边界的关系
bool KeyFrame::IsInImage(const float &x, const float &y) const
{
return (x>=mnMinX && x<mnMaxX && y>=mnMinY && y<mnMaxY);
}
关键帧——反投影得到3D点
找到第i个关键点对应的3D坐标。首先找到对应的像素坐标,利用相机投影模型得到相机坐标,在根据变换矩阵,得到世界坐标系下3D点。
/**
* @brief Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
* @param i 第i个keypoint
* @return 3D点(相对于世界坐标系)
*/
cv::Mat KeyFrame::UnprojectStereo(int i)
{
const float z = mvDepth[i];
if(z>0)
{
// 由2维图像反投影到相机坐标系
// mvDepth是在ComputeStereoMatches函数中求取的
// mvDepth对应的校正前的特征点,因此这里对校正前特征点反投影
// 可在Frame::UnprojectStereo中却是对校正后的特征点mvKeysUn反投影
// 在ComputeStereoMatches函数中应该对校正后的特征点求深度?? (wubo???)
const float u = mvKeys[i].pt.x;
const float v = mvKeys[i].pt.y;
const float x = (u-cx)*z*invfx;
const float y = (v-cy)*z*invfy;
cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
unique_lock<mutex> lock(mMutexPose);
// 由相机坐标系转换到世界坐标系
// Twc为相机坐标系到世界坐标系的变换矩阵
// Twc.rowRange(0,3).colRange(0,3)取Twc矩阵的前3行与前3列
return Twc.rowRange(0,3).colRange(0,3)*x3Dc+Twc.rowRange(0,3).col(3);
}
else
return cv::Mat();
}
关键帧——得到场景深度
计算并返回所有地图点的深度中值,得到的相机坐标系下,场景的深度中值。仅仅求取对应的深度值,只需要取对应的矩阵行列进行计算即可。
/**
* @brief 评估当前关键帧场景深度,q=2表示中值
* @param q q=2
* @return Median Depth
*/
float KeyFrame::ComputeSceneMedianDepth(const int q)
{
vector<MapPoint*> vpMapPoints;
cv::Mat Tcw_;
{
unique_lock<mutex> lock(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPose);
vpMapPoints = mvpMapPoints;
Tcw_ = Tcw.clone();
}
vector<float> vDepths;
vDepths.reserve(N);
cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
Rcw2 = Rcw2.t();
float zcw = Tcw_.at<float>(2,3);
for(int i=0; i<N; i++)
{
if(mvpMapPoints[i])
{
MapPoint* pMP = mvpMapPoints[i];
cv::Mat x3Dw = pMP->GetWorldPos();
float z = Rcw2.dot(x3Dw)+zcw; // (R*x3Dw+t)的第三行,即z
vDepths.push_back(z);
}
}
sort(vDepths.begin(),vDepths.end());
return vDepths[(vDepths.size()-1)/q];
}