视觉SLAM理论到实践系列（六）——特征点法视觉里程计（3）

本文链接：https://blog.csdn.net/qq_37648371/article/details/134658258

本文介绍了视觉SLAM中的特征点法和视觉里程计，重点讲解了对极几何在确定本征矩阵E和尺度scale中的应用，以及如何通过特征匹配恢复相机旋转R和平移t。同时讨论了如何确定绝对尺度，包括来自直接测量、运动约束和与其他传感器集成的方法。

摘要由CSDN通过智能技术生成

视觉SLAM理论到实践系列文章

下面是《视觉SLAM十四讲》学习笔记的系列记录的总链接，本人发表这个系列的文章链接均收录于此

视觉SLAM理论到实践专栏

文章目录

视觉SLAM理论到实践系列文章
- 视觉SLAM理论到实践系列文章链接
- 视觉SLAM理论到实践专栏
前言
视觉SLAM理论到实践系列（六）——特征点法视觉里程计（3）
- 对极几何补充

前言

高翔博士的《视觉SLAM14讲》学习笔记的系列记录

视觉SLAM理论到实践系列（六）——特征点法视觉里程计（3）

对极几何补充

参考：

SFM中平移向量的尺度（scale）问题

Monocular slam 的理论基础(1)

Monocular slam 中的理论基础(2)

Homography 知多少？

单目视觉里程计 mono vo

从对极几何恢复相机运动

单目SLAM一般处理流程包括track和map两部分。所谓的track是用来估计相机的位姿。而map部分就是计算pixel的深度，如果相机的位姿有了，就可以通过三角法(triangulation)确定pixel的深度，把这些计算好深度的pixel放到map里就重建出了三维环境。

两个摄像机的光心 $C_0、C_1$ ，三维空间中一点 $P$ ，在两幅图像中的位置为 $p_0、p_1$ 。如下图所示：

在这里插入图片描述

由于 $C_0、C_1、P$ 三点共面，得到：

在这里插入图片描述

$p_0$ 在坐标系 $C_0$ 中的表示,以及 $p_1$ 在坐标系 $C_1$ 中的表示为：
$\left.p_0=\left(\begin{array}{c}x_0\\y_0\\1\end{array}\right.\right)_{c_0}\quad\quad p_1=\left(\begin{array}{c}x_1\\y_1\\1\end{array}\right)_{c_1}$
请特别注意：

这里的 $p_i$ 是在摄像机坐标系 $C_i$ 中的表示，而我们从图像平面得到的 $u,v,1)^T$ 是在图像坐标系下的表示。我们知道从摄像机坐标系到图像坐标系，是左乘了一个内参数矩阵 $K$ 。所以根据图像坐标中的 $(u, v)$ 得到其在摄像机坐标系下的坐标 $p$ 需要左乘内参数的逆矩阵 $K^{−1}$ 。

在摄像机坐标系中的表示就称为normalized coordinates（hartley 书的第257页）。在程序中得到p的坐标代码演示如下：

在这里插入图片描述

这时，由共面得到的向量方程可写成：
$\mathbf{p}_{0}\cdot(\mathbf{t}\times\mathbf{R}\mathbf{p}_{1})=0 \tag{1}$
其中， $t$ 是两个摄像机光心的平移量； $R$ 是从坐标系 $C_1$ 到坐标系 $C_0$ 的旋转变换，左乘旋转矩阵 $R$ 的目的是把向量 $\overrightarrow{C_1p_1}$ 在坐标系 $C_1$ 下的表示旋转到坐标系 $C_0$ 下，注意点 $p_1$ 是在坐标系 $C_1$ 中的表示。

一个向量 $a$ 叉乘一个向量 $b$ 可以表示为一个反对称矩阵乘以向量 $b$ 的形式这时由向量 $a$ 表示的反对称矩阵(skew symmetric matrix)如下:
$[\mathbf{a}]_{\times}=\begin{pmatrix}0&-a_{3}&a_{2}\\a_{3}&0&-a_{1}\\-a_{2}&a_{1}&0\end{pmatrix}$
所以式（1）可以写成：
$\mathbf{p}_0^T[\mathbf{t}]_\times\mathbf{R}\mathbf{p}_1=0$
本征矩阵 $E$ 定义为
$E=[t]_\times R$
它是一个3x3的矩阵
$\mathbf{p}_0^T\mathbf{E}\mathbf{p}_1=0$
$\begin{pmatrix}x_0&y_0&1\end{pmatrix}\begin{pmatrix}E_{11}&E_{12}&E_{13}\\E_{21}&E_{22}&E_{23}\\E_{31}&E_{32}&E_{33}\end{pmatrix}\begin{pmatrix}x_1\\y_1\\1\end{pmatrix}=0$
本征矩阵的性质：

一个3x3的矩阵是本征矩阵的充要条件是对它奇异值分解后，它有两个相等的奇异值，并且第三个奇异值为0。

牢记这个性质，它在实际求解本征矩阵时有很重要的意义。这个性质的证明见Hartley的著作《Multiple view geometry》的第258页。

计算本征矩阵E、尺度scale的来由

将矩阵相乘的形式拆开得到
$\mathrm{Write~as~{\bf{A}}x=0,~where~x=(E11,E12,E13,\ldots,E33)}$
$\begin{pmatrix} x_0x_1&x_0y_1&x_0&y_0x_1&y_0y_1&y_0&x_1&y_1&1 \end{pmatrix} \begin{pmatrix}E_{11}\\E_{12}\\E_{13}\\\vdots\\E_{33}\end{pmatrix}=0$
上面这个方程左边进行任意缩放都不会影响方程的解：
$\begin{pmatrix}x_0x_1&x_0y_1&x_0&y_0x_1&y_0y_1&y_0&x_1&y_1&1\end{pmatrix}E_{33}\begin{pmatrix}E_{11}/E_{33}\\E_{12}/E_{33}\\E_{13}/E_{33}\\.\\.\\.\\1\end{pmatrix}=0$
所以 $E$ 虽然有9个未知数，但是有一个变量 $E_{33}$ 可以看做是缩放因子，因此实际只有8个未知量，这里就是尺度scale的来由，后面会进一步分析这个尺度。

$\bf{Ax}=0$ ， $\bf{x}$ 有8个未知量，需要 $A$ 的秩等于8，所以至少需要8对匹配点。有了匹配点后，就只需要求解最小二乘问题了，上面这个方程的解就是矩阵 $A$ 进行SVD分解 ${\bf{A=U\Sigma V}}^T$ 后， $V$ 矩阵最右边那一列的值。

另外如果这些匹配点都在一个平面上那就会出现 $A$ 的秩小于8的情况，这时会出现多解，会让你计算的 $E$ 可能是错误的。上面是计算本征矩阵 $E$ 的八点法，大家也可以去看看wiki的详细说明：Eight-point algorithm。

上面使用8点法计算 $E$ 的过程使用的约束是 $p^T_0Ep_1=0$ 。另外 $E$ 这个矩阵它自身还有一个很棒的约束:
$EE^\top E-\frac{1}{2}trace(EE^\top)E=0.$
在这里插入图片描述

这个矩阵约束实际上是9个子约束，每个子约束都是对应矩阵元素相减等于0,，详见Essential matrix。利用这个约束去求解E就是五点法的思路，具体算法可以看 David Nister的论文。

然而在实际计算过程中，匹配点坐标存在误差，这会使得计算出的 $E$ 可能不会满足之前提到的那条性质，所以我们需要把计算出的E投影到真正的本征矩阵空间，也就是使得它的三个奇异值中两个相等，一个为0。投影的方法如下，实际就是强制改变这个矩阵的奇异值，具体证明见YI Ma著作《An Invitation to 3D vision》的第86页。

在这里插入图片描述

在应用的时候，考虑到E矩阵反正已经是缩放了的，所以更多的是直接令奇异值为 $(1, 1, 0)$ ，程序如下：

在这里插入图片描述

有了本征矩阵 $E$ ，就可以从 $E$ 中恢复平移 $t$ 和旋转 $R$ 。

特征点匹配

在讲解恢复 $R, T$ 前，稍微提一下特征点匹配的方法。常见的有如下两种方式：

计算特征点，然后计算特征描述子，通过描述子来进行匹配，优点准确度高，缺点是描述子计算量大。
光流法：在第一幅图中检测特征点，使用光流法(Lucas Kanade method)对这些特征点进行跟踪，得到这些特征点在第二幅图像中的位置，得到的位置可能和真实特征点所对应的位置有偏差。所以通常的做法是对第二幅图也检测特征点，如果检测到的特征点位置和光流法预测的位置靠近，那就认为这个特征点和第一幅图中的对应。在相邻时刻光照条件几乎不变的条件下（特别是单目slam的情形），光流法匹配是个不错的选择，它不需要计算特征描述子，计算量更小。

从本征矩阵恢复R、T，尺度scale的进一步分析

我们知道本征矩阵 $E$ 定义为 $E=[t]_{\times}R$ 或者 $\hat{t}R$

可以用下面的计算式从本征矩阵中恢复 $R$ 和 $t$ ， $\hat{t}$ 表示 $t$ 的反对称矩阵：
$R=UR_Z^T\left(\pm\frac{\pi}{2}\right)V^T,\quad\widehat{t}=UR_Z\left(\pm\frac{\pi}{2}\right)\Sigma U^T.$
其中 $R_Z\left(\frac{\pi}{2}\right)$ 表示绕Z轴旋转90度得到的旋转矩阵：
$\left.R_Z\left(+\frac{\pi}{2}\right)=\left[\begin{array}{ccc}0&-1&0\\1&0&0\\0&0&1\end{array}\right.\right]$
从 $R, t$ 的计算公式中可以看到 $R, t$ 都有两种情况，组合起来 $R, t$ 有4种组合方式。由于一组 $R, t$ 就决定了摄像机光心坐标系 $C$ 的位姿，所以选择正确 $R 、 t$ 的方式就是，把所有特征点的深度计算出来，看深度值是不是都大于0，深度都大于0的那组 $R, t$ 就是正确的。

在这里插入图片描述

但是特征点深度怎么计算出来呢？这部分的推导见。

这里我们有必要进一步的分析下尺度scale，从 $R ， t$ 的计算公式中，可以发现平移向量 $t$ 和 $E$ 的奇异值 $\Sigma$ 有关。再回顾一下之前求 $E$ 时候的方程以及提到的伸缩:
$\begin{array}{c}AE=AU\Sigma V=0\\\\ AsE=AUs\Sigma V=0\end{array}$
所以，之前推导过程中所有对 $E$ 进行的尺度伸缩实际上都是作用在奇异值 $\Sigma$ 上。

也就是说这个尺度scale实际作用的是两个相机之间的平移，图示如下：

在这里插入图片描述

这个图简单明了的演示了这种平移缩放作用。

从图中也可以看出，由于尺度scale的关系，不同的 $t$ ，决定了以后计算点 $P$ 的深度也是不同的，所以恢复的物体深度也是跟尺度scale有关的，这就是论文中常说的结构恢复 structure reconstruction，只是恢复了物体的结构框架，而不是实际意义的物体尺寸。并且要十分注意，每两对图像计算 $E$ 并恢复 $R, T$ 时，他们的尺度都不是一样的，本来是同一点，在不同尺寸下，深度不一样了，这时候地图map它最头痛了，所以这个尺度需要统一。

那么如何让scale之间统一呢？如果你一直采用这种2d-2d匹配计算位姿的方式，那每次计算的 $t$ 都是在不同尺度下的，Davide Scaramuzza的论文《Visual odometry I》的computing the relative scale那部分讲了一种方法使得相邻位姿间的不同的尺度s经过缩放进行统一。

我们已经知道出现尺度不一致是由于每次都是用这种计算本征矩阵的方式，而尺度就是在计算 $E$ 时产生的。

所以尺度统一的另一种思路就是后续的位姿估计我不用这种2d-2d计算本征的方式了，也就说你通过最开始的两帧图像计算 $E$ 恢复了 $R, t$ ，并通过三角法计算出了深度，那我就有了场景点的3D坐标，后续的视频序列就可以通过 3D to 2d (opencv里的solvePnp)来进行位姿估计，这样后续的视频序列就不用计算本征矩阵从而绕过了尺度，所以后续的视频序列的位姿和最开始的两帧的尺度是一样的了。

但是，这样计算又会产生新的问题–scale drift。因为，两帧间的位姿总会出现误差，这些误差积累以后，使得尺度不再统一了，如下图所示(借用@清华大学王京同学的图，很容易帮助理解)：

在这里插入图片描述

随着相机位姿误差的积累，地图中的四个点在第二帧的位置相对于第一帧中来说像是缩小了一样。位姿误差累计导致尺度漂移这一点，对照上面讲尺度不确定问题时的那个图就很容易理解。

关于如何纠正这个scale drift的问题很多单目slam里都提到了，所以这里不再深入。

如何确定scale

参考：

monoSLAM

The absolute scale can then be determined from direct measurements(e.g., measuring the size of an element in the scene),motion constraints,or from the integration with other sensors.
如果另一个传感器可以获得真实的尺度，那么就可以利用另一个传感器来获得这个scale。