ORB-SLAM2算法10之图像关键帧KeyFrame

news2024/9/25 9:36:42

文章目录

  • 0 引言
  • 1 KeyFrame类
    • 1.1 构造函数
    • 1.2 成员函数
    • 1.3 关键帧之间共视图
      • 1.3.1 AddConnection
      • 1.3.2 UpdateBestCovisibles
      • 1.3.3 UpdateConnections
      • 1.3.4 EraseConnection
      • 1.3.5 SetBadFlag
    • 1.4 地图点
    • 1.5 生成树
  • 2 KeyFrame用途

0 引言

ORB-SLAM2算法7详细了解了System主类和多线程、ORB-SLAM2算法8详细了解了图像特征点提取和描述子的生成及ORB-SLAM2算法9详细了解了图像帧，本文在此基础上，继续学习ORB-SLAM2中的图像关键帧，也就是KeyFrame类，该类中主要包含关键帧姿态计算、关键帧之间共视图、生成树及地图点操作等函数。

请添加图片描述

1 KeyFrame类

1.1 构造函数

KeyFrame的构造函数输入普通图像帧，地图对象和关键帧数据库对象三个参数，而且对成员变量进行初始化，比如帧的ID，时间戳，相机内参，位姿，特征点，描述子等，还复制了帧对象中的网格数据，以用于加速特征点的匹配，最后还调用了SetPose()函数，用于设置关键帧的位姿。

KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
    mnFrameId(F.mnId),                          // 当前帧的ID
    mTimeStamp(F.mTimeStamp),                    // 当前帧的时间戳
    mnGridCols(FRAME_GRID_COLS),                  // 网格的列数
    mnGridRows(FRAME_GRID_ROWS),                  // 网格的行数
    mfGridElementWidthInv(F.mfGridElementWidthInv),  // 网格元素的宽度的倒数
    mfGridElementHeightInv(F.mfGridElementHeightInv),// 网格元素的高度的倒数
    mnTrackReferenceForFrame(0),                 // 当前关键帧被其他关键帧引用的次数
    mnFuseTargetForKF(0),                        // 当前关键帧作为融合目标的次数
    mnBALocalForKF(0),                           // 当前关键帧作为局部BA的次数
    mnBAFixedForKF(0),                           // 当前关键帧作为固定BA的次数
    mnLoopQuery(0),                              // 当前关键帧作为回环查询的次数
    mnLoopWords(0),                              // 当前关键帧回环匹配的单词数
    mnRelocQuery(0),                             // 当前关键帧作为重定位查询的次数
    mnRelocWords(0),                             // 当前关键帧重定位匹配的单词数
    mnBAGlobalForKF(0),                          // 当前关键帧作为全局BA的次数
    fx(F.fx),                                    // 相机的焦距x
    fy(F.fy),                                    // 相机的焦距y
    cx(F.cx),                                    // 相机的光心x
    cy(F.cy),                                    // 相机的光心y
    invfx(F.invfx),                              // 焦距x的倒数
    invfy(F.invfy),                              // 焦距y的倒数
    mbf(F.mbf),                                  // 基线乘以焦距的值
    mb(F.mb),                                    // 相机的基线长度
    mThDepth(F.mThDepth),                        // 深度值的阈值
    N(F.N),                                      // 特征点的数量
    mvKeys(F.mvKeys),                            // 特征点的像素坐标
    mvKeysUn(F.mvKeysUn),                        // 特征点的归一化坐标
    mvuRight(F.mvuRight),                        // 右目特征点的像素坐标
    mvDepth(F.mvDepth),                          // 特征点的深度值
    mDescriptors(F.mDescriptors.clone()),        // 特征点的描述子
    mBowVec(F.mBowVec),                          // 特征点的词袋表示
    mFeatVec(F.mFeatVec),                        // 特征点的尺度金字塔信息
    mnScaleLevels(F.mnScaleLevels),              // 尺度金字塔的层数
    mfScaleFactor(F.mfScaleFactor),              // 尺度金字塔的尺度因子
    mfLogScaleFactor(F.mfLogScaleFactor),        // 尺度金字塔的尺度因子的对数值
    mvScaleFactors(F.mvScaleFactors),            // 尺度金字塔每层的尺度因子
    mvLevelSigma2(F.mvLevelSigma2),              // 尺度金字塔每层的尺度值的平方
    mvInvLevelSigma2(F.mvInvLevelSigma2),        // 尺度金字塔每层尺度的平方的倒数
    mnMinX(F.mnMinX),                            // 特征点的最小x坐标
    mnMinY(F.mnMinY),                            // 特征点的最小y坐标
    mnMaxX(F.mnMaxX),                            // 特征点的最大x坐标
    mnMaxY(F.mnMaxY),                            // 特征点的最大y坐标
    mK(F.mK),                                    // 相机的内参矩阵
    mvpMapPoints(F.mvpMapPoints),                // 关联的地图点的指针
    mpKeyFrameDB(pKFDB),                         // 关键帧数据库指针
    mpORBvocabulary(F.mpORBvocabulary),          // ORB词袋模型的指针
    mbFirstConnection(true),                     // 是否是第一个与其他关键帧连接的关键帧
    mpParent(NULL),                              // 父关键帧的指针
    mbNotErase(false),                           // 是否不被删除
    mbToBeErased(false),                         // 是否待删除
    mbBad(false),                                // 是否是坏关键帧
    mHalfBaseline(F.mb/2),                       // 基线长度的一半
    mpMap(pMap)                                  // 关联的地图指针
{
    mnId = nNextId++;                            // 关键帧的唯一标识符

    // 复制网格数据，用于加速匹配
    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);                             // 设置关键帧的位姿
}

1.2 成员函数

KeyFrame类中的成员函数一览表：

成员函数类型定义
void KeyFrame::ComputeBoW()public计算词袋表示
void KeyFrame::SetPose(const cv::Mat &Tcw_)public设置当前关键帧的位姿
cv::Mat KeyFrame::GetPose()public获取位姿
cv::Mat KeyFrame::GetPoseInverse()public获取位姿的逆
cv::Mat KeyFrame::GetCameraCenter()public获取（左目）相机的中心在世界坐标系下的坐标
cv::Mat KeyFrame::GetStereoCenter()public获取双目相机的中心，这个只有在可视化时才会用到
cv::Mat KeyFrame::GetRotation()public获取姿态，旋转矩阵
cv::Mat KeyFrame::GetTranslation()public获取位置，平移矩阵
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)public为当前关键帧新建或更新和其他关键帧的连接权重
void KeyFrame::UpdateBestCovisibles()public按照权重从大到小对连接（共视）的关键帧进行排序
set<KeyFrame*> KeyFrame::GetConnectedKeyFrames()public得到与该关键帧连接（大于15个共视地图点）的关键帧（没有排序的）
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()public得到与该关键帧连接的关键帧（已按权值排序）
vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)public得到与该关键帧连接的前N个最强共视关键帧（已按权值排序）
vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)public得到与该关键帧连接的权重超过w的关键帧
int KeyFrame::GetWeight(KeyFrame *pKF)public得到该关键帧与pKF的权重
void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)public添加地图点到关键帧
void KeyFrame::EraseMapPointMatch(const size_t &idx);void KeyFrame::EraseMapPointMatch(MapPoint* pMP)public删除bad==true的地图点，将该地图点置为NULL
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)public地图点的替换
set<MapPoint*> KeyFrame::GetMapPoints()public获取当前关键帧中的所有地图点
int KeyFrame::TrackedMapPoints(const int &minObs)public关键帧中，大于等于最小观测数目minObs的MapPoints的数量，这些特征点被认为追踪到了
vector<MapPoint*> KeyFrame::GetMapPointMatches();MapPoint* KeyFrame::GetMapPoint(const size_t &idx)public获取当前关键帧的具体的地图点
void KeyFrame::UpdateConnections()public更新关键帧之间的连接图
void KeyFrame::AddChild(KeyFrame *pKF)public添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧）
void KeyFrame::EraseChild(KeyFrame *pKF)public删除某个子关键帧
void KeyFrame::ChangeParent(KeyFrame *pKF)public改变当前关键帧的父关键帧
set<KeyFrame*> KeyFrame::GetChilds()public获取当前关键帧的子关键帧
KeyFrame* KeyFrame::GetParent()public获取当前关键帧的父关键帧
bool KeyFrame::hasChild(KeyFrame *pKF)public判断某个关键帧是否是当前关键帧的子关键帧
void KeyFrame::AddLoopEdge(KeyFrame *pKF)public给当前关键帧添加回环边，回环边连接了形成了闭环关系的关键帧
set<KeyFrame*> KeyFrame::GetLoopEdges()public获取和当前关键帧形成闭环关系的关键帧
void KeyFrame::SetNotErase()public设置当前关键帧不要在优化的过程中被删除，由回环检测线程调用
void KeyFrame::SetErase()public删除当前的这个关键帧，表示不进行回环检测过程，由回环检测线程调用
void KeyFrame::SetBadFlag()public真正地执行删除关键帧的操作
bool KeyFrame::isBad()public判断关键帧是否是无效的
void KeyFrame::EraseConnection(KeyFrame* pKF)public删除当前关键帧和指定关键帧之间的共视关系
vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) constpublic获取某个特征点的邻域中的特征点id，类比Frame类的函数
bool KeyFrame::IsInImage(const float &x, const float &y) constpublic判断某个点是否在当前关键帧的图像中
cv::Mat KeyFrame::UnprojectStereo(int i)public在双目和RBGD情况下将特征点反投影到空间中得到世界坐标系下三维点
float KeyFrame::ComputeSceneMedianDepth(const int q)public评估当前关键帧场景深度，q=2表示中值，只是在单目情况下才会使用

成员函数比较多，有些函数功能虽不可少但比较简单，以下重点围绕关键帧之间共视关系Covisibility Graph、生成树Spanning tree和地图点MapPoint这三部分重点学习。

而实际上主要就是为了图优化，图优化需要构建节点和边，节点即关键帧的位姿，位姿的相关函数这里就不细说了，而边有两种：

  1. 和其他关键帧之间的边，需要通过MapPoint产生联系，两帧都能够共同观测到一定数量的MapPoint，建立共视关系，所以需要有管理关键帧之间共视关系的函数；（详见1.3）
  2. 和MapPoint之间的边， 所以也需要管理和MapPoint之间关系的函数；（详见1.4）

至于生成树，由于和其他关键帧之间的边仍旧有很多，为了简化并提高计算速度，ORB-SLAM2中的通过生成树来管理各关键帧之间的关系，设定每个帧都有一个父节点和子节点，节点是其他关键帧，在构建优化模型时，只有具有父子关系的关键帧之间才能建立边，这样能大大减少边的数量，因此，还需要管理生成树的函数。（详见1.5）

1.3 关键帧之间共视图

ORB-SLAM论文中有张图能比较清晰地展示关键帧，地图点，共视图，生成树等关系：

  • 图a绿色表示当前相机，蓝色表示关键帧，红色和黑色表示地图点；
  • 图b绿色即是共视图Covisibility graph，共视图用来描述不同关键帧可以看到多少相同的地图点：每个关键帧是一个节点，如果两个关键帧之间的共视地图点数量大于15，则两个节点建立边；
  • 图c绿色即是生成树Spanning tree，生成树保留了所有关键帧的节点，但给各个关键帧设定了父节点和子节点，每帧只跟各自的父节点和子节点相连；
  • 图d绿色即是本质图essential graph，是根据生成树建立的图模型，简化版的共视图。

请添加图片描述

1.3.1 AddConnection

该函数主要目的是新建或更新关键帧之间的连接权重：

  • 输入参数：pKF是当前关键帧共视的其他关键帧
  • 输入参数：weight是当前关键帧和其他关键帧的权重（共视地图点数目）
// 为当前关键帧新建或更新和其他关键帧的连接权重

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        // 互斥锁，防止同时操作共享数据产生冲突
        unique_lock<mutex> lock(mMutexConnections);

        // 新建或更新连接权重
        if(!mConnectedKeyFrameWeights.count(pKF)) 
            // count函数返回0，说明mConnectedKeyFrameWeights中没有pKF，新建连接
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight) 
            // 之前连接的权重不一样了，需要更新
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;
    }

    // 连接关系变化就要更新最佳共视，主要是重新进行排序
    UpdateBestCovisibles();
}

1.3.2 UpdateBestCovisibles

该函数主要目的是按照连接权重weight对连接的关键帧进行排序，按权重大小从大到小排列，更新后的变量存储在mvpOrderedConnectedKeyFrames 和 mvOrderedWeights中。

// 按照权重从大到小对连接（共视）的关键帧进行排序

void KeyFrame::UpdateBestCovisibles()
{
    // 互斥锁，防止同时操作共享数据产生冲突
    unique_lock<mutex> lock(mMutexConnections);
    // http://stackoverflow.com/questions/3389648/difference-between-stdliststdpair-and-stdmap-in-c-stl (std::map 和 std::list<std::pair>的区别)
    
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    // 取出所有连接的关键帧，mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>，而vPairs变量将共视的地图点数放在前面，利于排序
    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;   // 所有连接关键帧
    list<int> lWs;          // 所有连接关键帧对应的权重（共视地图点数目）
    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());
    // 从大到小排列的权重，和mvpOrderedConnectedKeyFrames一一对应
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

1.3.3 UpdateConnections

该函数主要目的是更新关键帧之间的连接图，主要分为三步：

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

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

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

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

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;
        }
    }
}

1.3.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();
}

1.3.5 SetBadFlag

该函数的主要目的真正地执行删除关键帧及相关的共视图和生成树，但是直接删除会引入一些问题，比如删除了一个父节点，那和它相关的子节点怎么办？ORB-SLAM2中是直接给这些子节点换个父节点。详细的步骤如下：

  1. 首先处理一下删除不了的特殊情况，比如第0帧不允许被删除；
  2. 遍历所有和当前关键帧相连的关键帧，删除他们与当前关键帧的联系；
  3. 遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系；
  4. 更新生成树，主要是处理好父子关键帧，不然会造成整个关键帧维护的图断裂或混乱；
  5. 遍历每一个子关键帧，让它们更新它们指向的父关键帧；
  6. 子关键帧遍历每一个与它共视的关键帧；
  7. sParentCandidates 中刚开始存的是这里子关键帧的“爷爷”，也是当前关键帧的候选父关键帧，如果孩子和sParentCandidates中有共视，选择共视最强的那个作为新的父节点；（可以理解成爷爷变父亲了）

请添加图片描述

// 真正地执行删除关键帧的操作

void KeyFrame::SetBadFlag()
{   
    // Step 1 首先处理一下删除不了的特殊情况
    {
        unique_lock<mutex> lock(mMutexConnections);

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

    // Step 2 遍历所有和当前关键帧相连的关键帧，删除他们与当前关键帧的联系
    for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
        mit->first->EraseConnection(this); // 让其它的关键帧删除与自己的联系

    // Step 3 遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系
    for(size_t i=0; i<mvpMapPoints.size(); i++)
        if(mvpMapPoints[i])
            mvpMapPoints[i]->EraseObservation(this); 

    {
        unique_lock<mutex> lock(mMutexConnections);
        unique_lock<mutex> lock1(mMutexFeatures);

        // 清空自己与其它关键帧之间的联系
        mConnectedKeyFrameWeights.clear();
        mvpOrderedConnectedKeyFrames.clear();

        // Update Spanning Tree 
        // Step 4 更新生成树，主要是处理好父子关键帧，不然会造成整个关键帧维护的图断裂，或者混乱
        // 候选父关键帧
        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;

            // Step 4.1 遍历每一个子关键帧，让它们更新它们指向的父关键帧
            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
                // Step 4.2 子关键帧遍历每一个与它共视的关键帧    
                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++)
                    {
                        // Step 4.3 如果孩子和sParentCandidates中有共视，选择共视最强的那个作为新的父
                        if(vpConnected[i]->mnId == (*spcit)->mnId)
                        {
                            int w = pKF->GetWeight(vpConnected[i]);
                            // 寻找并更新权值最大的那个共视关系
                            if(w>max)
                            {
                                pC = pKF;                   //子关键帧
                                pP = vpConnected[i];        //目前和子关键帧具有最大权值的关键帧（将来的父关键帧） 
                                max = w;                    //这个最大的权值
                                bContinue = true;           //说明子节点找到了可以作为其新父关键帧的帧
                            }
                        }
                    }
                }
            }

            // Step 4.4 如果在上面的过程中找到了新的父节点
            // 下面代码应该放到遍历子关键帧循环中?
            // 回答：不需要！这里while循环还没退出，会使用更新的sParentCandidates
            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
        // Step 4.5 如果还有子节点没有找到新的父节点
        if(!mspChildrens.empty())
            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
            {
                // 直接把父节点的父节点作为自己的父节点 即对于这些子节点来说,他们的新的父节点其实就是自己的爷爷节点
                (*sit)->ChangeParent(mpParent);
            }

        mpParent->EraseChild(this);
        // mTcp 表示原父关键帧到当前关键帧的位姿变换，在保存位姿的时候使用
        mTcp = Tcw*mpParent->GetPoseInverse();
        // 标记当前关键帧已经挂了
        mbBad = true;
    }  

    // 地图和关键帧数据库中删除该关键帧
    mpMap->EraseKeyFrame(this);
    mpKeyFrameDB->erase(this);
}

1.4 地图点

和地图点相关的函数，主要是针对存放MapPoint的容器mvpMapPoint进行的，比如新增，删除，替换等操作。

// Add MapPoint to KeyFrame
// 新增 MapPoint
void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=pMP;
}
/**
 * @brief 由于其他的原因,导致当前关键帧观测到的某个地图点被删除(bad==true)了,将该地图点置为NULL
 * 
 * @param[in] idx   地图点在该关键帧中的id
 */
void KeyFrame::EraseMapPointMatch(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    // NOTE 使用这种方式表示其中的某个地图点被删除
    mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}

// 同上
void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
{
    //获取当前地图点在某个关键帧的观测中，对应的特征点的索引，如果没有观测，索引为-1
    int idx = pMP->GetIndexInKeyFrame(this);
    if(idx>=0)
        mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}

// 地图点的替换
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
{
    mvpMapPoints[idx]=pMP;
}

// 获取当前关键帧中的所有地图点
set<MapPoint*> KeyFrame::GetMapPoints()
{
    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的MapPoints的数量.这些特征点被认为追踪到了
int KeyFrame::TrackedMapPoints(const int &minObs)
{
    unique_lock<mutex> lock(mMutexFeatures);

    int nPoints=0;
    // 是否检查数目
    const bool bCheckObs = minObs>0;
    // N是当前帧中特征点的个数
    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = mvpMapPoints[i];
        if(pMP)     //没有被删除
        {
            if(!pMP->isBad())   //并且不是坏点
            {
                if(bCheckObs)
                {
                    // 满足输入阈值要求的地图点计数加1
                    if(mvpMapPoints[i]->Observations()>=minObs)
                        nPoints++;
                }
                else
                    nPoints++; //!bug
            }
        }
    }

    return nPoints;
}

// 获取当前关键帧的具体的地图点
vector<MapPoint*> KeyFrame::GetMapPointMatches()
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints;
}

// 获取当前关键帧的具体的某个地图点
MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints[idx];
}

另外，还需要关注上述函数的调用时机，即关键帧何时与地图点发生关系：

  • 关键帧增加对地图点观测的时机：
  1. Tracking线程和LocalMapping线程创建新地图点后,会马上调用函数KeyFrame::AddMapPoint()添加当前关键帧对该地图点的观测.
  2. LocalMapping线程处理完毕缓冲队列内所有关键帧后会调用LocalMapping::SearchInNeighbors()融合当前关键帧和共视关键帧间的重复地图点,其中调用函数ORBmatcher::Fuse()实现融合过程中会调用函数KeyFrame::AddMapPoint()；
  3. LoopClosing线程闭环矫正函数LoopClosing::CorrectLoop()将闭环关键帧与其匹配关键帧间的地图进行融合,会调用函数KeyFrame::AddMapPoint()；
  • 关键帧替换和删除对地图点观测的时机：
  1. MapPoint删除函数MapPoint::SetBadFlag()或替换函数MapPoint::Replace()会调用KeyFrame::EraseMapPointMatch()和KeyFrame::ReplaceMapPointMatch()删除和替换关键针对地图点的观测；
  2. LocalMapping线程调用进行局部BA优化的函数Optimizer::LocalBundleAdjustment()内部调用函数KeyFrame::EraseMapPointMatch()删除对重投影误差较大的地图点的观测。

1.5 生成树

和生成树相关的函数，主要操作时围绕自己的子节点和父节点，其中子节点有多个，即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);
}

2 KeyFrame用途

如上所述，在ORB-SLAM2中，关键帧Keyframes是系统中的重要组成部分，具有以下作用和用途：

  1. 地图构建：关键帧用于构建场景的三维地图。它们通过提取关键点、计算特征描述子和建立特征点之间的匹配关系来捕获场景的结构和几何信息。关键帧之间的匹配信息被用于三角化重建场景中的特征点，并估计相机的运动和场景的几何结构。

  2. 定位：关键帧用于相机的实时定位。当新的图像帧进入系统时，ORB-SLAM2会与之前的关键帧进行匹配，通过计算特征点之间的匹配关系和求解相机位姿来估计相机的当前位置。关键帧中保存的地图信息能够提供更好的定位精度和鲁棒性。

  3. 回环检测：关键帧用于检测环回（Loop Closure）事件，即相机在场景中经过一段时间后再次经过之前的位置。ORB-SLAM2使用关键帧之间的特征匹配来检测回环，并利用回环信息进行地图的优化和校正，提高系统的一致性和鲁棒性。

  4. 关键帧选择：ORB-SLAM2使用一种自适应的关键帧选择策略，根据一些准则选择最具代表性和信息丰富的关键帧进行处理。这可以减少计算复杂性，提高系统的实时性能。

总之，关键帧在ORB-SLAM2系统中扮演着重要的角色，用于场景重建、定位、回环检测和关键帧选择，使系统能够实时地感知和理解环境，提供稳定和精确的定位和地图重建能力。


Reference:

  • https://github.com/raulmur/ORB_SLAM2
  • https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master
  • http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO15.pdf
  • https://blog.csdn.net/ncepu_Chen/article/details/116784875#t5
  • https://zhuanlan.zhihu.com/p/84293190



须知少时凌云志，曾许人间第一流。



⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/921934.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

机器学习实战之模型的解释性：Scikit-Learn的SHAP和LIME库详解

机器学习实战之模型的解释性：Scikit-Learn的SHAP和LIME库详解

引言&#xff1a;机器学习模型的“黑箱”困境 机器学习模型的崛起让我们惊叹不已&#xff01;不论是预测房价、识别图片中的猫狗&#xff0c;还是推荐给你喜欢的音乐&#xff0c;这些模型都表现得非常出色。但是&#xff0c;有没有想过&#xff0c;这些模型到底是如何做出这些决…
阅读更多...
STM32 F103C8T6学习笔记13：IIC通信—AHT10温湿度传感器模块

STM32 F103C8T6学习笔记13：IIC通信—AHT10温湿度传感器模块

今日学习一下这款AHT10 温湿度传感器模块&#xff0c;给我的OLED手环添加上测温湿度的功能。 文章提供源码、测试工程下载、测试效果图。 目录 AHT10温湿度传感器&#xff1a; 特性&#xff1a; 连接方式&#xff1a; 适用场所范围&#xff1a; 程序设计&#xff1a; 设…
阅读更多...
大模型技术实践（二）｜关于Llama 2你需要知道的那些事儿

大模型技术实践（二）｜关于Llama 2你需要知道的那些事儿

在上期文章中&#xff0c;我们简要回顾了Llama模型的概况&#xff0c;本期文章我们将详细探讨【关于Llama 2】&#xff0c;你需要知道的那些事儿。 01-Llama 2的性能有多好&#xff1f; 作为Meta新发布的SOTA开源大型语言模型&#xff0c;Llama 2是Llama模型的延续和升级。Ll…
阅读更多...
免费的png打包plist工具CppTextu，一款把若干资源图片拼接为一张大图的免费工具

免费的png打包plist工具CppTextu，一款把若干资源图片拼接为一张大图的免费工具

经常做游戏打包贴图的都知道&#xff0c;要把图片打包为一张或多张大图&#xff0c;要使用打包工具TexturePacker。 TexturePacker官方版可以直接导入PSD、SWF、PNG、BMP等常见的图片格式&#xff0c;主要用于网页、游戏和动画的制作&#xff0c;它可以将多个小图片汇聚成一个…
阅读更多...
java八股文面试[java基础]——CGLIB动态代理与JDK动态代理

java八股文面试[java基础]——CGLIB动态代理与JDK动态代理

CGLIB CGLIB简介&#xff1a; 什么是CGLIB CGLIB是一个强大的、高性能的代码生成库。其被广泛应用于AOP框架&#xff08;Spring、dynaop&#xff09;中&#xff0c;用以提供方法拦截操作。Hibernate作为一个比较受欢迎的ORM框架&#xff0c;同样使用CGLIB来代理单端&#xff…
阅读更多...
iPhone 15预计在A16仿生芯片上运行，性能将有何提升？

iPhone 15预计在A16仿生芯片上运行，性能将有何提升？

苹果最新的移动芯片A17仿生芯片无疑让人兴奋不已&#xff0c;该芯片将成为今年一些iPhone 15机型的驱动力。A17基于3nm处理器&#xff0c;这是第一款提出这一主张的移动硅&#xff0c;由于其更紧凑的尺寸&#xff0c;它有望在性能和能效方面都有所提高。今年秋天买一部A17供电的…
阅读更多...
【Java架构-包管理工具】-Maven私服搭建-Nexus（三）

【Java架构-包管理工具】-Maven私服搭建-Nexus（三）

本文摘要 Maven作为Java后端使用频率非常高的一款依赖管理工具&#xff0c;在此咱们由浅入深&#xff0c;分三篇文章&#xff08;Maven基础、Maven进阶、私服搭建&#xff09;来深入学习Maven&#xff0c;此篇为开篇主要介绍Maven私服搭建-Nexus 文章目录 本文摘要1. Nexus安装…
阅读更多...
爬虫逆向实战（二十）--某99网站登录

爬虫逆向实战（二十）--某99网站登录

一、数据接口分析 主页地址&#xff1a;某99网站 1、抓包 通过抓包可以发现登录接口是AC_userlogin 2、判断是否有加密参数 请求参数是否加密&#xff1f; 通过查看“载荷”可以发现txtPassword和aws是加密参数 请求头是否加密&#xff1f; 无响应是否加密&#xff1f; 无…
阅读更多...
Win11本地安装Ubuntu 22.04 双系统简易教程

Win11本地安装Ubuntu 22.04 双系统简易教程

1.制作启动U盘 首先找到一个硬盘容量不小于4G的空U盘&#xff0c;需要对其进行格式化。 然后下载Ubuntu 22.04的iso文件到本地。 Ubuntu 22.04.1 LTS 中国地区下载链接 下载 UltraISO并制作启动U盘 UltraISO的下载地址 下载免费试用版 选择安装地址&#xff0c;无脑下一步…
阅读更多...
Error running ‘FileApp‘: Command line is too long. Shorten command line for

Error running ‘FileApp‘: Command line is too long. Shorten command line for

报错如下 Error running FileApp: Command line is too long. Shorten command line for 解决方案如下&#xff1a; 打开运行配置 点击上面&#xff0c;默认是收起来的&#xff0c;点击下&#xff0c;下面选择标注的红色的&#xff0c; 重新运行&#xff0c;可以正常启动了
阅读更多...
首发！2025年超500万辆规模，揭榜「融合泊车」TOP10玩家

首发！2025年超500万辆规模，揭榜「融合泊车」TOP10玩家

作为行泊一体赛道关键的一环&#xff0c;融合泊车&#xff08;基于全景环视超声波雷达&#xff09;及后续的高阶泊车方案再次成为行业关注的焦点。 除了部分头部车企自研之外&#xff0c;第三方供应商的市场机会也在扩大。一方面&#xff0c;泊车厂商也在拓展行泊一体方案&…
阅读更多...
ChatGPT⼊门到精通(1):ChatGPT 是什么

ChatGPT⼊门到精通(1):ChatGPT 是什么

⼀、直观感受 1、公司 OpenAI&#xff08;美国&#xff09; 2、官⽅⽹站 3、登录ChatGPT ![在这里插入图片描述](https://img-blog.csdnimg.cn/26901096553a4ba0a5c88c49b2601e6a.png 填⼊帐号、密码&#xff0c;点击登录。登录成功&#xff0c;如下 3、和ChatGPT对话 开始…
阅读更多...
专题-【稀疏矩阵的三元组存储】

专题-【稀疏矩阵的三元组存储】

三元组存储表示&#xff1a; 列序递增转置法&#xff1a;
阅读更多...
计算机竞赛 基于YOLO实现的口罩佩戴检测 - python opemcv 深度学习

计算机竞赛 基于YOLO实现的口罩佩戴检测 - python opemcv 深度学习

文章目录 0 前言1 课题介绍2 算法原理2.1 算法简介2.2 网络架构 3 关键代码4 数据集4.1 安装4.2 打开4.3 选择yolo标注格式4.4 打标签4.5 保存 5 训练6 实现效果6.1 pyqt实现简单GUI6.3 视频识别效果6.4 摄像头实时识别 7 最后 0 前言 &#x1f525; 优质竞赛项目系列&#xf…
阅读更多...
CSS中如何实现元素之间的间距（Margin）合并效果？

CSS中如何实现元素之间的间距（Margin）合并效果？

聚沙成塔每天进步一点点 ⭐ 专栏简介⭐ 外边距合并的示例&#xff1a;⭐ 如何控制外边距合并&#xff1a;⭐ 写在最后 ⭐ 专栏简介 前端入门之旅&#xff1a;探索Web开发的奇妙世界 记得点击上方或者右侧链接订阅本专栏哦 几何带你启航前端之旅 欢迎来到前端入门之旅&#xff…
阅读更多...
sizeof和strlen的对比

sizeof和strlen的对比

文章目录 &#x1f6a9;前言&#x1f6a9;sizeof&#x1f6a9;strlen&#x1f6a9;sizeof和strlen对比 &#x1f6a9;前言 很多小白在学习中&#xff0c;经常将sizeof和strlen弄混了。本篇文章&#xff0c;小编讲解一下sizeof和strlen的区别。&#x1f937;‍♂️ &#x1f6a9…
阅读更多...
windows查看/删除DNS缓存

windows查看/删除DNS缓存

一、查看DNS缓存 打开CMD&#xff0c;输入ipconfig/displaydns 二、删除DNS缓存 打开CMD,输入ipconfig/flushdns
阅读更多...
基于ssm的水果蔬菜商城java jsp网上购物超市mysql源代码

基于ssm的水果蔬菜商城java jsp网上购物超市mysql源代码

本项目为前几天收费帮学妹做的一个项目&#xff0c;Java EE JSP项目&#xff0c;在工作环境中基本使用不到&#xff0c;但是很多学校把这个当作编程入门的项目来做&#xff0c;故分享出本项目供初学者参考。 一、项目描述 基于ssm的水果蔬菜商城 系统有2权限&#xff1a;管理…
阅读更多...
11-Manager 和 模型Model

11-Manager 和 模型Model

准备工作: 一. Manager 库: Manager: 用于管理相关操作端命令和使用相关操作端命令 (1). 安装flask-script: pip install flask-script2.0.3 (2). 在app.py中 包装 app from apps import create_app# Manager类用于管理相关操作端命令和使用相关操作端命令 from flask_scrip…
阅读更多...
记录Taro大坑2丢失api无法启动

记录Taro大坑2丢失api无法启动

现象 解决方案 看了很多。很多说要改成一致的版本号。其实没什么用。 正确方案 再新建一个模板跑起来对比config的配置&#xff0c;以及package.json发现关闭预编译即可。预编译导致api丢失
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023