ORB-SLAM2 --- KeyFrame::UpdateConnections 函数

最新推荐文章于 2025-02-03 07:55:41 发布

APS2023

最新推荐文章于 2025-02-03 07:55:41 发布

阅读量817

点赞数 1

CC 4.0 BY-SA版权

分类专栏：视觉SLAM - ORBSLAM2 原理解析文章标签：算法人工智能 c++ slam

本文链接：https://blog.youkuaiyun.com/qq_41694024/article/details/128516249

视觉SLAM - ORBSLAM2 原理解析专栏收录该内容

66 篇文章

订阅专栏

该函数主要更新关键帧间的连接图，统计每个关键帧与当前关键帧的共视程度，建立边并按权重排序。超过共视阈值的关键帧会被加入连接列表，同时更新最大共视关键帧作为父关键帧，并建立子关键帧的双向连接。该过程涉及关键帧、地图点、权重计算和排序策略。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

一、函数作用

更新关键帧之间的连接图。

更新变量

@mConnectedKeyFrameWeights：当前关键帧的共视信息，记录当前关键帧共视关键帧的信息（哪一帧和当前关键帧有共视，共视程度是多少）

@mvpOrderedConnectedKeyFrames：对mConnectedKeyFrameWeights中超过共视阈值的关键帧（按照共视程度从大到小排序）

@mvOrderedWeights：对mConnectedKeyFrameWeights中超过共视阈值的关键帧的共视程度（按照共视程度从大到小排序）

@mpParent：共视程度最高的那个关键帧

@mspChildrens：建立双向关系，将当前帧的共视程度最高的那个关键帧的子关键帧添加当前帧

二、函数流程

1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
3. 对这些连接按照权重从大到小进行排序，以方便将来的处理。更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树。

三、code

void KeyFrame::UpdateConnections()
{
    // 关键帧-权重，权重为其它关键帧与当前关键帧共视地图点的个数，也称为共视程度
    map<KeyFrame*,int> KFcounter; 
    vector<MapPoint*> vpMP;

    {
        // 获得该关键帧的所有地图点
        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
    // Step 1 通过地图点被关键帧观测来间接统计关键帧之间的共视程度
    // 统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter
    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;
            // 这里的操作非常精彩！
            // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对
            // mit->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数
            // 所以最后KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度
            KFcounter[mit->first]++;
        }
    }

    // This should not happen
    // 没有共视关系，直接退出 
    if(KFcounter.empty())
        return;

    // 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;
    // 至少有15个共视地图点才会添加共视关系
    int th = 15;

    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    // Step 2 找到对应权重最大的关键帧（共视程度最高的关键帧）
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }

        // 建立共视关系至少需要大于等于th个共视地图点
        if(mit->second>=th)
        {
            // 对应权重需要大于阈值，对这些关键帧建立连接
            vPairs.push_back(make_pair(mit->second,mit->first));
            // 对方关键帧也要添加这个信息
            // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
            // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);
        }
    }

    //  Step 3 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
    if(vPairs.empty())
    {
	    // 如果每个关键帧与它共视的关键帧的个数都少于th，
        // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
        // 这是对之前th这个阈值可能过高的一个补丁
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

    //  Step 4 对满足共视程度的关键帧对更新连接关系及权重（从大到小）
    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序
    sort(vPairs.begin(),vPairs.end());         // sort函数默认升序排列
    // 将排序后的结果分别组织成为两种数据类型
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        // push_front 后变成了从大到小顺序
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
        // 更新当前帧与其它关键帧的连接权重
        // ?bug 这里直接赋值，会把小于阈值的共视关系也放入mConnectedKeyFrameWeights，会增加计算量
        // ?但后续主要用mvpOrderedConnectedKeyFrames来取共视帧，对结果没影响
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

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

四、函数解析

先获得该关键帧的所有地图点vpMP = mvpMapPoints，用vpMP变量存储。

遍历该关键帧的所有地图点。对于每一个地图点，observations记录了可以观测到该地图点的所有关键帧。observations的类型为 map<KeyFrame*,size_t>，它存储着可以看到该地图点的所有关键帧KeyFrame和该地图点在该关键帧中的索引size_t。

遍历observations变量获得KFcounter。首先判断该帧的ID是不是调用该函数的帧ID，因为自己和自己不存在共视关系。我们遍历observations中每一帧，因为该帧和当前帧（调用这个函数的帧）存在共视地图点，我们将KFcounter[该帧]++，表示这一帧和调用函数的帧的共视关系。最终得到的KFcounter变量是[关键帧，int]的形式，对应了与当前关键帧共视关键帧的共视程度。

用nmax变量记录最高的共视程度，设置添加共视关系的阈值th。

遍历当前帧的共视关系向量KFcounter：

得到与当前关键帧最大的共视程度的关键帧pKFmax及共视程度nmax。

同时，若共视程度大于阈值th，则将该关键帧与共视程度放入变量vector<pair<int,KeyFrame*> > vPairs中，并更新当前帧的共视帧与当前帧的共视程度。

ORB-SLAM2 --- KeyFrame::AddConnection函数解析https://blog.youkuaiyun.com/qq_41694024/article/details/128537286

        如果没有超过阈值th的权重，则对权重最大的关键帧建立连接。

        对满足共视程度的关键帧对更新连接关系及权重（从大到小）：

vPairs里面的内容是和当前局部建图线程调用帧的共视程度超过我们指定阈值th的关键帧和共视程度。

我们将共视程度和关键帧分开存储在两个列表容器lKFs、lWs中并按照共视程度从大到小排序。

最后我们更新当前帧与其它关键帧的连接权重：

将共视关系存储给我们更新的最终变量：

将KFcounter赋值给mConnectedKeyFrameWeights，存储着当前局部线程调用关键帧的共视关键帧及共视程度。

将lKFs赋值给mvpOrderedConnectedKeyFrames，存储着与当前局部线程调用关键帧的共视关键帧信息（按共视程度从大到小）。（相比于mConnectedKeyFrameWeights中存储的信息，这里的关键帧与局部线程调用关键帧的共视程度大于阈值th）

        将lWs赋值给mvOrderedWeights，存储着与当前局部线程调用关键帧的共视关键帧的共视程度信息（按共视程度从大到小）。（相比于mConnectedKeyFrameWeights中存储的信息，这里的关键帧与局部线程调用关键帧的共视程度大于阈值th）

最后我们更新生成树的连接：

        初始化该关键帧的父关键帧mpParent为共视程度最高的那个关键帧，建立双向连接关系，将当前关键帧作为其子关键帧。将是否是第一次生成标志位mbFirstConnection设为false。
// 添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧）
void KeyFrame::AddChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.insert(pKF);
}
    bool mbFirstConnection;                     // 是否是第一次生成树
    KeyFrame* mpParent;                         // 当前关键帧的父关键帧 （共视程度最高的）
    std::set<KeyFrame*> mspChildrens;           // 存储当前关键帧的子关键帧