第9周课件-全网最详细的ORB-SLAM2精讲

目录

 

本周课程重点：

1 共视图 本质图 拓展树原理

1.1 关键帧、共视图、扩展树、本质图的区别

1.2 共视图 （Covisibility Graph）

1.3 本质图（Essential Graph）

1.4 扩展树（spanning tree）

1.5 essential graph 和 full BA 结果对比

1.6 计算Sim3函数ComputeSim3

2 全网最详细的EPnP 算法原理详解

2.1 背景介绍

2.2 算法的优点：

2.3 控制点如何选取？

2.4 计算控制点系数，用控制点重新表达数据

2.5  透视投影关系构建约束

2.6  ICP 求解位姿

3、总结

---

 

本周课程重点：

1. 掌握关键帧、共视图、扩展树、本质图的原理（重要）。

2. 掌握本质图优化原理和代码（重要）。

3. 理解 EPnP 的原理（重要），能看懂推导过程。

 

 

1 共视图 本质图 拓展树原理

1.1 关键帧、共视图、扩展树、本质图的区别

1.2 共视图 （Covisibility Graph）

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

 

共视图的作用：

1.3 本质图（Essential Graph）

共视图比较稠密，本质图比共视图更稀疏，这是因为本质图的作用是用在闭环矫正时，用相似变换来矫 正尺度漂移，把闭环误差均摊在本质图中。本质图中节点也是所有关键帧，但是连接边更少，只保留了 联系紧密的边来使得结果更精确。本质图中包含：

1. 扩展树连接关系

2. 形成闭环的连接关系，闭环后地图点变动后新增加的连接关系

3. 共视关系非常好（至少100个共视地图点，条件苛刻。所以比共视图稀疏）的连接关系

 

1.4 扩展树（spanning tree）

子关键帧和父关键帧构成

 

1.5 essential graph 和 full BA 结果对比 

从结果来看，

1 、全局 BA 存在收敛问题。即使迭代 100 次，相对均方误差 RMSE 也比较高

2 、 essential graph 优化可以快速收敛并且结果更精确。 θ min 表示被选为 essential graph 至少需要的共视地图点数目，从结果来看，θ min 的大小对精度影响不大，但是较大的 θmin值可以显著减少运行时间（因为本质图少，条件苛刻）

3 、 essential graph 优化 后增加全局 full BA 可以提升精度（但比较有限），但是会耗时较多

 

1.6 计算Sim3函数ComputeSim3

LoopClosing::ComputeSim3() 里查找  vpLoopConnectedKFs ， mvpLoopMapPoints

 

 

2 全网最详细的EPnP 算法原理详解

2.1 背景介绍

论文： Lepetit V , Fua M N . EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal of Computer Vision, 2009.

 

输入：

1. n个世界坐标系下的3D点，论文中称为3D参考点

2. 这 个3D点投影在图像上的2D坐标

3. 相机内参矩阵 ，包括焦距和主点

输出：

相机的位姿 R，t

 

应用：特征点的图像跟踪，需要实时处理有噪声的特征点，对计算精度和效率要求比较高，只需 4 对匹配 点即可求解。

 

2.2 算法的优点：

1. 只需要4对非共面点，对于平面只需要3对点

2. 闭式解，不需要迭代，不需要初始估计值。

3. 精度比较高。和迭代法里精度最高的方法LHM方法精度相当。

4. 比较鲁棒，可以处理带噪声的数据。迭代法受到初始估计的影响比较大，会不稳定

5. 线性计算复杂度为O(n)

6. 平面和非平面都适用

 

原理和步骤：

我们目前知道 n个世界坐标系下的 3D 点及其在图像上的 2D 投影点，还有相机内参，目的是为了求世界坐标系到相机坐标系下的位姿变换 R, t;

 

EPnP  的思路就是先把 2D图像点通过内参变换到相机坐标系下的3D 点，然后用 ICP 来求解 3D-3D 的变换就得到了位姿。那么问题的核心就转化为如何通过2D 信息，加上一些约束，来得到相机坐标系下的 3D 点。 因为我们这里的位姿变换是欧式空间下的刚体变换，所以点之间的相对距离信息在不同坐标系下是不变 的。我们称之为刚体结构不变性 。后面就是紧紧围绕这个特性来求解的。

 

1. 首先我们对 3D 点的表达方式进行了新的定义。之前不管是世界坐标系还是相机坐标系下的 3D 点，它们都是相对于自己坐标系下的原点的。那么两个坐标系原点不同，坐标的量级可能差异非常大，比如相机坐标系下3D 点坐标范围可能是 10-100 之间，世界坐标系下坐标可能是 1000-10000 之间，这对求解优化都是不利的。所以我们要统一一下量级。可以理解为归一化吧，这在求基础矩阵、单应矩阵时都是常规手段。( 归一化的原因 )

 

具体来说，我们对每个坐标系定义 4 个控制点 ，其中一个是质心（ 也就是各个方向均值 ），其他 3 个用PCA 从三个主方向选取，这 4 个控制点可以认为是参考基准，类似于坐标系里的基。所有的 3D 点都表达为这4 个参考点的线性组合。这些系数我们称之为权重，为了不改变数据的相对距离，权重和必须为1 。这样，我们就可以 用世界坐标系或相机坐标系下的 4 个控制点表示所有的世界坐标系或 相近坐标系下的 3D 点。

 

2. 利用投影方程将图像 2D 点恢复相机坐标系下 3D 点（未知量）。经过整理后，一组点对可以得到 2 个方程。我们待求的相机坐标系下3D 点对应的 4 个控制点，每个控制点 3 个分量，总共 12 个未知数组成的一个向量。

3. 用 SVD 分解可以求解上述向量，但是因为恢复的相机坐标系下 3D 点还有个 尺度因子 ， 这里我们根据结构信息不变性作为约束，求解。

4. 最后用高斯牛顿法优化上述求解的 。

注： ORB-SLAM 里使用的 EPnP 是直接拷贝 OpenCV 里的源码： modules → calib3d → src → epnp.cpp

 

统一变量格式：

 

4 个控制点系数 ，也就是论文中的homogeneous barycentric coordinates，我们翻译为齐次重心坐标。 同一 3D 点在世界坐标系下和相机坐标系下的控制点系数相 同 。后面会给出证明

 

2.3 控制点如何选取？

理论上，控制点的坐标可以任意选取。但在实践中，作者发现了一种可以提高结果稳定性的控制点选择 方法。具体如下

将剩余的3个控制点表示为：
 

2.4 计算控制点系数，用控制点重新表达数据

我们将世界坐标系下 3D 点的坐标表示为对应控制点坐标的线性组合：

那么，世界坐标系下控制点的系数可以这样计算得到

 

口说无凭，我们来推导一下，假设待求的相机位姿为 T ，那么

 

2.5  透视投影关系构建约束

求解

 

如何确定 ？

因为每个点对可以得到 2 个约束方程，总共有 12 个未知数，所以如果有 6组点对，我们就能直接求解，此时 N=1。如果相机的焦距逐渐增大，相机模型更趋近于使用正交相机代替透视相机，零空间的自由度就会增加到 N=4 。

我们来看论文中这张图：

 

横坐标表示通过MTM 特征值分解得到的12 个特征值的序号，纵坐标表示对应特征值的大小。 当焦距 f =100 时，我们看局部放大的右图，只有1 个特征值是 0 ，所以只要用最后一个特征向量就可以 了。

当焦距f =1000 时，右图中可以看到第9,10,11,12个特征值都是 0 ，也就是说只用最后一个特征向量是 没有办法表示的，要用到最后4个特征值对应的特征向量加权才行，这就是最大N=4  的来源。

分别求偏导后得到

 

高斯牛顿法的增量方程：

2.6  ICP 求解位姿

3、总结

参考

Lepetit V , Fua M N . EPnP: An Accurate O(n) Solution to the PnP Problem[J]. International Journal

of Computer Vision, 2009.

小葡萄 https://zhuanlan.zhihu.com/p/59070440

Jessie https://blog.csdn.net/jessecw79/article/details/82945918

代码： https://github.com/cvlab-epfl/EPnP