视觉SLAM十四讲学习5 位姿估计（2）本质矩阵的计算

最新推荐文章于 2023-01-10 19:24:44 发布

RuiH.AI

最新推荐文章于 2023-01-10 19:24:44 发布

阅读量3.6k

点赞数

分类专栏： SLAM 文章标签：矩阵线性代数算法

本文链接：https://blog.csdn.net/qq_41035283/article/details/121921573

版权

SLAM 专栏收录该内容

15 篇文章 18 订阅

订阅专栏

视觉SLAM十四讲学习5 位姿估计（2）本质矩阵的计算

前言
本质矩阵的性质
本质矩阵计算
- 五点法
- 八点法
位姿计算
对极几何计算本质矩阵的问题
后记

前言

本篇详细推导对极几何中，本质矩阵的计算。

本质矩阵的性质

根据上篇的内容，由对极几何约束获得相邻帧相对位姿与匹配点坐标的关系：
$P_2^Tt^\land RP_1 = 0 \\ \quad \\ p_2^TK^{-T}t^\land R K^{-1}p_1 = 0$

①对于本质矩阵 $E=t^\land R$ ，原本有旋转+位移六个自由度，但是等式右边为零，乘以任意尺度等式都成立，因此方程缺少尺度约束，减去一个自由度，共五个自由度。

②本质矩阵有一个内在性质：奇异值必为 ${\sigma,\sigma,0}$ 的形式。

性质②需要进行证明：

首先，对于奇数阶反对称矩阵 $A$ ，必不满秩：
$A^T=-A \\ |A^T|=|-A|\\ |A|=(-1)^n|A|=-|A| \to |A|=0$
任意 $n$ 阶反对称矩阵合同于矩阵
$diag\{\begin{bmatrix}0 & 1 \\ -1 & 0 \end{bmatrix} , \begin{bmatrix}0 & 1 \\ -1 & 0 \end{bmatrix}, \dots \begin{bmatrix}0 & 1 \\ -1 & 0 \end{bmatrix}, 0, 0 ,\dots , 0 \}$
并正交相似于
$diag\{\begin{bmatrix}0 & a_1 \\ -a_1 & 0 \end{bmatrix} , \begin{bmatrix}0 & a_2 \\ -a_2 & 0 \end{bmatrix}, \dots \begin{bmatrix}0 & a_r \\ -a_r & 0 \end{bmatrix}, 0, 0 ,\dots , 0 \}$
因此，反对称矩阵的秩必为偶数，则三阶非零反对称矩阵 $A$ 的秩必为2，可将 $A$ 分解：
$\begin{bmatrix}0 & a_1 & 0 \\ -a_1 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix}U^T,UU^T=E \\ \quad \\ Z= \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0\\ \end{bmatrix} \\ \quad \\ W = \begin{bmatrix} 0&1&0 \\ -1&0&0\\ 0&0&1\\ \end{bmatrix},WW^T=E \\ \quad \\ Z=W diag\{1,1,0 \} \\ \quad \\ A=a_1UZU^T=a_1UWdiag\{1,1,0 \}U^T \\ \quad \\ 注意，当Z=W^Tdiag\{-1,-1,0\} \quad (1) \\ \quad \\ A=-a_1UZU^T=a_2UW^Tdiag\{1,1,0 \}U^T也满足分解要求$

由本质矩阵 $E=t^\land R$ ， $t^\land$ 是一个反对称矩阵，可得：
$t^\land = \sigma UWdiag\{1,1,0 \}U^T \\ E = UWdiag\{\sigma,\sigma,0 \}U^TR \\ \quad \\ \because UW(UW)^T=E,U^TR(U^TR)^T=E \\ UW=U^*,U^TR=(V^*)^T\\ \therefore E = U^* diag\{\sigma,\sigma,0\}(V^*)^T$
即 $E$ 的SVD分解得到的奇异值为 $\{\sigma,\sigma,0 \}$ 的形式。

本质矩阵计算

五点法

由于本质矩阵自由度为5，因此最少需要五对匹配点就可以计算出 $E$ 。五点法计算比较复杂，后面有时间我再补充。

八点法

如果只考虑本质矩阵的尺度等价性，则 $E$ 具有八个自由度，需要八对匹配点计算出 $E$ 。对于一对匹配点 $P_1=\{x_1,y_1,1\},\{x_2,y_2,1\}$ ，由对极几何约束有：
$\begin{bmatrix} x_2 & y_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} x_1 \\ y_1 \\ 1 \end{bmatrix} = 0 \\ \quad \\ \to \begin{bmatrix} x_2x_1,x_2y_1,x_2,y_2x_1,y_2y_1,y_2,x_1,y_1,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 \\ \quad \\$

则对于八对匹配点而言，形成了一个线性齐次方程组 $Ax=0，A\in {\bf R}^{n\times n}$ 。如果 $A$ 行满秩， $d i m N U L (A) = 9 - r a n k (A) = 1$ ，即零空间是一维直线，刚好满足本质矩阵的尺度不确定性。

对于欠定方程组，可以采用SVD来解方程，如数值计算之最小二乘法（3）最小二乘的矩阵解法所示。

位姿计算

通过SVD解方程，获得本质矩阵 $E$ 后，需要解算位姿 $R, t$ ，根据上面的证明，将 $E$ 进行奇异值分解：
$U^* diag\{\sigma,\sigma,0\}(V^*)^T=UW\{\sigma,\sigma,0\} U^TR \\ \quad \\ 奇异值分解可得U^*,V^*,\sigma \\ \quad \\ U^*=UW,W = \begin{bmatrix} 0&1&0 \\ -1&0&0\\ 0&0&1\\ \end{bmatrix}, V^*=(U^TR)^T \\ \quad \\ U=U^*W^T \\ R=U^*W^T(V^*)^T \\ t^\land = U^*\{\sigma,\sigma,0\}W(U^*)^T$

关于《十四讲》中的四种情况，其原因是根据式(1)， $W,W^T$ 都能满足本质矩阵奇异值为 $\{\sigma,\sigma,0\}$ 的形式，导致 $E$ 的分解有两种形式；而通过欠定方程求 $E$ 时，因为缺少尺度约束，可以得到 $E$ 和 $- E$ 两种解，因此最后出现了四种 $R, t$ 可能。

在这里插入图片描述
这时，只要把其中一个匹配点代入四个解确定的相机位姿模型，得到两个相机坐标的深度都为正时，表示该解正确。

对极几何计算本质矩阵的问题

①由于本质矩阵缺乏尺度约束，因此求出来的 $R, t$ 也是没有尺度的。不过旋转矩阵 $R$ 在正交群中，自身有一个 $∣ R ∣ = 1$ 的约束； $t$ 的尺度没有任何约束，因此单目求出的 $t$ 是没有尺度的。

也就是说，单目给出的 $t = 1$ ，可能指1m，1cm，10cm；而 $t = 2$ ，就是对应尺度的2倍。

②因为 $t$ 没有尺度，因此在单目SLAM中，需要在开始时进行尺度初始化，将初始化两帧之间的 $t$ 作为之后SLAM的尺度。因此，初始化的两帧不能是纯旋转。

③匹配点多于八组。通常ORB提取和匹配到的点数远多于8组。此时可以先通过RANSAC将误匹配点去除，然后通过线性最小二乘计算最优本质矩阵。

④求出的本质矩阵奇异值不满足 $\sigma,\sigma,0$ 的形式。假设得到的奇异值为 $\sigma_1\ge \sigma_2\ge \sigma_3$ ，可以将奇异值调整为 $(\sigma_1+\sigma_2)/2,(\sigma_1-\sigma_2)/2,0$ ，或者直接设定为 $1, 1, 0$ 的形式。