ORB-SLAM2代码笔记（三）：KeyFrame

2019-07-20 本文已影响0人 liampayne_66d0

KeyFrame

由于KeyFrame中一部分数据会被多个线程访问修改，因此需要在这些成员中加线程锁，保证同一时间只有一个线程有访问权。涉及线程安全的有：

关键帧位姿的设置（lock(mMutexPose)）；

关键帧间连接关系的设置（lock(mMutexConnections)）；

关键帧对应地图点的操作（lock(mMutexFeatures)），

void KeyFrame::SetPose(const cv::Mat &Tcw_)
设置当前关键帧的位姿

KeyFrame::UpdateConnections()更新图的连接

首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键与其它所有关键帧之间的共视程度，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。

for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)//遍历3D点
{
    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]++;
    }
}

权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）

    // 对于一个和当前关键帧具有共视关系的关键帧
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));
        (mit->first)->AddConnection(this,mit->second);
    }
}
// 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
if(vPairs.empty())
{
    mConnectedKeyFrameWeights
    // 这是对之前th这个阈值可能过高的一个补丁
    vPairs.push_back(make_pair(nmax,pKFmax));
    pKFmax->AddConnection(this,nmax);
}

对这些连接按照权重从大到小进行排序，以方便将来的处理更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树

void KeyFrame::UpdateBestCovisibles()按照权重对连接的关键帧进行排序
这里声明了一个变量vector <pair<int,KeyFrame*> > vPairs
和mConnectedKeyFrameWeights的区别是吧int放在前面，方便排序
set<KeyFrame*> KeyFrame::GetConnectedKeyFrames()得到与该关键帧连接的关键帧，这里直接遍历mConnectedKeyFrameWeights，把关键帧提出来
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
得到与该关键帧连接的关键帧(已按权值排序)
直接return mvpOrderedConnectedKeyFrames;这一变量在UpdateBestCovisibles()算好了
vector<KeyFrame> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)
得到与该关键帧连接的前N个关键帧(已按权值排序)
vector<KeyFrame> KeyFrame::GetCovisiblesByWeight(const int &w)
权重大于等于w的关键帧
set<MapPoint*> KeyFrame::GetMapPoints()获取当前关键帧中的所有地图点
直接遍历mvpMapPoints就好了
int KeyFrame::TrackedMapPoints(const int &minObs)关键帧中，大于等于最少观测数目minObs的MapPoints的数量.这些特征点被认为追踪到了
还有添加子关键帧，删除子关键帧，改变当前关键帧的父关键帧，获取当前关键帧的子关键帧和父关键帧等
void KeyFrame::SetBadFlag() 删除关键帧
- 要删除其他KeyFrame和自己的联系，删除地图点和自己的联系。
- 这个函数中比较复杂的是给子关键帧选择父关键帧:

如果这个关键帧有自己的孩子关键帧，告诉这些子关键帧，它们的父关键帧不行了，赶紧找新的父关键帧。
1. 遍历每一个子关键帧，让它们更新它们指向的父关键帧
2. 子关键帧遍历每一个与它相连的关键帧（共视关键帧）
3. 如果子节点找到了可以作为其新父关键帧的帧，则更换父节点
4. 如果没找到，直接把父节点的父节点作为自己的父节点

ORB-SLAM2代码笔记（三）：KeyFrame

KeyFrame

猜你喜欢

热点阅读