05 关键帧 KeyFrame

05 关键帧 KeyFrame

5.1 共视图：mConnectedKeyFrameWeights

能看到同一地图点的两关键帧之间存在共视关系，共视地图点的数量称为权重。

成员函数/变量	访问控制	意义
std::map<KeyFrame*,int> mConnectedKeyFrameWeights	private	当前关键帧的共视关键帧及其权重
std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames	private	所有共视关键帧,按权重从大到小排序
std::vector<int> mvOrderedWeights	private	所有共视权重,按从大到小排序
void UpdateConnections()	public	基于当前关键帧对地图点的观测构造共视图
void AddConnection(KeyFrame* pKF, const int &weight)	public	添加共视关键帧
void EraseConnection(KeyFrame* pKF)	public	删除共视关键帧
void UpdateBestCovisibles()	public	基于共视图信息修改对应变量
std::set<KeyFrame*> GetConnectedKeyFrames()	public	get 方法
std::vector<KeyFrame* > GetVectorCovisibleKeyFrames()	public	get 方法
std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int &N)	public	get 方法
std::vector<KeyFrame*> GetCovisiblesByWeight(const int &w)	public	get 方法
int GetWeight(KeyFrame* pKF)	public	get 方法

共视图结构由3个成员变量维护：

• mConnectedKeyFrameWeights 是一个 std::map，无序地保存当前关键帧的共视关键帧及其权重；

• mvpOrderedConnectedKeyFrames 和 mvOrderedWeights 按权重降序分别保存当前关键帧的共视关键帧列表和权重列表。

5.1.1 基于对地图点的观测重新构造共视图：UpdateConnections()

这 3 个变量由函数 KeyFrame::UpdateConnections() 进行初始化和维护，基于当前关键帧看到的地图点信息重新生成共视关键帧。

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

    //===============1==================================
    map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数

    vector<MapPoint*> vpMP;

    {
        // 获得该关键帧的所有3D点
        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
    // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
    // 即统计每一个关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        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]++;
        }
    }

    // This should not happen
    if(KFcounter.empty())
        return;

    //===============2==================================
    // 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;
    int th = 15;

    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    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));
            // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
            // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);
        }
    }

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

    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小
    sort(vPairs.begin(),vPairs.end());
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    //===============3==================================
    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
        // 更新图的连接(权重)
        mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

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

只要关键帧与地图点间的连接关系发生变化（包括关键帧创建和地图点重新匹配关键帧特征点），函数 KeyFrame::UpdateConnections() 就会被调用。具体来说，函数 KeyFrame::UpdateConnections() 的调用时机包括：

• Tracking 线程中初始化函数 Tracking::StereoInitialization() 或 Tracking::MonocularInitialization() 函数创建关键帧后会调用 KeyFrame::UpdateConnections()初始化共视图信息；

• LocalMapping 线程接受到新关键帧时会调用函数 LocalMapping::ProcessNewKeyFrame() 处理跟踪过程中加入的地图点，之后会调用 KeyFrame::UpdateConnections() 初始化共视图信息。（实际上这里处理的是 Tracking 线程中函数 Tracking::CreateNewKeyFrame() 创建的关键帧）

• LocalMapping 线程处理完毕缓冲队列内所有关键帧后会调用 LocalMapping::SearchInNeighbors() 融合当前关键帧和共视关键帧间的重复地图点，之后会调用 KeyFrame::UpdateConnections() 更新共视图信息；

• LoopClosing 线程闭环矫正函数 LoopClosing::CorrectLoop() 会多次调用 KeyFrame::UpdateConnections() 更新共视图信息。

函数 AddConnection(KeyFrame* pKF, const int &weight) 和 EraseConnection(KeyFrame* pKF) 先对变量 mConnectedKeyFrameWeights 进行修改，再调用函数 UpdateBestCovisibles() 修改变量 mvpOrderedConnectedKeyFrames 和 mvOrderedWeights。

5.2 生成树

生成树是一种稀疏连接，以最小的边数保存图中所有节点。对于含有 N 个节点的图，只需构造一个 N-1 条边的最小生成树就可以将所有节点连接起来。如下图

10 个节点 20 条边的稠密图，只需 9 条边即可将所有节点连接起来。

在 ORB-SLAM2 中，保存所有关键帧构成的最小生成树（优先选择权重大的边作为生成树的边），在回环闭合时只需对最小生成树做 BA 优化就能以最小代价优化所有关键帧和地图点的位姿，相比于优化共视图大大减少了计算量。

成员变量/函数	访问控制	意义
bool mbFirstConnection	protected	当前关键帧 是否还未加入到生成树，构造函数中初始化为 true，加入生成树后置为 false
KeyFrame* mpParent	protected	当前关键帧在生成树中的父节点
std::set<KeyFrame*> mspChildrens	protected	当前关键帧在生成树中的子节点列表
KeyFrame* GetParent()	public	mpParent 的 get 方法
void ChangeParent(KeyFrame* pKF)	public	mpParent 的 set 方法
std::set<KeyFrame*> GetChilds()	public	mspChildrens 的 get 方法
void AddChild(KeyFrame* pKF)	public	添加子节点，mspChildrens 的 set 方法
void EraseChild(KeyFrame* pKF)	public	删除子节点，mspChildrens 的 set 方法
bool hasChild(KeyFrame* pKF)	public	判断 mspChildrens 是否为空

生成树结构由成员变量 mpParent 和 mspChildrens 维护，我们主要关注生成树结构发生改变的时机。

• 关键帧增加到生成树中的时机：

成功创建关键帧之后会调用函数 KeyFrame::UpdateConnections()，该函数第一次被调用时会将该新关键帧加入到生成树中。新关键帧的父关键帧会被设为其共视程度最高的共视关键帧。

• 共视图的改变（除了删除关键帧以外）不会引发生成树的改变。

• 只有当某个关键帧删除时，与其相连的生成树结构才会发生改变。（因为生成树是个单线联系的结构，没有冗余，一旦某关键帧删除了就得更新树结构才能保证所有关键帧依旧相连）。生成树结构改变的方式类似于最小生成树算法中的加边法，见后文对函数 setbadflag() 的分析。

5.3 关键帧的删除

成员函数/变量	访问控制	初值	意义
bool mbBad	protected	false	标记坏帧
bool isBad()	public		mbBad 的 get 方法
void SetBadFlag()	public		删除帧
bool mbNotErase	protected	flase	当前关键帧是否具有不被删除的特权
bool mbToBeErased	protected	false	当前关键帧是否曾被豁免过删除
void SetNotErase()	public		mbNotErase 的 set 方法
void SetErase()	public

5.3.1 参与回环检测的关键帧具有不被删除的特权：mbNotErase

参与回环检测的关键帧具有不被删除的特权，该特权由成员变量mbNotErase 存储，创建 KeyFrame 对象时该成员变量默认被初始化为 false。

若某关键帧参与了回环检测，LoopClosing 线程就会就调用函数 KeyFrame::SetNotErase() 将该关键帧的成员变量 mbNotErase 设为 true，标记该关键帧暂时不要被删除。

删除函数 SetBadFlag() 根据成员变量 mbNotErase 判断当前 KeyFrame 是否具有豁免删除的特权，若当前 KeyFrame 的 mbNotErase 为 true，则函数 SetBadFlag() 不能删除当前 KeyFrame，但会将其成员变量 mbToBeErased 置为 true（记录当前关键帧曾被豁免过删除）。

void KeyFrame::SetBadFlag() {
    // step1. 特殊情况:豁免 第一帧 和 具有mbNotErase特权的帧
    {
        unique_lock<mutex> lock(mMutexConnections);

        if (mnId == 0)
            return;
        else if (mbNotErase) {
            mbToBeErased = true;
            return;
        }
    }
    
    // 两步删除: 先逻辑删除,再物理删除...
}

成员变量 mbToBeErased 标记当前 KeyFrame 是否被豁免过删除特权。LoopClosing 线程不再需要某关键帧时，会调用函数 KeyFrame::SetErase() 剥夺该关键帧不被删除的特权，将成员变量 mbNotErase 复位为 false；同时检查成员变量 mbToBeErased，若 mbToBeErased 为 true 就会调用函数 KeyFrame::SetBadFlag() 删除该关键帧。

5.3.2 删除关键帧时维护共视图和生成树

函数 SetBadFlag() 在删除关键帧时维护其 共视图和生成树 结构。共视图结构的维护比较简单，这里主要关心如何维护生成树的结构。

当一个关键帧被删除时，其父关键帧和所有子关键帧的生成树信息也会受到影响，需要为其所有子关键帧寻找新的父关键帧，如果父关键帧找的不好的话，就会产生回环，导致生成树就断开。

被删除关键帧的子关键帧所有可能的父关键帧包括其 兄弟关键帧和其被删除关键帧的父关键帧。以下图为例，关键帧 4 可能的父关键帧包括关键帧 3、5、6 和 7。

采用类似最小生成树算法中的加边法重新构建生成树结构：每次循环取权重最高的候选边建立父子连接关系，并将新加入生成树的子节点到加入候选父节点集合 sParentCandidates 中。

5.4 对地图点的观测

KeyFrame 类除了像一般的 Frame 类那样保存二维图像特征点以外，还保存三维地图点 MapPoint 信息。

成员函数/变量	访问控制	意义
std::vector<MapPoint*> mvpMapPoints	protected	当前关键帧观测到的地图点
void AddMapPoint(MapPoint* pMP, const size_t &idx)	public	添加地图点
void EraseMapPointMatch(const size_t &idx)	public	删除地图点（按索引）
void EraseMapPointMatch(MapPoint* pMP)	public	删除地图点（按地图点）
void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)	public	替换地图点
std::set<MapPoint*> GetMapPoints()	public	遍历，获取所有地图点
std::vector<MapPoint*> GetMapPointMatches()	public	获取该关键帧的 MapPoints
int TrackedMapPoints(const int &minObs)	public	获取观测多于 minObs 的地图点（一个地图点会被多个关键帧观测到）
MapPoint* GetMapPoint(const size_t &idx)	public	获取地图点（按索引）

（1）关键帧增加对地图点观测的时机：

• Tracking 线程和 LocalMapping 线程创建新地图点后，会马上调用函数 KeyFrame::AddMapPoint() 添加当前关键帧对该地图点的观测；

• LocalMapping 线程处理完毕缓冲队列内所有关键帧后会调用 LocalMapping::SearchInNeighbors() 融合当前关键帧和共视关键帧间的重复地图点，其中调用函数 ORBmatcher::Fuse() 实现融合过程中会调用函数 KeyFrame::AddMapPoint()；

• LoopClosing 线程闭环矫正函数 LoopClosing::CorrectLoop() 将闭环关键帧与其匹配关键帧间的地图进行融合，会调用函数 KeyFrame::AddMapPoint()。

（2）关键帧替换和删除对地图点观测的时机：

• MapPoint 删除函数 MapPoint::SetBadFlag() 或替换函数 MapPoint::Replace() 会调用 KeyFrame::EraseMapPointMatch() 和 KeyFrame::ReplaceMapPointMatch() 删除和替换关键针对地图点的观测；

• LocalMapping 线程调用进行局部 BA 优化的函数 Optimizer::LocalBundleAdjustment() 内部调用函数 KeyFrame::EraseMapPointMatch() 删除对重投影误差较大的地图点的观测。

5.5 回环检测与本质图

成员函数/变量	访问控制	意义
std::set<KeyFrame*> mspLoopEdges	protected	和当前帧形成回环的关键帧集合
std::set<KeyFrame*> GetLoopEdges()	public	mspLoopEdge 的 get 函数
void AddLoopEdge(KeyFrame* pKF)	public	mspLoopEdge 的 set 函数

LoopClosing 线程中回环矫正函数 LoopClosing::CorrectLoop() 在调用本质图 BA 优化函数 Optimizer::OptimizeEssentialGraph() 之前会调用函数 KeyFrame::AddLoopEdge()，在当前关键帧和其闭环匹配关键帧间添加回环关系。

在调用本质图 BA 优化函数 Optimizer::OptimizeEssentialGraph() 中会调用函数 KeyFrame::GetLoopEdges() 将所有闭环关系加入到本质图中进行优化。

5.6 KeyFrame 的用途

5.6.1 KeyFrame 的生命周期

（1）KeyFrame的创建：

Tracking 线程中通过函数 Tracking::NeedNewKeyFrame() 判断是否需要关键帧，若需要关键帧，则调用函数 Tracking::CreateNewKeyFrame() 创建关键帧。

（2）KeyFrame的销毁：

LocalMapping 线程剔除冗余关键帧函数 LocalMapping::KeyFrameCulling() 中若检查到某关键帧为冗余关键帧，则调用函数 KeyFrame::SetBadFlag() 删除关键帧。