《视觉SLAM十四讲》学习笔记-对极约束问题

最新推荐文章于 2022-05-05 17:34:12 发布

teddyluo

最新推荐文章于 2022-05-05 17:34:12 发布

阅读量1.7k

点赞数 1

分类专栏： slam

slam 专栏收录该内容

20 篇文章 17 订阅

订阅专栏

对极约束

问题描述：求取两帧图像 $I_1, I_2$ 之间的运动。设第一帧到第二帧的运动为 $\mathbf{R}, \vec{t}$ ，其中心分别为 $O_1, O_2$ ， $I_1$ 中有一个点 $p_1$ 对应到 $I_2$ 的点为 $p_2$ . $P$ 为两个摄像机在远处的交点。 $O_1O_2P$ 称为极平面(Epipolar plane)， $O_1O_2$ 连线与相机平面交点称为极点(Epipoles), $O_1O_2$ 称为基线， $l_1,l_2$ 为极线(Epipolar line).

以第一帧的坐标系作为基准，设 $P$ 坐标为 $\mathbf{P}=[X, Y, Z]^\top$ , $p_1, p_2$ 位置为：

s 1 p 1 = K P, s 2 p 2 = K (R P + t ⃗)

$s_1p_1=\mathbf{K}\mathbf{P},~s_2p_2=\mathbf{K}(\mathbf{R}\mathbf{P} + \vec{t})$
其中

K K $\mathbf{K}$ 为相机内参,

R,t⃗ R , t → $\mathbf{R},\vec{t}$ 为坐标系的相机运动。转化为齐次等式：

p 1 = K P, p 2 = K (R P + t ⃗)

$p_1=\mathbf{K}\mathbf{P},~p_2=\mathbf{K}(\mathbf{R}\mathbf{P} + \vec{t})$
取

x⃗ 1=K−1p⃗ 1 x → 1 = K − 1 p → 1 $\vec{x}_1=\mathbf{K}^{-1}\vec{p}_1$ ,

x⃗ 2=K−1p⃗ 2 x → 2 = K − 1 p → 2 $\vec{x}_2=\mathbf{K}^{-1}\vec{p}_2$ ,则有：

x ⃗ 2 = R x ⃗ 1 + t ⃗

$\vec{x}_2 = \mathbf{R}\vec{x}_1 + \vec{t}$
两边左乘

t⃗ ∧ t → ∧ $\vec{t}^\wedge$ ，有：

t ⃗ \land x ⃗ 2 = t ⃗ \land R x ⃗ 1 (x ⃗ 2 t ⃗ = 0)

$\vec{t}^\wedge\vec{x}_2 = \vec{t}^\wedge \mathbf{R}\vec{x}_1~~(\vec{x}_2 \vec{t}=0)$
两边再左乘

x⃗ ⊤2 x → 2 ⊤ $\vec{x}_2^\top$ ：

x ⃗ ⊤ 2 t ⃗ \land x ⃗ 2 = x ⃗ ⊤ 2 t ⃗ \land R x ⃗ 1

$\vec{x}_2^\top\vec{t}^\wedge\vec{x}_2 = \vec{x}_2^\top\vec{t}^\wedge \mathbf{R}\vec{x}_1$
因为

t⃗ ∧x⃗ 2 t → ∧ x → 2 $\vec{t}^\wedge\vec{x}_2$ 与

t⃗ ∧ t → ∧ $\vec{t}^\wedge$ 和

x⃗ 2 x → 2 $\vec{x}_2$ 皆为垂直，所以左侧为0：

x ⃗ ⊤ 2 t ⃗ \land R x ⃗ 1 = 0

$\vec{x}_2^\top\vec{t}^\wedge \mathbf{R}\vec{x}_1 = 0$
再代入

x1,x2 x 1 , x 2 $x_1, x_2$ 得到：

(K - 1 p ⃗ 2) ⊤ t ⃗ \land R K - 1 p ⃗ 1 = 0

$(\mathbf{K}^{-1}\vec{p}_2)^\top\vec{t}^\wedge \mathbf{R}\mathbf{K}^{-1}\vec{p}_1 = 0$
即：

p ⃗ ⊤ 2 K - ⊤ t ⃗ \land R K - 1 p ⃗ 1 = 0

$\vec{p}_2^\top \mathbf{K}^{-\top} \vec{t}^\wedge \mathbf{R}\mathbf{K}^{-1}\vec{p}_1 = 0$

此式即为对极约束，几何意义为 $O_1, O_2, P$ 共面。
将中间拆为基础矩阵和本质矩阵，可简化约束为:

E = t ⃗ \land R, R = K - T E K - 1, x ⃗ ⊤ 2 E x ⃗ 1 = p ⃗ ⊤ 2 F p ⃗ 1 = 0

$\mathbf{E}=\vec{t}^\wedge\mathbf{R}, \mathbf{R} = \mathbf{K}^{-T}\mathbf{E}\mathbf{K}^{-1}, \vec{x}_2^\top\mathbf{E}\vec{x}_1=\vec{p}_2^\top\mathbf{F}\vec{p}_1=0$
上式中，

E E $\mathbf{E}$ 为 本质矩阵(Essential Matrix),

F F $\mathbf{F}$ 为 基础矩阵(Fundamental Matrix).所以相机位势估计问题变为：

根据配对点的像素位置, 求出 $\mathbf{E}$ 或 $\mathbf{F}$ ;
根据 $\mathbf{E}$ 或 $\mathbf{F}$ , 求出 $\mathbf{R}, \vec{t}$ .

本质矩阵 $\mathbf{E}$ 性质：

尺度等价性： $\mathbf{E}$ 在不同尺度下等价
内在性质： $\mathbf{E}$ 的奇异值必定是 $[\rho, \rho, 0]$ 的形式
$\vec{t}^\wedge\mathbf{R}$ 有6个自由度，而 $\mathbf{E}$ 有5个自由度

如何求解本质矩阵 $\mathbf{E}$ :

方法一：因为 $\mathbf{E}$ 有五个自由度，说明可以用五对点来求解 $\mathbf{E}$ 。但 $\mathbf{E}$ 的内在性质是非线性的，用线性的方程求解会带来问题。
方法二：从尺度等价性出发，用八对点来解方程。

八对点求解本质矩阵 $\mathbf{E}$

考虑一对匹配点,它们的归一化坐标为 $\vec{x}_1 = [u_1, v_1, 1]^\top$ , $\vec{x}_2 = [u_2, v_2, 1]^\top$ ，根据对极约束有：

[u 1, v 1, 1] ⊤ ⎡ ⎣ ⎢ e 1 e 4 e 7 e 2 e 5 e 8 e 3 e 6 e 9 ⎤ ⎦ ⎥ [u 2, v 2, 1] ⊤

$[u_1, v_1, 1]^\top \begin{bmatrix} e_1 & e_2 & e_3\\ e_4 & e_5 & e_6\\ e_7 & e_8 & e_9 \end{bmatrix} [u_2, v_2, 1]^\top$
把

E E $\mathbf{E}$ 展开成向量表示，

e⃗ =[e1,⋯,e9]⊤ e → = [ e 1 , ⋯ , e 9 ] ⊤ $\vec{e}=[e_1,\cdots, e_9]^\top$ ,则线性方程为：

[u 1 u 2, u 1 v 2, u 1, v 1 u 2, v 1 v 2, v 1, u 2, v 2, 1] \cdot e ⃗ = 0 ⃗

$[u_1u_2, u_1v_2, u_1, v_1u_2, v_1v_2, v_1, u_2, v_2, 1]\cdot \vec{e} = \vec{0}$
对其他点对，也有类似表示。把这8个点对的方程放在一起可组成一个线性方程：

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ u 11 u 12 u 21 u 22 u 31 u 32 ⋮ u 81 u 82 u 11 v 12 u 21 v 22 u 31 v 32 ⋮ u 81 v 82 u 11 u 21 u 31 ⋮ u 81 v 11 u 12 v 21 u 22 v 31 u 32 ⋮ v 81 u 82 v 11 v 12 v 21 v 22 v 31 v 32 ⋮ v 81 v 82 v 11 v 21 v 31 ⋮ v 81 u 12 u 22 u 32 ⋮ u 82 v 12 v 22 v 32 ⋮ v 82 111 ⋮ 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ e ⃗ = 0 ⃗

$\begin{bmatrix} u_1^1u_2^1 & u_1^1v_2^1& u_1^1& v_1^1u_2^1& v_1^1v_2^1& v_1^1& u_2^1& v_2^1& 1\\ u_1^2u_2^2 & u_1^2v_2^2& u_1^2& v_1^2u_2^2& v_1^2v_2^2& v_1^2& u_2^2& v_2^2& 1\\ u_1^3u_2^3 & u_1^3v_2^3& u_1^3& v_1^3u_2^3& v_1^3v_2^3& v_1^3& u_2^3& v_2^3& 1\\ \vdots & \vdots & \vdots& \vdots & \vdots & \vdots& \vdots& \vdots & \vdots\\ u_1^8u_2^8 & u_1^8v_2^8& u_1^8& v_1^8u_2^8& v_1^8v_2^8& v_1^8& u_2^8& v_2^8& 1 \end{bmatrix} \vec{e}=\vec{0}$
则问题变为： 如何根据已估得的本质矩阵 $\mathbf{E}$ ，恢复得到 $\mathbf{R}$ 和 $\vec{t}$ .

不妨设 $\mathbf{E}$ 的SVD分解为:

E = U Σ V ⊤

$\mathbf{E} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top$
其中

U U $\mathbf{U}$ 和

V V $\mathbf{V}$ 为正交阵，

Σ Σ $\mathbf{\Sigma}$ 为奇异值矩阵，根据之前的推断

Σ=diag(ρ,ρ,0) Σ = d i a g ( ρ , ρ , 0 ) $\Sigma=diag(\rho, \rho, 0)$ ,可知对任一

E E $\mathbf{E}$ 存在两个可能的

R R $\mathbf{R}$ 和

t⃗ t → $\vec{t}$ 与之对应：

t ⃗ \land 1 = U R Z (π 2) Σ U ⊤, R 1 = U R ⊤ Z (π 2) V ⊤ t ⃗ \land 2 = U R Z (- π 2) Σ U ⊤, R 2 = U R ⊤ Z (- π 2) V ⊤

$\begin{aligned} \vec{t}_1^\wedge=\mathbf{U}\mathbf{R}_Z(\frac{\pi}{2})\mathbf{\Sigma} \mathbf{U}^\top, ~~\mathbf{R}_1 = \mathbf{U}\mathbf{R}_Z^\top(\frac{\pi}{2}) \mathbf{V}^\top\\ \vec{t}_2^\wedge=\mathbf{U}\mathbf{R}_Z(-\frac{\pi}{2})\mathbf{\Sigma} \mathbf{U}^\top, ~~\mathbf{R}_2 = \mathbf{U}\mathbf{R}_Z^\top(-\frac{\pi}{2}) \mathbf{V}^\top \end{aligned}$
式中

RZ(π2) R Z ( π 2 ) $\mathbf{R}_Z(\frac{\pi}{2})$ 表示为沿Z轴旋转90度得到的旋转矩阵。此外，由于-

E E $\mathbf{E}$ 和

E E $\mathbf{E}$ 等价，对任意的

t⃗ t → $\vec{t}$ 取负号也会得到同样结果。所以从

E E $\mathbf{E}$ 分解到

R R $\mathbf{R}$ 和

t⃗ t → $\vec{t}$ 时一共存在四个可能的解。

后续为了检查哪个解是正确的时候，可以把任意一点代入四个解中，当该点在两个相机下的深度皆为正的深度时(即解为正数)，即可确认该解是所述问题的正确的解。

剩下的问题：如何确认解出的 $\mathbf{E}$ 满足内在性质？

假设对 $\mathbf{E}$ 做SVD分解后，奇异值矩阵 $\Sigma=diag(\rho_1, \rho_2, \rho_3)$ , 不妨设 $\rho_1\geq \rho_2\geq \rho_3$ ，构造：

E = U d i a g (ρ 1 + ρ 2 2, ρ 2 + ρ 3 2, 0) V ⊤

$\mathbf{E} = \mathbf{U}diag\left(\frac{\rho_1+\rho_2}{2}, \frac{\rho_2 + \rho_3}{2}, 0\right) \mathbf{V}^\top$
即把求出的矩阵投影到

E E $\mathbf{E}$ 的流形上，即可保证其满足内在性质。更简单的做法是直接将奇异值矩阵取为diag(1, 1, 0)使得

E E $\mathbf{E}$ 具有尺度等价性。

单应矩阵

单应矩阵(Homography) $\mathbf{H}$ :描述了两个平面之间的映射关系。它描述了处于共同平面上的一些点在两张图像之间的变换关系。
假设图像 $I_1$ 和 $I_2$ 有匹配好的点 $p_1$ 和 $p_2$ ，这些特征点落在平面上，平面满足方程：

n ⃗ ⊤ P + d = 0

$\vec{n}^\top\mathbf{P} + d = 0$
整理有：

- n ⃗ ⊤ P d = 1

$-\frac{\vec{n}^\top\mathbf{P}}{d} = 1$
则：

p ⃗ 2 = K (R P + t ⃗) = K (R P + t ⃗ \cdot (- n ⃗ ⊤ P d)) = K (R - t ⃗ n ⃗ ⊤ d) P = K (R - t ⃗ n ⃗ ⊤ d) K - 1 p 1

$\begin{aligned} \vec{p}_2&=\mathbf{K}(\mathbf{RP} + \vec{t})\\ &=\mathbf{K}(\mathbf{RP} + \vec{t}\cdot (-\frac{\vec{n}^\top\mathbf{P}}{d} ) )\\ &=\mathbf{K}(\mathbf{R} - \frac{\vec{t}\vec{n}^\top}{d} )\mathbf{P}\\ &=\mathbf{K}(\mathbf{R} - \frac{\vec{t}\vec{n}^\top}{d} )\mathbf{K}^{-1}p_1 \end{aligned}$
这是一个关于图像坐标

p1 p 1 $p_1$ 和

p2 p 2 $p_2$ 的变换，为方便把中间这部记为

H H $\mathbf{H}$ , 于是：

p ⃗ 2 = H p ⃗ 1

$\vec{p}_2 = \mathbf{H}\vec{p}_1$
为求解

H H $\mathbf{H}$ ，类似于

E E $\mathbf{E}$ 的做法，将上式展开：

⎡ ⎣ ⎢ u 2 v 2 1 ⎤ ⎦ ⎥ = ⎡ ⎣ ⎢ h 1 h 4 h 7 h 2 h 5 h 8 h 3 h 6 h 9 ⎤ ⎦ ⎥ ⎡ ⎣ ⎢ u 1 v 1 1 ⎤ ⎦ ⎥

$\begin{bmatrix} u_2\\v_2\\1 \end{bmatrix} =\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}$
为简化问题，实际上常常乘以一个非0因子使得

h9=1 h 9 = 1 $h_9=1$ ,即乘上

1/(h7u1+h8v1+h9) 1 / ( h 7 u 1 + h 8 v 1 + h 9 ) $1/(h_7u_1+h_8v_1+h_9)$ ，再次展开得到式子：

u 2 = h 1 u 1 + h 2 v 1 + h 3 h 7 u 1 + h 8 v 1 + h 9 v 2 = h 4 u 1 + h 5 v 1 + h 6 h 7 u 1 + h 8 v 1 + h 9

$\begin{aligned} u_2 = \frac{h_1u_1 + h_2v_1 + h_3}{h_7u_1+h_8v_1+h_9}\\ v_2 = \frac{h_4u_1 + h_5v_1 + h_6}{h_7u_1+h_8v_1+h_9} \end{aligned}$
注意到

h9=1 h 9 = 1 $h_9=1$ , 整理后可得到：

u 2 = (h 1 u 1 + h 2 v 1 + h 3) - (h 7 u 1 + h 8 v 1) u 2 v 2 = (h 4 u 1 + h 5 v 1 + h 6) - (h 7 u 1 + h 8 v 1) v 2

$\begin{aligned} u_2 = (h_1u_1 + h_2v_1 + h_3)-(h_7u_1+h_8v_1)u_2\\ v_2 = ({h_4u_1 + h_5v_1 + h_6})-({h_7u_1+h_8v_1})v_2 \end{aligned}$
由此可见一对匹配点可构造两项约束，于是自由度为8的单应矩阵可通过4对匹配点来算出。构造方程如下：

⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ u 11 0 u 21 0 u 31 0 u 41 0 v 11 0 v 21 0 v 31 0 v 41 0 10101010 0 u 11 0 u 21 0 u 31 0 u 41 0 v 11 0 v 21 0 v 31 0 v 41 01010101 - u 11 u 12 - u 11 v 12 - u 21 u 22 - u 21 v 22 - u 31 u 32 - u 31 v 32 - u 41 u 42 - u 41 v 42 v 11 u 12 - v 11 v 12 v 21 u 22 - v 21 v 22 v 31 u 32 - v 31 v 32 v 41 u 42 - v 41 v 42 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ u 12 v 12 u 22 v 22 u 32 v 32 u 42 v 42 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$\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 H $\mathbf{H}$ .此法称为直接线性变换法(Direct Linear Transform).

同本质矩阵相似，为验证 $\mathbf{H}$ ，对 $\mathbf{H}$ 做SVD分解后可得到四组旋转矩阵与向量，考虑以下事实：

成像的地图点的深度是否全为正值？若是可排除两组解。
场景中的平面的法向量。若场景平面与相机平面平行，又可排除一组解，其法向量 $\vec{n}$ 的理论值应为 $\vec{1}$ .
可以看到最后的解是通过场景事实筛选而获得的。

问题讨论：

尺度不确定性问题：对 $\vec{t}$ 归一化时，会导致单目视觉的尺度不确定性(Scale Ambiguity). 当对两张图的 $\vec{t}$ 归一化时，相当于固定了尺度，即 $\vec{t}$ 的单位为1,称之为单目SLAM的初始化。要求初始化的两张图像须有一定程度的平移。
初始化的纯旋转问题：若相机发生的是纯旋转，导致 $\vec{t}$ 为零，会导致无法求解 $\mathbf{R}$ . 因而要求：单目初始化不能只有纯旋转，必须要有一定程度的平移。
多于八对点的情况：不妨设线性化后的对极约束等式中，左侧的系数矩阵为 $\mathbf{A}$ :
$A e ⃗ = 0 ⃗$ $\mathbf{A}\vec{e} = \vec{0}$
只用八点法的话， $\mathbf{A}$ 的大小为8*9；当多于八对点时，可以通过最小化二次型来求解：
$min e ⃗ ∥ A e ⃗ ∥ 22 = min e ⃗ e ⃗ ⊤ A ⊤ A e ⃗$ $\underset{\vec{e}}{\min} \|\mathbf{A}\vec{e}\|^2_2 = \underset{\vec{e}}{\min} \vec{e}^\top \mathbf{A}^\top\mathbf{A}\vec{e}$
这样就求出了在最小二乘意义下的矩阵。也可以通过Random Sample Concensus(随机采样一致性)来求解。