一、原理分析
KeyFrame为关键帧,关键帧之所以存在是因为优化需要,所以KeyFrame的几乎所有内容都是位优化服务的。该类中的函数较多,我们需要归类梳理一下,明白其功能原理,才能真正弄懂它的内容。
图优化需要构建节点和变,节点很好理解,就是关键帧的位姿,所以需要有读写位姿的功能,边分为两种,第一种边是和MapPoint之间的,所以需要有管理和MapPoint之间关系的函数,第二种边是和其他关键帧之间的,他们之间需要通过MapPoint产生联系,两帧能够共同观测到一定数量的MapPoint时则可以在他俩之间建立边,这种关系叫共视,所以需要有管理共视关系的函数,这种通过共视关系构建的优化模型叫做Covisibility Graph,但是,当需要优化较大范围的数据时,就会需要很大的计算量,因此需要简化,而ORB SLAM2中的Essential Graph就是Covisibility Graph的一种简化版,它通过“生成树(Spanning tree)”来管理各关键帧之间的关系,每个帧都有一个父节点和子节点,节点为其他关键帧,在构建优化模型时,只有具有父子关系的关键帧之间才建立边,换言之,Essential Graph就是Covisibility Graph的子集,这样就大大减少了边的数量,从而起到减小计算量的作用,因此,该类还需要有管理“生成树(Spanning tree)”的函数。
为了更清晰地了解Covisibility Graph 和 Essential Graph之间的区别,我们可以利用下面几张图再详细解释一下
图(a)没什么好说的
图(b)中Covisibility graph是用来描述不同关键帧可以看到多少相同的地图点:每个关键帧是一个节点,如果两个关键帧之间的共视地图点数量大于15,则这两个节点之间建立边,边的权重是共视地图点的数量
图(c)中Spanning tree就是生成树,保留了所有的节点(或者说关键帧),但给各个关键帧找了父节点和子节点,每帧只跟各自的父节点和子节点相连,与其他关键帧不连接,此即为spanning tree
图(d)中即是essential graph,是根据spanning tree建立的图模型,它是是简版的covisibility graph。
二、函数功能介绍
有了以上的分析,我们再回过头来看这个类中的这些函数,分析起来就容易很多了。我们按照上面的分析,对这些函数做一个归类,如下图所示
下面我们按照分类,讲解其中重要的一些函数
1. 构造函数
1)Frame:有参数构造函数
构造函数一共有三个参数:
Frame &F:当前帧
Map *pMap:地图Map
KeyFrameDatabase *pKFDB:指针和关键帧数据集的指针
{
//将下一帧的帧号赋值给mnId,然后自增1
mnId = nNextId++;
//根据栅格的列数重置栅格的size
mGrid.resize(mnGridCols);
//将该真的栅格内信息拷贝了一份给关键帧类内的变量
for (int i = 0; i < mnGridCols; i++)
{
mGrid[i].resize(mnGridRows);
for (int j = 0; j < mnGridRows; j++)
mGrid[i][j] = F.mGrid[i][j];
}
//最后将当前帧的姿态赋给该关键帧
SetPose(F.mTcw);
}
2. 位姿相关
1)SetPose:设置位姿
它只有一个参数Tcw_,这是传入的当前帧的位姿
void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
unique_lock<mutex> lock(mMutexPose);
Tcw_.copyTo(Tcw);
cv::Mat Rcw = Tcw.rowRange(0, 3).colRange(0, 3);
cv::Mat tcw = Tcw.rowRange(0, 3).col(3);
cv::Mat Rwc = Rcw.t();
Ow = -Rwc * tcw;//相机光心
Twc = cv::Mat::eye(4, 4, Tcw.type());
Rwc.copyTo(Twc.rowRange(0, 3).colRange(0, 3));
Ow.copyTo(Twc.rowRange(0, 3).col(3));
// center为相机坐标系(左目)下,立体相机中心的坐标
// 立体相机中心点坐标与左目相机坐标之间只是在x轴上相差mHalfBaseline,
// 因此可以看出,立体相机中两个摄像头的连线为x轴,正方向为左目相机指向右目相机
cv::Mat center = (cv::Mat_<float>(4, 1) << mHalfBaseline, 0, 0, 1);
// 世界坐标系下,左目相机中心到立体相机中心的向量,方向由左目相机指向立体相机中心
Cw = Twc * center;
}
3. Covisibility graph相关
1) AddConnection:增加连接
AddConnection(
KeyFrame *pKF, // 需要关联的关键帧
const int &weight) // 权重,即该关键帧与pKF共同观测到的3d点数量
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
{
unique_lock<mutex> lock(mMutexConnections);
// std::map::count函数只可能返回0或1两种情况
// 此处0表示之前没有过连接,1表示有过连接
//之前没有连接时,要用权重赋值,即添加连接
if (!mConnectedKeyFrameWeights.count(pKF))
mConnectedKeyFrameWeights[pKF] = weight;
//有连接,但权重发生变化时,也要用权重赋值,即更新权重
else if (mConnectedKeyFrameWeights[pKF] != weight)
mConnectedKeyFrameWeights[pKF] = weight;
else
return;
}
//更新最好的Covisibility
UpdateBestCovisibles();
}
2) UpdateBestCovisibles:更新最好的Covisibility
每一个关键帧都有一个容器,其中记录了与其他关键帧之间的weight,每次当关键帧添加连接、删除连接或者连接权重发生变化时,都需要根据weight对容器内内容重新排序。该函数的主要作用便是按照weight对连接的关键帧进行排序,更新后的变量存储在mvpOrderedConnectedKeyFrames 和 mvOrderedWeights中。
void KeyFrame::UpdateBestCovisibles()
{
unique_lock<mutex> lock(mMutexConnections);
vector<pair<int, KeyFrame *>> vPairs;
vPairs.reserve(mConnectedKeyFrameWeights.size());
// 取出所有连接的关键帧,将元素取出放入一个pair组成的vector中,排序后放入vPairs
// mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>,而vPairs变量将共视的3D点数放在前面,利于排序
for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)
vPairs.push_back(make_pair(mit->second, mit->first));
// 按照权重进行排序
sort(vPairs.begin(), vPairs.end());
list<KeyFrame *> lKFs; // keyframe
list<int> lWs; // weight
for (size_t i = 0, iend = vPairs.size(); i < iend; i++)
{
//所以定义的链表中权重由大到小排列要用push_front
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
//更新排序好的连接关键帧及其对应的权重
mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}
3) UpdateConnections:更新连接
该函数主要包含以下三部分内容:
a. 首先获得该关键帧的所有MapPoint点,然后遍历观测到这些3d点的其它所有关键帧,对每一个找到的关键帧,先存储到相应的容器中。
b. 计算所有共视帧与该帧的连接权重,权重即为共视的3d点的数量,对这些连接按照权重从大到小进行排序。当该权重必须大于一个阈值,便在两帧之间建立边,如果没有超过该阈值的权重,那么就只保留权重最大的边(与其它关键帧的共视程度比较高)。
c. 更新covisibility graph,即把计算的边用来给图赋值,然后设置spanning tree中该帧的父节点,即共视程度最高的那一帧。
void KeyFrame::UpdateConnections()
{
// 在没有执行这个函数前,关键帧只和MapPoints之间有连接关系,这个函数可以更新关键帧之间的连接关系
//===============对应a部分内容==================================
map<KeyFrame *, int> KFcounter;
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
// 即统计每一个关键帧都有多少关键帧与它存在共视关系,统计结果放在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;
//===============对应b部分内容==================================
//If the counter is greater than threshold add connection
//In case no keyframe counter is over threshold add the one with maximum counter
// 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
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);
}
//===============对应c部分内容==================================
{
unique_lock<mutex> lockCon(mMutexConnections);
// mspConnectedKeyFrames = spConnectedKeyFrames;
// 更新图的连接(权重)
mConnectedKeyFrameWeights = KFcounter;
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;
}
}
}
4) EraseConnection:移除连接
清楚一个关键帧与其他帧对应的边
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();
}
5) SetBadFlag:删除与该帧相关的所有连接关系
需要删除的是该关键帧和其他所有帧、地图点之间的连接关系,但是删除会带来一个问题,就是它可能是其他节点的父节点,在删除之前需要告诉自己所有的子节点,换个爸爸,这个函数里绝大部分代码都是在完成这一步。有一片博客对这一函数的步骤讲解比较清晰,个人也比较认同,所以借鉴过来(博客链接:https://blog.csdn.net/weixin_39373577/article/details/85226187):
步骤一:遍历所有和当前关键帧共视的关键帧,删除他们与当前关键帧的联系。
步骤二:遍历每一个当前关键帧的地图点,删除每一个地图点和当前关键帧的联系。
步骤三:清空和当前关键帧的共视关键帧集合和带顺序的关键帧集合。
步骤四:共视图更新完毕后,还需要更新生成树。这个比较难理解。。。真实删除当前关键帧之前,需要处理好父亲和儿子关键帧关系,不然会造成整个关键帧维护的图断裂,或者混乱,不能够为后端提供较好的初值(理解起来就是父亲挂了,儿子需要找新的父亲,在候选父亲里找,当前帧的父亲肯定在候选父亲中)。
步骤五:遍历所有把当前关键帧当成父关键帧的子关键帧。重新为他们找父关键帧。设置一个候选父关键帧集合(集合里包含了当前帧的父帧和子帧?)
步骤六:对于每一个子关键帧,找到与它共视的关键帧集合,遍历它,看看是否有候选父帧集合里的帧,如果有,就把这个帧当做新的父帧。
步骤七:如果有子关键帧没有找到新的父帧,那么直接把当前帧的父帧(爷)当成它的父帧
配合下图可以看得更清楚:
4. spanning tree 相关
这一类函数所有的操作都是在围绕自己的子节点和父节点,其中子节点可能有多个,所以是一个容器mspChildrens,父节点只能有一个,所以是个变量mpParent。函数内容都比较简单,在此一并列出
void KeyFrame::AddChild(KeyFrame *pKF) //增加子树
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.insert(pKF);
}
void KeyFrame::EraseChild(KeyFrame *pKF) //删除子树
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.erase(pKF);
}
void KeyFrame::ChangeParent(KeyFrame *pKF) //更换父节点
{
unique_lock<mutex> lockCon(mMutexConnections);
mpParent = pKF;
pKF->AddChild(this);
}
set<KeyFrame *> KeyFrame::GetChilds() //获取所有子节点
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens;
}
KeyFrame *KeyFrame::GetParent() //获取父节点
{
unique_lock<mutex> lockCon(mMutexConnections);
return mpParent;
}
bool KeyFrame::hasChild(KeyFrame *pKF) //判断是否有子节点
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens.count(pKF);
}
5. MapPoint相关
这一类函数的内容同样比较j简单,主要围绕存放MapPoint的容器mvpMapPoints进行。在此一并列出这些函数
void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)//增加MapPoint
{
unique_lock<mutex> lock(mMutexFeatures);
mvpMapPoints[idx] = pMP;
}
void KeyFrame::EraseMapPointMatch(const size_t &idx)//删除MapPoint
{
unique_lock<mutex> lock(mMutexFeatures);
mvpMapPoints[idx] = static_cast<MapPoint *>(NULL);
}
void KeyFrame::EraseMapPointMatch(MapPoint *pMP)//删除MapPoint的匹配关系
{
int idx = pMP->GetIndexInKeyFrame(this);
if (idx >= 0)
mvpMapPoints[idx] = static_cast<MapPoint *>(NULL);
}
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint *pMP)//替换MapPoint的匹配关系
{
mvpMapPoints[idx] = pMP;
}
set<MapPoint *> KeyFrame::GetMapPoints()//获取所有MapPoint
{
unique_lock<mutex> lock(mMutexFeatures);
set<MapPoint *> s;
for (size_t i = 0, iend = mvpMapPoints.size(); i < iend; i++)
{
if (!mvpMapPoints[i])
continue;
MapPoint *pMP = mvpMapPoints[i];
if (!pMP->isBad())
s.insert(pMP);
}
return s;
}
//获取被观测相机数大于等于minObs的MapPoint
int KeyFrame::TrackedMapPoints(const int &minObs)
{
unique_lock<mutex> lock(mMutexFeatures);
int nPoints = 0;
const bool bCheckObs = minObs > 0;
for (int i = 0; i < N; i++)
{
MapPoint *pMP = mvpMapPoints[i];
if (pMP)
{
if (!pMP->isBad())
{
if (bCheckObs)
{
if (mvpMapPoints[i]->Observations() >= minObs)
nPoints++;
}
else
nPoints++;
}
}
}
return nPoints;
}
6. 其他类
1)ComputeSceneMedianDepth:计算场景中的中位深度
步骤一:获取每个地图点的世界位姿
步骤二:找出当前帧Z方向上的旋转和平移,求每个地图点在当前相机坐标系中的z轴位置,求平均值。
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];
}
评论(0)
您还未登录,请登录后发表或查看评论