ORB-SLAM3 单目地图初始化（终结篇）

点击上方“计算机视觉工坊”，选择“星标”

干货第一时间送达

一、前言

请阅读本文之前最好把ORB-SLAM3的单目初始化过程再过一遍（ORB-SLAM3 细读单目初始化过程(上)、超详细解读ORB-SLAM3单目初始化（下篇）），以提高学习效率。单目初始化过程中最重要的是两个函数实现，分别是构建帧（Frame）和初始化（Track）。接下来，就是完成初始化过程的最后一步：地图的初始化，是由CreateInitialMapMonocular函数完成的，本文基于该函数的流程出发，目的是为了结合代码流程，把单目初始化的上下两篇的知识点和ORB-SLAM3整个系统的知识点串联起来，系统化零碎的知识，告诉你平时学到的各个小知识应用在SLAM系统中的什么位置，达到快速高效学习的效果。

二、CreateInitialMapMonocular 函数的总体流程

1. 将初始关键帧，当前关键帧的描述子转为BoW；

2. 将关键帧插入到地图；

3. 用三角测量初始化得到的3D点来生成地图点，更新关键帧间的连接关系；

4. 全局BA优化，同时优化所有位姿和三维点；

5. 取场景的中值深度，用于尺度归一化；

6. 将两帧之间的变换归一化到平均深度1的尺度下；

7. 把3D点的尺度也归一化到1；

8. 将关键帧插入局部地图，更新归一化后的位姿、局部地图点。

三、必备知识

1. 为什么单目需要专门策略生成初始化地图

根据论文《ORB-SLAM: a Versatile and Accurate Monocular SLAM System》，即ORB-SLAM1的论文（中文翻译版[ORB-SLAM: a Versatile and Accurate Monocular SLAM System](https://blog.csdn.net/weixin_42905141/article/details/102857958)）可知：

1） 单目SLAM系统需要设计专门的策略来生成初始化地图，这也是为什么代码中单独设计一个CreateInitialMapMonocular()函数来实现单目初始化，也是我们这篇文章要讨论的。为什么要单独设计呢？就是因为单目没有深度信息。

2） 怎么解决单目没有深度信息问题？有2种，论文用的是第二种，用一个具有高不确定度的逆深度参数来初始化点的深度信息，该参数会在后期逐渐收敛到真值。

3） 说了ORB-SLAM为什么要同时计算基础矩阵F和单应矩阵H的原因：这两种摄像头位姿重构方法在低视差下都没有很好的约束，所以提出了一个新的基于模型选择的自动初始化方法，对平面场景算法选择单应性矩阵，而对于非平面场景，算法选择基础矩阵。

4）说了ORB-SLAM初始化容易失败的原因：（条件比较苛刻）在平面的情况下，为了保险起见，如果最终存在双重歧义，则算法避免进行初始化，因为可能会因为错误选择而导致算法崩溃。因此，我们会延迟初始化过程，直到所选的模型在明显的视差下产生唯一的解。

2. 共视图 Covisibility Graph

共视图非常的关键，需要先理解共视图，才能更好的理解后续程序中如何设置顶点和边。

2.1 共视图定义

共视图是无向加权图，每个节点是关键帧，如果两个关键帧之间满足一定的共视关系（至少15个共同观测地图点）他们就连成一条边，边的权重就是共视地图点数目。

2.2 共视图作用

2.2.1 跟踪局部地图，扩大搜索范围

• Tracking::UpdateLocalKeyFrames()

2.2.2 局部建图里关键帧之间新建地图点

• LocalMapping::CreateNewMapPoints()

• LocalMapping::SearchInNeighbors()

2.2.3 闭环检测、重定位检测

• LoopClosing::DetectLoop()、LoopClosing::CorrectLoop()

• KeyFrameDatabase::DetectLoopCandidates

• KeyFrameDatabase::DetectRelocalizationCandidates

2.2.4 优化

• Optimizer::OptimizeEssentialGraph

3. 地图点 MapPoint 和关键帧 KeyFrame

地图点云保存以下信息：

1）它在世界坐标系中的3D坐标

2） 视图方向，即所有视图方向的平均单位向量（该方向是指连接该点云和其对应观测关键帧光心的射线方向）

3）ORB特征描述子，与其他所有能观测到该点云的关键帧中ORB描述子相比，该描述子的汉明距离最小

4）根据ORB特征尺度不变性约束，可观测的点云的最大距离和最小距离

4. 图优化 Graph SLAM

可先看看这些资料[《计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)》](https://blog.csdn.net/plateros/article/details/103498039)，还有《概率机器人学》的第11章，深入理解图优化的概念。

我们在文章开头说过，单目初始化结果得到了三角测量初始化得到的3D地图点Pw，计算得到了初始两帧图像之间的相对位姿（相当于得到了SE(3)），通过相机坐标系Pc和世界坐标系Pw之间的公式，（参考[《像素坐标系、图像坐标系、相机坐标系和世界坐标系的关系（简单易懂版）》](https://blog.csdn.net/shanpenghui/article/details/110481140)）

得到相机坐标系的坐标Pc，但是这样还是不能和像素坐标比较。我们接着通过相机坐标系Pc和像素坐标系P(u,v)之间的公式

5. g2o使用方法

关于g2o库的使用方法，可以参考[《G2O图优化基础和SLAM的Bundle Adjustment(光束平差)》](http://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html#2g2o%E5%BA%93%E7%AE%80%E4%BB%8B%E4%B8%8E%E7%BC%96%E8%AF%91%E5%AE%89%E8%A3%85)和[《理解图优化，一步步带你看懂g2o代码》](https://www.cnblogs.com/CV-life/p/10286037.html)。一般来说，g2o的使用流程如下：

5.1创建一个线性求解器LinearSolver

5.2创建BlockSolver，并用上面定义的线性求解器LinearSolver初始化

5.3创建总求解器solver，并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

5.4创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义的总求解器solver作为求解方法

5.5定义图的顶点和边，并添加到稀疏优化器（SparseOptimizer）中

5.6设置优化参数，开始执行优化

四、代码

1. 将初始关键帧，当前关键帧的描述子转为BoW

pKFini->ComputeBoW();
pKFcur->ComputeBoW();

不展开词袋BoW，只需要知道一点，就是我们在回环检测的时候，需要用到词袋向量mBowVec和特征点向量mFeatVec，所以这里要计算。

2. 向地图添加关键帧

mpAtlas->AddKeyFrame(pKFini);
mpAtlas->AddKeyFrame(pKFcur);

3. 生成地图点，更新图（节点和边）

3.1 遍历

for(size_t i=0; i<mvIniMatches.size();i++)

因为要用三角测量初始化得到的3D点，所以外围是一个大的循环，遍历三角测量初始化得到的3D点mvIniP3D。

3.2 检查

if(mvIniMatches[i]<0)
continue;