ORB-SLAM2代码详解05: 关键帧KeyFrame

pdf版本笔记的下载地址: ORB-SLAM2代码详解05_关键帧KeyFrame,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）

可以看看我录制的视频5小时让你假装大概看懂ORB-SLAM2源码

各成员函数/变量

共视图: mConnectedKeyFrameWeights

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

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

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

• mConnectedKeyFrameWeights是一个std::map,无序地保存当前关键帧的共视关键帧及权重.
• mvpOrderedConnectedKeyFrames和mvOrderedWeights按权重降序分别保存当前关键帧的共视关键帧列表和权重列表.

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

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

void KeyFrame::UpdateConnections() {
    
    // 1. 通过遍历当前帧地图点获取其与其它关键帧的共视程度,存入变量KFcounter中
    vector<MapPoint *> vpMP;
    {
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }
    map<KeyFrame *, int> KFcounter; 
    for (MapPoint *pMP : vpMP) {
        map<KeyFrame *, size_t> observations = pMP->GetObservations();
        for (map<KeyFrame *, size_t>::iterator mit = observations.begin(); mit != observations.end(); mit++) {
            if (mit->first->mnId == mnId)		// 与当前关键帧本身不算共视
                continue;
            KFcounter[mit->first]++;
        }
    }
  
    // step2. 找到与当前关键帧共视程度超过15的关键帧,存入变量vPairs中
    vector<pair<int, KeyFrame *> > vPairs;
    int th = 15;
    int nmax = 0;
    KeyFrame *pKFmax = NULL;   
    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));
            (mit->first)->AddConnection(this, mit->second);				// 对超过阈值的共视边建立连接
        }
    }

    //  step3. 对关键帧按照共视权重降序排序,存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中
    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);
    }
    {
        unique_lock<mutex> lockCon(mMutexConnections);
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        // step4. 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
        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.

这3个函数都只在函数KeyFrame::UpdateConnections()内部被调用了,应该设为私有成员函数.

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) {
	// step1. 修改变量mConnectedKeyFrameWeights
    {
        unique_lock<mutex> lock(mMutexConnections);

        if (!mConnectedKeyFrameWeights.count(pKF) || mConnectedKeyFrameWeights[pKF] != weight)
            mConnectedKeyFrameWeights[pKF] = weight;
        else
            return;
    }
	
    // step2. 调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeights
    UpdateBestCovisibles();
}


void KeyFrame::EraseConnection(KeyFrame *pKF) {
    // step1. 修改变量mConnectedKeyFrameWeights
    bool bUpdate = false;
    {
        unique_lock<mutex> lock(mMutexConnections);
        if (mConnectedKeyFrameWeights.count(pKF)) {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate = true;
        }
    }

    // step2. 调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeights
    if (bUpdate)
        UpdateBestCovisibles();
}

void KeyFrame::UpdateBestCovisibles() {    
    unique_lock<mutex> lock(mMutexConnections);
	
    // 取出所有关键帧进行排序,排序结果存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中
    vector<pair<int, KeyFrame *> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    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++) {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

生成树: mpParent、mspChildrens

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

下图表示含有一个10个节点,20条边的稠密图;粗黑线代表其最小生成树,只需9条边即可将所有节点连接起来.

在ORB-SLAM2中,保存所有关键帧构成的最小生成树(优先选择权重大的边作为生成树的边),在回环闭合时只需对最小生成树做BA优化就能以最小代价优化所有关键帧和地图点的位姿,相比于优化共视图大大减少了计算量.(实际上并没有对最小生成树做BA优化,而是对包含生成树的本质图做BA优化)

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

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

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

  成功创建关键帧之后会调用函数KeyFrame::UpdateConnections(),该函数第一次被调用时会将该新关键帧加入到生成树中.

  新关键帧的父关键帧会被设为其共视程度最高的共视关键帧.

  void KeyFrame::UpdateConnections() {
      
      // 更新共视图信息
  	// ...
      
      // 更新关键帧信息: 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
      // 该操作会改变当前关键帧的成员变量mpParent和父关键帧的成员变量mspChildrens
      unique_lock<mutex> lockCon(mMutexConnections);
      if (mbFirstConnection && mnId != 0) {
          mpParent = mvpOrderedConnectedKeyFrames.front();
          mpParent->AddChild(this);
          mbFirstConnection = false;
      }
  }
  

• 共视图的改变(除了删除关键帧以外)不会引发生成树的改变.

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

关键帧的删除

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

与MapPoint类似,函数KeyFrame::SetBadFlag()对KeyFrame的删除过程也采取先标记再清除的方式: 先将坏帧标记mBad置为true,再依次处理其各成员变量.

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

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

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

void KeyFrame::SetNotErase() {
    unique_lock<mutex> lock(mMutexConnections);
    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()删除该关键帧.

void KeyFrame::SetErase() {
    {
        unique_lock<mutex> lock(mMutexConnections);
		// 若当前关键帧没参与回环检测,但其它帧与当前关键帧形成回环关系,也不应当删除当前关键帧
        if (mspLoopEdges.empty()) {
            mbNotErase = false;
        }
    }

    // mbToBeErased：删除之前记录的想要删但时机不合适没有删除的帧
    if (mbToBeErased) {
        SetBadFlag();
    }
}

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

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

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

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

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

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

        if (mnId == 0)
            return;
        else if (mbNotErase) {
            mbToBeErased = true;
            return;
        }
    }

    // step2. 从共视关键帧的共视图中删除本关键帧
	for (auto mit : mConnectedKeyFrameWeights)
        mit.first->EraseConnection(this);

    // step3. 删除当前关键帧中地图点对本帧的观测
    for (size_t i = 0; i < mvpMapPoints.size(); i++)
        if (mvpMapPoints[i])
            mvpMapPoints[i]->EraseObservation(this);

    {
        // step4. 删除共视图
        unique_lock<mutex> lock(mMutexConnections);
        unique_lock<mutex> lock1(mMutexFeatures);
        mConnectedKeyFrameWeights.clear();
        mvpOrderedConnectedKeyFrames.clear();

        // step5. 更新生成树结构
        set<KeyFrame *> sParentCandidates;
        sParentCandidates.insert(mpParent);

        while (!mspChildrens.empty()) {
            bool bContinue = false;
            int max = -1;
            KeyFrame *pC;
            KeyFrame *pP;
            for (KeyFrame *pKF : mspChildrens) {
                if (pKF->isBad())
                    continue;

                vector<KeyFrame *> vpConnected = pKF->GetVectorCovisibleKeyFrames();

                for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) {
                    for (set<KeyFrame *>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end();
                         spcit != spcend; spcit++) {
                        if (vpConnected[i]->mnId == (*spcit)->mnId) {
                            int w = pKF->GetWeight(vpConnected[i]);
                            if (w > max) {
                                pC = pKF;                   
                                pP = vpConnected[i];        
                                max = w;                    
                                bContinue = true;           
                            }
                        }
                    }
                }
            }

            if (bContinue) {
                pC->ChangeParent(pP);
                sParentCandidates.insert(pC);
                mspChildrens.erase(pC);
            } else
                break;
        }

        if (!mspChildrens.empty())
            for (set<KeyFrame *>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++) {
                (*sit)->ChangeParent(mpParent);
            }

        mpParent->EraseChild(this);
		mTcp = Tcw * mpParent->GetPoseInverse();
        // step6. 将当前关键帧的 mbBad 置为 true
        mbBad = true;
    } 
    
	// step7. 从地图中删除当前关键帧
    mpMap->EraseKeyFrame(this);
    mpKeyFrameDB->erase(this);
}

对地图点的观测

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

关键帧观测到的地图点列表由成员变量mvpMapPoints保存,下面是一些对该成员变量进行增删改查的成员函数,就是简单的列表操作,没什么值得说的地方.

成员函数/变量	访问控制	意义
std::vector 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 GetMapPoints()	public
std::vector GetMapPointMatches()	public
int TrackedMapPoints(const int &minObs)	public
MapPoint* GetMapPoint(const size_t &idx)	public

值得关心的是上述函数的调用时机,也就是说参考帧何时与地图点发生关系:

• 关键帧增加对地图点观测的时机:
  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()删除对重投影误差较大的地图点的观测.

回环检测与本质图

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

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

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

KeyFrame的用途

KeyFrame类的生命周期

• KeyFrame的创建:

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

• KeyFrame的销毁:

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

pdf版本笔记的下载地址: ORB-SLAM2代码详解05_关键帧KeyFrame,排版更美观一点,这个网站的默认排版太丑了（访问密码：3834）