视觉里程计：2D到2D，对极几何法

最新推荐文章于 2022-10-06 22:26:11 发布

Jason.Li_0012

最新推荐文章于 2022-10-06 22:26:11 发布

阅读量4.2k

点赞数 1

分类专栏：《视觉SLAM十四讲》笔记文章标签：计算机视觉算法自动驾驶

本文链接：https://blog.csdn.net/weixin_45929038/article/details/123529067

版权

《视觉SLAM十四讲》笔记专栏收录该内容

21 篇文章 19 订阅

订阅专栏

对于经特征匹配得到的两图像间配对的特征点，可恢复得到两帧图像间相机的运动。

对极约束

名词解释

在这里插入图片描述

如上图，对于两帧图像 $I_1、I_2$ ，P在其上投影分别为 $p_1、p_2$ 。相机的中心分别为 $O_1、O_2$ ，则有如下定义：

极平面（Epipolar plane） ：由 $O_1、O_2、P$ 组成的平面
极点（Epipoles） ： $O_1O_2$ 连线同像平面 $I_1、I_2$ 的交点 $e_1、e_2$
基线： $O_1O_2$ 连线
极线（Epipolar line） ：极平面同像平面的交线 $p_1e_1、p_2e_2$ ，记为 $l_1、l_2$

记两帧图像间的变换为 $T_{12}$ ，实际求解中，特征点 $p_1、p_2$ 通过特征匹配得到， $P、e_1、e_2$ 未知，待求变换 $T_{12}$ 。

像素坐标

对于世界坐标下某点 $P=\begin{bmatrix}X&Y&Z\end{bmatrix}^T$ ，由针孔相机模型可知其在图像坐标系下的像素坐标位置：
$s_1p_1 = KP\qquad s_2p_2=K(RP+t)$
其中， $K$ 为相机内参矩阵， $R 、 t$ 为两帧图像间的旋转、平移变换。

齐次坐标

通常采用齐次坐标表示像素坐标，也即一个向量同它自身乘以任意非零整数意义相同。通常用于表达一个投影变换。如 $s_1p_1$ 同 $p_1$ 成投影关系，他们在齐次坐标下意义相同。称其为尺度意义下相等，记作：
$sp\simeq p$
则可得：
$p_1\simeq KP\qquad p_2\simeq K(RP+t)$
取归一化平面上坐标 $x_1,x_2$ ：
$x_1=K^{-1}p_1\qquad x_2=K^{-1}p_2$
则可得：
$x_2 \simeq Rx_1+t$

对极约束

对上式两侧左乘 $t^\wedge$ 再左乘 $x_2^T$ ：
$t^\wedge x_2\simeq t^\wedge Rx_1\\ x_2^Tt^\wedge x_2\simeq x_2^T t^\wedge Rx_1$
对于上述公式左侧向量 $t^\wedge x_2$ 方向同 $t$ 以及 $x_2$ 垂直，故而再和 $x_2$ 进行内积计算时，结果为 $0$ ，由此对其进行公式简化：
$x_2^T t^\wedge Rx_1 = 0$
带入 $p_1、p_2$ ：
$p_2^TK^{-T}t^\wedge RK^{-1}p_1=0$
称其为对极约束，其中同时包含了旋转和平移两部分。其物理意义为刻画了 $O_1、O_2、P$ 共面的事实。

为简化公式，定义两个矩阵：基础矩阵（Fundamental Matrix）F和本质矩阵（Essential Matrix）E：
$E=t^\wedge R\qquad F=K^{-T}EK^{-1}$
则有：
$x_2^TEx_1=0\qquad p_2^TFp_1=0$
由此，相机位姿估计问题可分为如下两步：

根据匹配特征的像素位置求E或F
根据E或F求R，t

本质矩阵

本质矩阵 $E=t^\wedge R$ 是一个 $3\times 3$ 的矩阵，满足如下规则约束：

本质矩阵E的尺度等价：E乘以任意非零常数后，对极约束依然满足
本质矩阵E的内在性质：E的奇异值必然是 $\begin{bmatrix}\sigma&\sigma&0\end{bmatrix}^T$ 形式的
由于尺度等价性，E只有5个自由度（ $t^\wedge R$ 为6自由度）

八点法求解

本质矩阵E为 $3\times3$ 的矩阵，具有九个维度。此处仅考虑尺度等价性，则可用8对点估计本质矩阵E。

对于一对匹配的特征点，设其归一化坐标为 $x_1=\begin{bmatrix}u_1&v_1&1\end{bmatrix}$ 和 $x_2=\begin{bmatrix}u_2&v_2&1\end{bmatrix}$ ，则可根据对极约束得到：
$\begin{bmatrix}u_2&v_2&1\end{bmatrix}\begin{bmatrix}e_1&e_2&e_3\\e_4&e_5&e_6\\e_7&e_8&e_9\end{bmatrix}\begin{bmatrix}u_1\\v_1\\1\end{bmatrix}=0$
对本质矩阵E，将其展开写为向量形式：
$\mathscr{e}=\begin{bmatrix}e_1&e_2&e_3&e_4&e_5&e_6&e_7&e_8&e_9\end{bmatrix}^T$
则对极约束可写为关于 $e$ 的线性形式：
$\begin{bmatrix}u_2u_1&u_2v_1&u_2&v_2u_1&v_2v_1&v_2&u_1&v_1&1\end{bmatrix}\cdot \mathscr{e}=0$
同样，对所有8对匹配特征点做上述计算，则可得到如下线性方程组：
$\begin{bmatrix} u_2^1u_1^1&u_2^1v_1^1&u_2^1&v_2^1u_1^1&v_2^1v_1^1&v_2^1&u_1^1&v_1^1&1\\ u_2^2u_1^2&u_2^2v_1^2&u_2^2&v_2^2u_1^2&v_2^2v_1^2&v_2^2&u_1^2&v_1^2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots&\vdots\\ u_2^8u_1^8&u_2^8v_1^8&u_2^8&v_2^8u_1^8&v_2^8v_1^8&v_2^8&u_1^8&v_1^8&1 \end{bmatrix}\begin{bmatrix}e_1\\e_2\\e_3\\e_4\\e_5\\e_6\\e_7\\e_8\\e_9\end{bmatrix}=0$
其中， $u^iv^i$ 表示第 $i$ 对匹配的特征点。当系数矩阵满秩（Rank=8）时，E的各个元素可求解。此时e构成一条线，同尺度等价性一致。

通过求解上述线性方程组，即可得到本质矩阵E。

SVD分解

针对估计所得的本质矩阵E，计算对应的相机运动 $R 、 t$ ，应使用SVD分解进行求得。有关SVD分解的基础内容，可以学习如下文章：奇异值分解（SVD）

设本质矩阵E的SVD如下：
$E=U\Sigma V^T$
根据SVD定义：矩阵 $U 、 V$ 为正交阵， $\Sigma$ 为奇异值矩阵。由本质矩阵的内在性质知： $\Sigma=\mathrm{diag}(\sigma, \sigma, 0)$ ，则在SVD分解中，任意本质矩阵E具有两个可能的 $t 、 R$ 与之对应（此处直接给出了最终解，未推导）：
$t_1^\wedge=UR_Z(\frac{\pi}{2})\Sigma U^T\qquad R_1=UR_Z^T(\frac{\pi}{2})V^T\\ t_2^\wedge=UR_Z(-\frac{\pi}{2})\Sigma U^T\qquad R_2=UR_Z^T(-\frac{\pi}{2})V^T\\$
其中， $R_Z(\frac{\pi}{2})$ 表示绕Z轴旋转 $\frac{\pi}{2}$ 得到旋转矩阵。同时由于E和 $-$ E等价，对任意一个 $t$ 取负，可得相同结果。则存在四组可能的解：

在这里插入图片描述

如上图，用蓝色线表示相机，红色点（蓝线同黑线交点）表示空间点在相机上的投影。上述四种解中，仅第一种解中P在两个相机中都具备正向深度，故而可排除其余四种。

内在性质

根据线性方程组求解得到的E可能不满足其内在性质，也即奇异值不一定为 $\begin{bmatrix}\sigma&\sigma&0\end{bmatrix}^T$ 形式。

通常，刻意将 $\Sigma$ 矩阵调整为上述形式，也即对八点法求得的E进行SVD分解后，得到的奇异值矩阵 $\Sigma=\mathrm{diag}(\sigma_1, \sigma_2, \sigma_3)$ ，假设 $\sigma_1\ge \sigma_2\ge \sigma_3$ ，则取：
$E=U\:\mathrm{diag}(\frac{\sigma_1+\sigma_2}{2}, \frac{\sigma_1+\sigma_2}{2}, 0)\:V^T$
也即将求得的矩阵投影至E所在的流形上。

八点法存在的问题

尺度不确定性

由于E本身具备尺度等价性，分解计算得到的 $R 、 t$ 也具备尺度等价性。此时由于 $R\in SO(3)$ 存在约束，故而认为 $t$ 具备一个尺度。由此，对t进行归一化处理，使其长度为1。由于归一化，将直接导致单目视觉尺度不确定。

初始化纯旋转

若相机在初始化时发生的运动为纯旋转，也即由E分解得到的 $R 、 t$ 中 $t = 0$ 时，将导致无法直接求解 $R$

多于8对点时

当给定的匹配特征点多于8对时，可使用最小二乘计算对极约束，记系数矩阵为 $A$ 则有：
$A\mathscr{e}=0$
对于八点法， $A$ 大小为 $8\times 9$ 。多于8对点时可以构造超定方程：
$\min_e\begin{Vmatrix}A\mathscr{e}\end{Vmatrix}_2^2=\min_ee^TA^TAe$
从而，可以求解最小二乘意义下的E矩阵。当存在误匹配问题时，则可采用**随机采样一致性（RANSAC）**代替最小二乘进行计算。

单应矩阵

单应矩阵（Homography）H用于描述两平面间的映射关系。若场景内特征点都落至某一平面上（墙、地面），则可使用单应性估计运动。

对于像平面 $I_1、I_2$ 上匹配的两特征点 $p_1、p_2$ ，若特征点落在某平面P上：
$n^TP+d=0\\ -\frac{n^TP}{d}=1$
其中 $n$ 为平面的法向量，d为截距。同样在相机平面中，存在投影关系：
$\begin{aligned} p_2\simeq& K\Bigl(RP+t\Bigr)\\ \simeq& K\Bigl(RP+t\cdot1\Bigr)\\ \simeq& K\Bigl(RP+t\cdot(-\frac{n^TP}{d})\Bigr)\\ \simeq& K\Bigl(R-\frac{tn^T}{d}\Bigr)P\\ \simeq& K\Bigl(R-\frac{tn^T}{d}\Bigr)K^{-1}P_1 \end{aligned}$
由此得到对应匹配特征点间的变换关系：
$H=K\Bigl(R-\frac{tn^T}{d}\Bigr)K^{-1}$
称矩阵H为单应矩阵，为一个 $3\times 3$ 的矩阵。

将 $p_1、p_2$ 的归一化像素坐标带入：
$p_2\simeq Hp_1\\ \begin{bmatrix}u_2\\v_2\\1\end{bmatrix}\simeq \begin{bmatrix}h_1&h_2&h_3\\h_4&h_5&h_6\\h_7&h_8&h_9\end{bmatrix} \begin{bmatrix}u_1\\v_1\\1\end{bmatrix}$
符号 $\simeq$ 表示了尺度意义下的相等，故而矩阵H乘以任意非零常数依旧等价于其本身。实际处理中，取 $h_9=1$ 从而展开：
$h_1u_1+h_2v_1+h_3-h_7u_1u_2-h_8v_1u_2=u_2\\ h_4u_1+h_5v_1+h_6-h_7u_1v_2-h_8v_1v_2=v_2$
也即，一堆匹配特征点可以提供两个约束条件，则自由度为8的单应矩阵H只需要4对点即可求解得到：
$\begin{bmatrix} u_1^1&v_1^1&1&0&0&0&-u_1^1u_2^1&-v_1^1u_2^1\\ 0&0&0&u_1^1&v_1^1&1&-u_1^1v_2^1&-v_1^1v_2^1\\ u_1^2&v_1^2&1&0&0&0&-u_1^2u_2^2&-v_1^2u_2^2\\ 0&0&0&u_1^2&v_1^2&1&-u_1^2v_2^2&-v_1^2v_2^2\\ u_1^3&v_1^3&1&0&0&0&-u_1^3u_2^3&-v_1^3u_2^3\\ 0&0&0&u_1^3&v_1^3&1&-u_1^3v_2^3&-v_1^3v_2^3\\ u_1^4&v_1^4&1&0&0&0&-u_1^4u_2^4&-v_1^4u_2^4\\ 0&0&0&u_1^4&v_1^4&1&-u_1^4v_2^4&-v_1^4v_2^4\\ \end{bmatrix}\begin{bmatrix}h_1\\h_2\\h_3\\h_4\\h_5\\h_6\\h_7\\h_8\end{bmatrix}=\begin{bmatrix}u_2^1\\v_2^1\\u_2^2\\v_2^2\\u_2^3\\v_2^3\\u_2^4\\v_2^4\end{bmatrix}$
计算得到单应矩阵H后，应同本质矩阵一样，进一步分解计算 $R 、 t$