视觉SLAM前端——对极约束、本质矩阵与单应矩阵

考拉AI

已于 2023-02-13 13:16:36 修改

阅读量353

点赞数 1

分类专栏：视觉SLAM 文章标签：计算机视觉人工智能

于 2022-11-17 17:44:32 首次发布

本文链接：https://blog.csdn.net/qq_17449077/article/details/127907562

版权

视觉SLAM 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

如果我们已知多组匹配特征点对的2D/2D像素坐标，那么此时可以使用对极几何或者单应性求出相机的相对运动，其典型的应用场景就是在单目视觉SLAM的初始化的时候。但对极几何要求相机必须是移动的，单应性要求特征点必须在同一平面上，正是由于这些局限性，单目SLAM的初始化是一件非常困难的事情，一旦我们初始化完成之后，由于此时相机的运动已知，我们就可以三角化出特征点的3D坐标，当新的一帧进来之后，我们可以匹配出多对2D/3D特征点对，此时就可以使用PnP来求解相机运动。

对极约束

在这里插入图片描述

如图所示（图片取自视觉SLAM十四讲）， $I_1$ 和 $I_2$ 为两帧图像， $O_1$ 和 $O_2$ 为相机中心位置。 $p_1$ 为图像 $I_1$ 中的某个特征点， $p_2$ 为图像 $I_2$ 中与 $p_1$ 匹配的特征点。射线 $O_1p_1$ 上的点都会投影到像素点 $p_1$ ，射线 $O_2p_2$ 同理。在匹配正确的情况下，由于 $p_1$ 和 $p_2$ 对应的是空间中的同一点，所以射线 $O_1p_1$ 和 $O_2p_2$ 必相交于点P。面 $O_1O_2P$ 称为极平面； $O_1O_2$ 称为基线；基线与两个成像平面 $I_1$ 和 $I_2$ 分别相交于点 $e_1$ 和 $e_2$ ，称为极点；极平面与两个成像平面 $I_1$ 和 $I_2$ 分别相交于线段 $l_1$ 、 $l_2$ ，称为极线。
由相机模型有 $s_1p_1=KP$ $s_2p_2=K(RP+t)$ $K$ 为相机内参， $p_1$ 和 $p_2$ 为齐次像素坐标。如果用 $x_1$ 和 $x_2$ 分别表示 $P$ 点在两个相机下的归一化坐标，那么 $x_1=K^{-1}p_1, x_2=K^{-1}p_2$ ，因此 $s_2x_2=Rs_1x_1+t$ 等式两边对 $t$ 做外积并左乘 $x_2^T$ 得 $s_2x_2^Tt^{\land}x_2=s_1x_2^Tt^{\land}Rx_1+x_2^Tt^{\land}t$ 因为正交向量内积为零，同方向向量外积为零，因为一三项等于零，可得对极约束公式：
$x_2^Tt^{\land}Rx_1=0$ $p_2^TK^{-T}t^\land RK^{-1}p_1=0$
令 $E=t^{\land}R$ $，$ $F=K^{-T}t^\land RK^{-1}$ ，那么 $E$ 和 $F$ 分别称为本质矩阵和基础矩阵。

本质矩阵

本质矩阵具有以下三个特点：任意系数倍的 $E$ 依然满足对极约束，所以 $E$ 在不同尺度下是等价的； $E$ 的奇异值满足 $[\sigma,\sigma,0]$ 的形式，称为本质矩阵的内在性质； $E$ 的自由度为5（旋转3+平移3-尺度等价约束1）。如果我们只考虑尺度等价的约束，那么 $E$ 的自由度为8，可以使用8对点来求解 $E$ 。
$\begin{array}{c} \begin{array}{c} \end{array}\left(\begin{array}{ccccccccc} u_{2}^{1} u_{1}^{1} & u_{2}^{1} v_{1}^{1} & u_{2}^{1} & v_{2}^{1} u_{1}^{1} & v_{2}^{1} v_{1}^{1} & v_{2}^{1} & u_{1}^{1} & v_{1}^{1} & 1 \\ u_{2}^{2} u_{1}^{2} & u_{2}^{2} v_{1}^{2} & u_{2}^{2} & v_{2}^{2} u_{1}^{2} & v_{2}^{2} v_{1}^{2} & v_{2}^{2} & u_{1}^{2} & v_{1}^{2} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{2}^{8} u_{1}^{8} & u_{2}^{8} v_{1}^{8} & u_{2}^{8} & v_{2}^{8} u_{1}^{8} & v_{2}^{8} v_{1}^{8} & v_{2}^{8} & u_{1}^{8} & v_{1}^{8} & 1 \end{array}\right)\left(\begin{array}{l} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{array}\right) = 0 \end{array}$
如果8对点的归一化坐标 $u_i$ 、 $v_i$ 构成的矩阵满秩，即可求出 $E$ 。求出 $E$ 之后，可以使用SVD分解，求出 $R$ 和 $t$ 。设 $E=U\Sigma V^T$ ，其中 $U$ 、 $V$ 为正交矩阵， $\Sigma=diag(\sigma_1,\sigma_2,\sigma_3)$ 。由于八点法没有考虑 $E$ 的内在性质，所以 $\Sigma$ 不满足 $E$ 的奇异值形式。假设 $\sigma_1>\sigma_2>\sigma_2$ ，我们可以构造一个新的 $E=Udiag((\sigma_1+\sigma_2)/2,(\sigma_1+\sigma_2)/2,0) V^T$ ，然后可以分解出两个 $R$ 、 $t$ 分别为：
$\begin{array}{ll} \boldsymbol{t}_{1}^{\wedge}=\boldsymbol{U} \boldsymbol{R}_{Z}\left(\frac{\pi}{2}\right) \boldsymbol{\Sigma} \boldsymbol{U}^{\mathrm{T}}, \quad \boldsymbol{R}_{1}=\boldsymbol{U} \boldsymbol{R}_{Z}^{\mathrm{T}}\left(\frac{\pi}{2}\right) \boldsymbol{V}^{\mathrm{T}} \\ \boldsymbol{t}_{2}^{\wedge}=\boldsymbol{U} \boldsymbol{R}_{Z}\left(-\frac{\pi}{2}\right) \boldsymbol{\Sigma} \boldsymbol{U}^{\mathrm{T}}, \quad \boldsymbol{R}_{2}=\boldsymbol{U} \boldsymbol{R}_{Z}^{\mathrm{T}}\left(-\frac{\pi}{2}\right) \boldsymbol{V}^{\mathrm{T}} \end{array}$
由于尺度等价性， $- E$ 也是满足对极约束的一个解。可分解得到两个 $R$ 、 $t$ ，其中 $R$ 与 $E$ 分解的相同， $t$ 与 $E$ 分解的相反。所以可以得到共4个结果。我们把4个结果分别代入三角化可以求得 $P$ 点在两个相机坐标系下的深度，满足深度为正的条件的解才是最终的解。

单应矩阵

如果所有的特征点都处于现实世界的同一平面之内，那么此时需要使用单应性来估计相机的运动。假设平面方程为 $n^TP+d=0$ ，其中 $n$ 为平面法向量， $d$ 为平面到相机原点的距离。
$s_2p_2=K(RP+t)=K(RP-\frac {n^TPt} d)=K(R- \frac {n^Tt} d )P=s_1K(R- \frac {n^Tt} d )K^{-1}p_1$
令 $\frac {n^Tt} d )K^{-1}$ ，则矩阵 $H$ 称为单应矩阵，其描述了处于同一平面上的空间点在两幅图像间的变换关系，那么 $p_2\simeq Hp_1$ 。考虑尺度等价性，那么单应矩阵的自由度为8，每一对点可以提供两个约束方程，那么使用4对不共线的点即可求出 $H$ ，如果设 $h_9=1$ 那么可得到如下展开式：
$\left(\begin{array}{cccccccc} u_{1}^{1} & v_{1}^{1} & 1 & 0 & 0 & 0 & -u_{1}^{1} u_{2}^{1} & -v_{1}^{1} u_{2}^{1} \\ 0 & 0 & 0 & u_{1}^{1} & v_{1}^{1} & 1 & -u_{1}^{1} v_{2}^{1} & -v_{1}^{1} v_{2}^{1} \\ u_{1}^{2} & v_{1}^{2} & 1 & 0 & 0 & 0 & -u_{1}^{2} u_{2}^{2} & -v_{1}^{2} u_{2}^{2} \\ 0 & 0 & 0 & u_{1}^{2} & v_{1}^{2} & 1 & -u_{1}^{2} v_{2}^{2} & -v_{1}^{2} v_{2}^{2} \\ u_{1}^{3} & v_{1}^{3} & 1 & 0 & 0 & 0 & -u_{1}^{3} u_{2}^{3} & -v_{1}^{3} u_{2}^{3} \\ 0 & 0 & 0 & u_{1}^{3} & v_{1}^{3} & 1 & -u_{1}^{3} v_{2}^{3} & -v_{1}^{3} v_{2}^{3} \\ u_{1}^{4} & v_{1}^{4} & 1 & 0 & 0 & 0 & -u_{1}^{4} u_{2}^{4} & -v_{1}^{4} u_{2}^{4} \\ 0 & 0 & 0 & u_{1}^{4} & v_{1}^{4} & 1 & -u_{1}^{4} v_{2}^{4} & -v_{1}^{4} v_{2}^{4} \end{array}\right)\left(\begin{array}{l} h_{1} \\ h_{2} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \end{array}\right)=\left(\begin{array}{c} 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{array}\right)$
求出单应矩阵后通过分解同样可以得到四组 $R$ 、 $t$ ，通过验证深度为正可以过滤两组解，然后通过假设场景平面与相机平面平行等先验假设来获得最终解。在单目相机初始化的时候，我们通常会同时计算单应矩阵和基础矩阵，来估计相机运动，选择重投影误差小的估计作为最终的运动估计。由于尺度等价的存在，分解出来的 $R$ 和 $t$ 也是尺度等价的，所以我们通常归一化 $t$ 或者令初始化时所有特征点平均深度为1来固定尺度。