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

文章目录

• [0 引言](#0 引言)
• [1 KeyFrame类](#1 KeyFrame类)
• • [1.1 构造函数](#1.1 构造函数)
  • [1.2 成员函数](#1.2 成员函数)
  • [1.3 关键帧之间共视图](#1.3 关键帧之间共视图)
  • • [1.3.1 AddConnection](#1.3.1 AddConnection)
    • [1.3.2 UpdateBestCovisibles](#1.3.2 UpdateBestCovisibles)
    • [1.3.3 UpdateConnections](#1.3.3 UpdateConnections)
    • [1.3.4 EraseConnection](#1.3.4 EraseConnection)
    • [1.3.5 SetBadFlag](#1.3.5 SetBadFlag)
  • [1.4 地图点](#1.4 地图点)
  • [1.5 生成树](#1.5 生成树)
• [2 KeyFrame用途](#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) const	public	获取某个特征点的邻域中的特征点id，类比Frame类的函数
bool KeyFrame::IsInImage(const float &x, const float &y) const	public	判断某个点是否在当前关键帧的图像中
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

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

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