【ORB-SLAM2关键知识点梳理1】关键帧、共视图、扩展树、本质图之间的联系

---

前言

越来越不知道这前言写什么，大家共勉吧。

---

一、关键帧（KeyFrame）

简而言之：关键帧是几帧普通帧中较具有代表性的一帧。

1. 作用、意义

1. 降低局部相邻关键帧中的信息冗余度；
2. 由于在SLAM方案中会将普通帧的深度投影到关键帧上，故一定程度上，关键帧是普通帧滤波和优化的结果，防止无效和错误信息进入优化过程；
3. 面向后端优化的算力与精度的折中，提高计算资源的效率。

2. 选择

1. 本身的质量高：清晰的画面、特征点数量充足、特征点分布均匀等；
2. 良好的联系网：与其他关键帧之间有一定的共视关系，同时不能重复度太高，即既存在约束，又要减少冗余的信息。

选取方向（可选取后再按上述标准进行筛选）：
① 与上一关键帧之间的时间（帧数）间隔是否合适；
② 与上一关键帧之间的距离是否足够远；
③ 跟踪局部地图的质量（共视特征点的数量）

---

二、共视图（Covisibility Graph）

1. 概念

图源：计算机视觉life

共视关键帧：任取一关键帧，与该关键帧能观测到相同地图点的关键帧（一级关键帧）。
该关键帧的一级关键帧自身的一级关键帧称为该关键帧的二级关键帧。
注：能观测到相同关键帧的数目由共视程度来表征。

共视图：以任一关键帧为中心，与其共视关键帧及联系构建的共视网。
共视图中的节点为关键帧；若两个关键帧共视地图点超过15个，则用一条边将两者相连接。用权重值θ表示两个关键帧能共同观测到的点云数量。

通常，只会使用一级相邻层级的共视关系及信息，而局部地图优化时用两级相邻共视关系进行优化。

2. 作用

① 增加地图点信息，以优化地图；
② 表示关键帧之间的关系、联系的紧密程度。

3. ORB-SLAM2中的应用场景

① 跟踪局部地图，扩大搜索范围：Tracking::updateLocalKeyFrames()；
② 局部建图中关键帧之间新建地图点：LocalMapping::CreateNewMapPoints()、LocalMapping::SearchInNeighbors()；
③ 闭环检测、重定位检测：LoopClosing::DetectLoop()、LoopClosing::CorrectLoop()、KeyFrameDatabase::DetectLoopCandidates()、KeyFrameDatabase::DetectRelocalizationCandidates()；
④ 优化：Optimizer::OptimizeEssentialGraph()。

---

三、扩展树（Spinning Tree）

由父子关键帧构成，常用于本质图（Essential Graph）中。

图源：计算机视觉life

图中：
红色：父关键帧；
绿色：子关键帧；
黄色：没有父子关系的关键帧。

---

四、本质图（Essential Graph）

针对关键帧而构成的图像。

1. 特点

1. 与共视图相比，本质图比较稀疏；
2. 节点表示所有关键帧，但连接的边只保留联系紧密关键帧之间的边，使得结果更加精确；
3. 图中包含的信息有：
   ① 扩展树的连接关系；
   ② 形成闭环的连接关系；
   ③ 闭环后，引起地图点变动而新增加的连接关系；
   ④ 共视关系较好的连接关系（至少有100个共视地图点）。

2. 作用

在闭环矫正（LoopCorrect）时，用相似变换（Sim3）来矫正尺度漂移，将闭环的误差均摊在本质图中。

3. 与全局BA相比，本质图的优势

从结果来看，
① 全局BA存在收敛问题，即使迭代100次，相对均方误差RMSE也比较大；
② Essential Graph优化可以快速收敛并且结果更精确。θmin表示：被选为Essential Graph节点至少需要的共视地图点数目。从结果来看，θmin的大小对精度影响不大，但是较大的θmin值可以显著减少运行时间；
③ Essential Graph优化+全局Full BA可以在一定程度上提升精度，但是会耗时增大。

作者最后选用的策略：
共视地图点数目至少为100的本质图优化 + 迭次次数为20的全局BA优化。

---

五、与上述内容相关的代码

1. 数据结构定义：

// KeyFrame.h 文件中
    bool mbFirstConnection;                     // 是否是第一次生成树
    KeyFrame* mpParent;                         // 当前关键帧的父关键帧 （共视程度最高的）
    std::set<KeyFrame*> mspChildrens;           // 存储当前关键帧的子关键帧

2. 新关键帧生成时更新连接关系：

// KeyFrame.cc文件中
void KeyFrame::UpdateConnections()
{
	//省略...
	        // Step 5 更新生成树的连接
        if(mbFirstConnection && mnId!=0)
        {
            // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent = mvpOrderedConnectedKeyFrames.front();
            // 建立双向连接关系，将当前关键帧作为其子关键帧
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}


	// 添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧）
	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);
	}

3. 使用
   ① TrackLocalMap里UpdateLocalKeyFrame()更新局部地图中的关键帧

void Tracking::UpdateLocalKeyFrames()
{
	// 省略...
	// 策略2.2:将自己的子关键帧作为局部关键帧（将邻居的子孙们拉拢入伙）
        const set<KeyFrame*> spChilds = pKF->GetChilds();
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
        {
            KeyFrame* pChildKF = *sit;
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //? 找到一个就直接跳出for循环？
                    // 是的
                    break;
                }
            }
        }

        // 策略2.3:自己的父关键帧（将邻居的父母们拉拢入伙）
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
            // mnTrackReferenceForFrame防止重复添加局部关键帧
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                //! 感觉是个bug！如果找到父关键帧会直接跳出整个循环。这里开始是个if啊、还有一些没循环到
                break;
            }
        }
    }

② 闭环校正时，优化Essential Graph

// 在Optimizer.cc
void Optimizer::OptimizeEssentialGraph()
{
	// 省略...
	        // Spanning tree edge
        // Step 4.1：添加第2种边：扩展树的边（有父关键帧）
        // 父关键帧就是和当前帧共视程度最高的关键帧
        if(pParentKF)
        {
            int nIDj = pParentKF->mnId;

            g2o::Sim3 Sjw;

            LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);

            //优先使用未经过Sim3传播调整的位姿
            if(itj!=NonCorrectedSim3.end())
                Sjw = itj->second;
            else
                Sjw = vScw[nIDj];

            // 计算父子关键帧之间的相对位姿
            g2o::Sim3 Sji = Sjw * Swi;

            g2o::EdgeSim3* e = new g2o::EdgeSim3();
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
            // 希望父子关键帧之间的位姿差最小
            e->setMeasurement(Sji);
            // 所有元素的贡献都一样;每个误差边对总误差的贡献也都相同
            e->information() = matLambda;
            optimizer.addEdge(e);
        }
}

等等…