试图学会ORB-SLAM2(4)——KeyFrame类

最新推荐文章于 2023-10-23 23:54:24 发布

什么都不会的小澎友

最新推荐文章于 2023-10-23 23:54:24 发布

阅读量318

点赞数 1

分类专栏： ORB-SLAM2 文章标签： c++ 计算机视觉

本文链接：https://blog.csdn.net/weixin_45432823/article/details/129022510

版权

ORB-SLAM2 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

关键帧是很多SLAM框架都会用到的一个概念，在之前的代码流程里可以看到，Tracking线程向LocalMapping和LoopClosing线程传递的就只有关键帧。关键帧相当于slam的骨架，是在局部一系列普通帧中选出一帧作为局部帧的代表，记录局部信息。关键帧的筛选、增加和剔除在ORB-SLAM2里都有着很严密的设计。但在KeyFrame类里，实现的更多的是关键帧自身的一些功能。
具体在函数里，在Tracking中进行关键帧判断，调用的是Tracking类的CreateNewKeyFrame函数，然后再调用LocalMapping线程的InsertKeyFrame函数插入到局部地图之中。

// Tracking.cc
if(NeedNewKeyFrame())
	CreateNewKeyFrame();
// CreateNewKeyFrame
// 关键帧插入到列表 mlNewKeyFrames中，等待local mapping线程临幸
mpLocalMapper->InsertKeyFrame(pKF);

然后在LocalMapping线程中，在Run函数中会直接把当前关键帧插入到LoopClosing线程中

mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

成员变量

关键帧的成员变量有很多和Frame类是相同的，像public的网格化用到的变量mnGridCols等，相机的参数，特征点的变量mvKeys等，词袋的mBowVec和mFeatVec，尺度信息、图像边界信息，还有protected的相机位姿mTcw等，这里把一些不同的变量列出来

变量名	访问控制	简单解释
`static long unsigned int nNextId`	`public`	上一帧的ID(用nLastId会不会好一些?)
`long unsigned int mnId`	`public`	当前帧的ID(用上一帧的ID+1)
`const long unsigned int mnFrameId`	`public`	记录当前关键帧是由哪个`Frame`初始得到的
`const double mTimeStamp`	`public`	时间戳
`long unsigned int mnTrackReferenceForFrame`	`public`	这个变量主要是用做一个记录的功能
`long unsigned int mnFuseTargetForKF`	`public`	也是一个标记，在局部建图线程中标记和哪个关键帧融合了
`long unsigned int mnBALocalForKF` `long unsigned int mnBAFixedForKF`	`public`	这两个都是在`LocalMapping`中的LocalBA中用到的，前面的是当前局部关键帧的ID，后边的是添加进优化中做约束条件但不参与优化的关键帧ID
`long unsigned int mnLoopQuery`	`public`	在回环检测中使用，标记候选关键帧
`int mnLoopWords`	`public`	当前关键帧和形成回环的候选关键帧中具有相同Word的个数
`float mLoopScore`	`public`	和回环候选关键帧词袋匹配得分
`long unsigned int mnRelocQuery`	`public`	重定位中，需要进行重定位的帧ID
`int mnRelocWords`	`public`	和重定位帧的相同Word个数
`float mRelocScore`	`public`	和重定位帧的词袋匹配得分
`cv::Mat mTcwGBA` `cv::Mat mTcwBefGBA` `long unsigned int mnBAGlobalForKF`	`public`	全局BA使用，第一个是全局BA后的位姿，第二个是记录的优化前的位姿，第三个是记录哪个帧触发的全局BA，主要是防止重复
`cv::Mat mTcp`	`public`	相对于父关键帧的位姿，在删除关键帧连接关系的时候会用到
`std::vector<MapPoint*> mvpMapPoints`	`protected`	和特征点联系起来的地图点
`KeyFrameDatabase* mpKeyFrameDB`	`protected`	关键帧数据库
`ORBVocabulary* mpORBvocabulary`	`protected`	词袋对象
`std::vector` `<std::vector<std::vector<size_t>>> mGrid`	`protected`	加速匹配时用到的，把关键帧的特征点信息存储起来
`std::map<KeyFrame*,int> mConnectedKeyFrameWeights`	protected	与该关键帧连接的关键帧与权重
`std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames`	protected	共视关键帧的排序，权重从大到小
`std::vector<int> mvOrderedWeights`	protected	共视关键帧权重的排序，和上一个变量对应
`bool mbFirstConnection` `KeyFrame* mpParent` `std::set<KeyFrame> mspChildrens` `std::set<KeyFrame> mspLoopEdges`	protected	生成树相关的一些变量
`bool mbNotErase` `bool mbToBeErased` `bool mbBad`	protected	一些标记
`float mHalfBaseline`	protected	基线长的一半，只有在可视化中使用了
`Map* mpMap`	protected	对应的地图
`std::mutex mMutexPose` `std::mutex mMutexConnections` `std::mutex mMutexFeatures`	protected	互斥锁

成员函数

KeyFrame类和Frame类也有很多相同的函数，相同的函数主要是为了后边把图像网格化做特征提取与匹配用，关键帧自身的特殊操作可能就在于连接关系，里面有很多添加、更新或删除连接关系的操作。同时关键帧在后边两个线程中是处理的主要对象，所以具体的实现与应用在三大线程中看得会更加直接。

构造函数

构造函数的参数变量有三个，分别是Frame &F，也就是初始化成关键帧的那一帧，Map *pMap，这个是和当前关键帧可能产生联系的地图，然后就是KeyFrameDatabase *pKFDB，也就是关键帧数据库，这个是在跟踪线程初始化的，在关键帧中被不断的操作。
KeyFrame构造函数使用了列表初始化的方法，把F的一些变量直接赋值给了当前关键帧，同时也对位姿进行了一个初始化。

关键帧类重要函数1 `UpdateConnections`

UpdateConnections在三个线程中都有使用，主要的作用就是更新关键帧之间的连接关系。

// Tracking线程中在初始化中使用到
// CreateInitialMapmonocular
pKFini->UpdateConnections();
// LocalMapping线程中在处理关键帧队列和融合当前帧与相邻关键帧地图点时使用
mpCurrentKeyFrame->UpdateConnections();
// LoopClosing线程中在回环矫正中使用
mpCurrent->UpdateConnections();
pKFi->UpdateConnections();

先是定义了一个很重要的变量，map<KeyFrame*,int> KFcounter，这个变量代表的就是其他关键帧和当前关键帧的共视程度，再把所有地图点取出来(用了mutex防止地图点被其他地方改变)，放在定义的vector<MapPoint*> vpMp中。

先通统计关键帧之间的共视程度，就是看地图点有没有被当前帧和其他关键帧同时看到

for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
	MapPoint* pMP = *vit;
	if(!pMP)
		continue;
	if(pMP->isBad())
		continue;

	// 对于每一个地图点，observations记录了可以观测到该地图点的所有关键帧
	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]++;
}
}

之后共视程度最高的关键帧，这里涉及到一个新变量vector<pair<int,KeyFrame*>> vPairs，它记录的是共视帧数大于阈值(th=15)的关键帧，这里用到了另一个函数AddConnections

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())
{
    vPairs.push_back(make_pair(nmax,pKFmax));
    pKFmax->AddConnection(this,nmax);
}

对满足共视程度的关键帧对更新连接关系和权重(从大到小排列)
排序直接用了C++ 的sort函数，默认的是升序排列因此后边用了push_front把整个序列调了过来，就实现从大到小排列了。
又用了一个mutex，把上边得到的结果都赋值给对应的成员变量，再更新生成树的连接

 if(mbFirstConnection && mnId!=0)
{
	// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
	mpParent = mvpOrderedConnectedKeyFrames.front();
	// 建立双向连接关系，将当前关键帧作为其子关键帧
	mpParent->AddChild(this);
	mbFirstConnection = false;
}

这里涉及到了父关键帧和子关键帧两个概念，父关键帧只有一个，这里选择了与当前帧共视程度最高的关键帧，子关键帧则是只要有共视关系即可，子关键帧的添加就是把当前关键帧添加到父关键帧的子关键帧集里

关键帧类重要函数2 `SetBadFlag`

成员函数中还有一个SetErase函数，那个函数的作用是在LoopClosing函数中删除当前关键帧，表示不进行回环检测的工作。而SetBadFlag函数也会在SetErease中被调用，是真正执行删除关键帧操作的函数。主要是在局部建图线程中，当当前关键帧的90%以上地图点被认为是冗余时，就会删除当前关键帧。

if(nRedundantObservations>0.9*nMPs)
	pKF->SetBadFlag();

首先处理无法删除的情况
不允许删除的情况分为两种，第一种就是当前关键帧是第一帧，那么作为整个系统的基，是不可以被删除的；第二种是mbNotErase变量被其他地方设置为true。

if(mnId==0)
	return;
else if(mbNotErase)
{
	// mbNotErase表示不应该删除，于是把mbToBeErased置为true，假装已经删除，其实没有删除
	mbToBeErased = true;
	return;
}

遍历当前关键帧的所有相连关键帧，因为之前添加的连接关系都是双向的，所以这里要把连接关系再删掉，调用EraseConnection函数就可以了。
遍历当前关键帧的地图点，把地图点和关键帧的连接关系也删掉，EraseObservation
更新生成树，主要是处理好父子关键帧的关系
先把自己和其他关键帧的关系清空

set<KeyFrame*> sParentCandidates;
// 将当前帧的父关键帧放入候选父关键帧
sParentCandidates.insert(mpParent);

遍历每一个子关键帧，为其选择新的父关键帧


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++)
	{
		// sParentCandidates 中刚开始存的是这里子关键帧的“爷爷”，也是当前关键帧的候选父关键帧
		for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
		{
			if(vpConnected[i]->mnId == (*spcit)->mnId)
			{
				int w = pKF->GetWeight(vpConnected[i]);
				// 寻找并更新权值最大的那个共视关系
				if(w>max)
				{
					pC = pKF;                   //子关键帧
					pP = vpConnected[i];        //目前和子关键帧具有最大权值的关键帧（将来的父关键帧） 
					max = w;                    //这个最大的权值
					bContinue = true;           //说明子节点找到了可以作为其新父关键帧的帧
				}
			}
		}
	}
}

如果找到了就更新子节点的父关键帧信息，如果找不到新的，就把当前关键帧(也就是要删除的这一帧)的父关键帧直接设为子节点的新关键帧，最后把这一帧分别在地图和关键帧数据库中删除，就彻底完成了关键帧的删除工作。

取出关键帧的函数1 `GetBestCovisibilityKeyFrames`

这个函数非常简单，但在后边用到的也非常多，函数输入的参数是设定要取出的关键帧的数目，因为之前得到的集合都是按共视程度从大到小排列好的，因此这里直接就取出了共视程度最高的前N个共视关键帧。

取出关键帧的函数2 `GetCovisiblesByWeight`

和上一个函数一样，这个函数的作用也是把关键帧取出来，不过这里输入的参数就相当于是一个阈值，利用了C++中的upper_bound函数（官方解释），把权重大于阈值w的关键帧取了出来。

vector<int>::iterator it = upper_bound( mvOrderedWeights.begin(),   //起点
                                        mvOrderedWeights.end(),     //终点
                                        w,                          //目标阈值
                                        KeyFrame::weightComp);      //比较函数从大到小排序
    
// 如果没有找到，说明最大的权重也比给定的阈值小，返回空
if(it==mvOrderedWeights.end() && *mvOrderedWeights.rbegin()<w)
return vector<KeyFrame*>();
else
{
	// 如果存在，返回满足要求的关键帧
	int n = it-mvOrderedWeights.begin();
	return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
}