ORB-SLAM2学习笔记10之图像关键帧KeyFrame

news2024/11/19 18:41:32

文章目录

  • 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/919439.html

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

相关文章

华为数通方向HCIP-DataCom H12-821题库(单选题：41-60)

华为数通方向HCIP-DataCom H12-821题库(单选题：41-60)

第41题 以下关于IS-IS协议说法错误的是? A、IS-IS协议支持CLNP网络 B、IS-IS 协议支持IP 网络 C、IS-IS 协议的报文直接由数据链路层封装 D、IS-IS协议是运行在AS之间的链路状态协议 答案&#xff1a;D 解析&#xff1a; 关于IS-IS协议的说法错误是D. IS-IS协议是运行在A…
阅读更多...
机器视觉学习三大忌-贪多，贪杂​，贪快

机器视觉学习三大忌-贪多，贪杂​，贪快

​很多年前&#xff0c;我刚做机器视觉工程师的时候&#xff0c;我师傅第一件事和我说&#xff0c;不要学多&#xff0c;不要学那么多&#xff0c;不要以为会了&#xff0c;就怠慢以后的学习&#xff0c;学习是一生的事情&#xff0c;不是一时的事情。我牢牢记住我师傅的话。 …
阅读更多...
知识蒸馏开山之作（部分解读）—Distilling the Knowledge in a Neural Network

知识蒸馏开山之作（部分解读）—Distilling the Knowledge in a Neural Network

1、蒸馏温度T 正常的模型学习到的就是在正确的类别上得到最大的概率&#xff0c;但是不正确的分类上也会得到一些概率尽管有时这些概率很小&#xff0c;但是在这些不正确的分类中&#xff0c;有一些分类的可能性仍然是其他类别的很多倍。但是对这些非正确类别的预测概率也能反…
阅读更多...
Ext JS 之Microloader(微加载器)

Ext JS 之Microloader(微加载器)

“Microloader”是 Sencha 数据驱动的 JavaScript 和 CSS 动态加载器的名称。 清单 app.json 用于应用的设置，Sencha Cmd 在构建的时候会读取这个文件。 Sencha Cmd 转换“app.json”的内容并将生成的清单传递给 Microloader 以在运行时使用。 最后，Ext JS 本身也会查阅运…
阅读更多...
（WAF）Web应用程序防火墙介绍

（WAF）Web应用程序防火墙介绍

&#xff08;WAF&#xff09;Web应用程序防火墙介绍 1. WAF概述 ​ Web应用程序防火墙&#xff08;WAF&#xff09;是一种关键的网络安全解决方案&#xff0c;用于保护Web应用程序免受各种网络攻击和威胁。随着互联网的不断发展&#xff0c;Web应用程序变得越来越复杂&#x…
阅读更多...
Java代码审计12之JDNI注入以及rmi和Ldap的利用

Java代码审计12之JDNI注入以及rmi和Ldap的利用

文章目录 1、Jndi、Ldap、Rmi协议1.1、什么是ladp协议1.2、jndi协议1.3、rmi协议 2、jndi注入2.1、简介与jdk版本限制2.2、rmi协议的利用2.2.1、更换idea的执行jdk版本2.2.2、生成恶意class文件payload2.2.3、模拟测试低版本jdk2.2.4、模拟高版本测试 2.3、rmi攻击的疑问之两个…
阅读更多...
关于ios Universal Links apple-app-site-association文件 Not Found的问题

关于ios Universal Links apple-app-site-association文件 Not Found的问题

1. 背景说明 1.1 Universal Links 是什么 Support Universal Links 里面有说到 Universal Links 是什么、注意点、以及如何配置的。简单来说就是 当您支持通用链接时&#xff0c;iOS 用户可以点击指向您网站的链接&#xff0c;并无缝重定向到您安装的应用程序 大白话就是说&am…
阅读更多...
3D旅游情景实训教学展示

3D旅游情景实训教学展示

随着科技的不断发展&#xff0c;情景实训教学在教育领域中的应用越来越广泛。通过虚拟现实技术&#xff0c;3D视觉技术&#xff0c;计算机技术等为学生提供了一个身临其境的学习环境&#xff0c;让他们能够在模拟的场景中学习和实践&#xff0c;从而更好地理解和掌握知识。 3D虚…
阅读更多...
机器学习基础之《分类算法（4）—案例：预测facebook签到位置》

机器学习基础之《分类算法（4）—案例：预测facebook签到位置》

一、背景 1、说明 2、数据集 row_id&#xff1a;签到行为的编码 x y&#xff1a;坐标系&#xff0c;人所在的位置 accuracy&#xff1a;定位的准确率 time&#xff1a;时间戳 place_id&#xff1a;预测用户将要签到的位置 3、数据集下载 https://www.kaggle.com/navoshta/gr…
阅读更多...
微信小程序创建项目以及注意事项

微信小程序创建项目以及注意事项

1.申请账号并完善信息 2.下载安装开发工具 3.开发小程序 4.上传代码 5.提交审核 6.发布 创建项目 根据需求选择模板&#xff0c;也可以不选择模板 创建完毕之后 进入页面点击终端 然后新建终端 输入npm init 一直按回车即可 安装成功 出现package.json 如何使用组件&#x…
阅读更多...
Spring Cache的介绍以及怎么使用（redis）

Spring Cache的介绍以及怎么使用（redis）

Spring Cache 文章目录 Spring Cache1、Spring Cache介绍2、Spring Cache常用注解2.1、EnableCaching注解2.2、CachePut注解2.3、CacheEvict注解2.4、Cacheable注解 3、Spring Cache使用方式--redis 1、Spring Cache介绍 Spring Cache是一个框架&#xff0c;实现了基于注解的缓…
阅读更多...
【LeetCode】模拟实现FILE以及认识缓冲区

【LeetCode】模拟实现FILE以及认识缓冲区

模拟实现FILE以及认识缓冲区 刷新缓冲逻辑图自定义实现如何强制刷新内核缓冲区例子 刷新缓冲逻辑图 自定义实现 mystdio.h #pragma once #include <stdio.h>#define NUM 1024 #define BUFF_NOME 0x1 #define BUFF_LINE 0x2 #define BUFF_ALL 0x4typedef struct _MY_FIL…
阅读更多...
对《VB.NET通过VB6 ActiveX DLL调用PowerBasic及FreeBasic动态库》的改进

对《VB.NET通过VB6 ActiveX DLL调用PowerBasic及FreeBasic动态库》的改进

《VB.NET通过VB6 ActiveX DLL调用PowerBasic及FreeBasic动态库》使用的Activex DLL公共对象是需要先注册的。https://blog.csdn.net/weixin_45707491/article/details/132437502?spm1001.2014.3001.5501 Activex DLL事前注册&#xff0c;一次多用说起来也不是啥大问题&#x…
阅读更多...
C语言小白急救 指针进阶讲解1

C语言小白急救 指针进阶讲解1

文章目录 指针一、 字符指针二、 指针数组三、数组指针1.数组的地址2.数组指针3.数组指针的应用 四、数组参数、指针参数1. 一维数组传参2.二维数组传参3.一级指针传参4.二级指针传参 五、函数指针1.函数的地址2.函数指针3.练习 指针 指针的概念&#xff1a; 1.指针就是个变量…
阅读更多...
数据库（DQL，多表设计，事务，索引）

数据库（DQL，多表设计，事务，索引）

目录 查询数据库表中数据 where 条件列表 group by 分组查询 having 分组后条件列表 order by 排序字段列表 limit 分页参数 多表设计 一对多 多对多 一对一 多表查询 事物 索引 查询数据库表中数据 关键字&#xff1a;SELECT 中间有空格&#xff0c;加引…
阅读更多...
H.265视频无插件流媒体播放器EasyPlayer.js播放webrtc断流重连的异常修复

H.265视频无插件流媒体播放器EasyPlayer.js播放webrtc断流重连的异常修复

H5无插件流媒体播放器EasyPlayer属于一款高效、精炼、稳定且免费的流媒体播放器&#xff0c;可支持多种流媒体协议播放&#xff0c;可支持H.264与H.265编码格式&#xff0c;性能稳定、播放流畅&#xff0c;能支持WebSocket-FLV、HTTP-FLV&#xff0c;HLS&#xff08;m3u8&#…
阅读更多...
FxFactory 8 Pro Mac 苹果电脑版 fcpx/ae/motion视觉特效软件包

FxFactory 8 Pro Mac 苹果电脑版 fcpx/ae/motion视觉特效软件包

FxFactory pro for mac是应用在Mac上的fcpx/ae/pr视觉特效插件包&#xff0c;包含了成百上千的视觉效果&#xff0c;打包了很多插件&#xff0c;如调色插件&#xff0c;转场插件&#xff0c;视觉插件&#xff0c;特效插件&#xff0c;文字插件&#xff0c;音频插件&#xff0c;…
阅读更多...
百望云华为云共建零售数字化新生态 聚焦数智新消费升级

百望云华为云共建零售数字化新生态 聚焦数智新消费升级

零售业是一个充满活力和创新的行业&#xff0c;但也是当前面临很大新挑战和新机遇的行业。数智新消费时代&#xff0c;数字化转型已经成为零售企业必须面对的重要课题。 8 月 20 日-21日&#xff0c;以“云上创新 韧性增长”为主题的华为云数智新消费创新峰会2023在成都隆重召…
阅读更多...
stm32之10.系统定时器

stm32之10.系统定时器

delay_s()延时秒 delay_ms()毫秒*1000 delay_us()微秒*1000000 微秒定时器代码 void delay_us(uint32_t n) { SysTick->CTRL 0; // Disable SysTick&#xff0c;关闭系统定时器 SysTick->LOAD SystemCoreClock/1000000*n-1; // 就是nus SysTick->LOAD Sys…
阅读更多...
有趣的数学 数学建模入门二 一些理论基础

有趣的数学 数学建模入门二 一些理论基础

一、什么是数学建模? 现实世界中混乱的问题可以用数学来解决&#xff0c;从而产生一系列可能的解决方案来帮助指导决策。大多数人对数学建模的概念感到不舒服&#xff0c;因为它是如此开放。如此多的未知信息似乎令人望而却步。哪些因素最相关&#xff1f;但正是现实世界问题的…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023