视觉里程计:2D到2D,对极几何法

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

对极约束

名词解释

视觉里程计:2D到2D,对极几何法_第1张图片

如上图,对于两帧图像 I 1 、 I 2 I_1、I_2 I1I2,P在其上投影分别为 p 1 、 p 2 p_1、p_2 p1p2。相机的中心分别为 O 1 、 O 2 O_1、O_2 O1O2,则有如下定义:

  • 极平面(Epipolar plane) :由 O 1 、 O 2 、 P O_1、O_2、P O1O2P组成的平面
  • 极点(Epipoles) O 1 O 2 O_1O_2 O1O2连线同像平面 I 1 、 I 2 I_1、I_2 I1I2的交点 e 1 、 e 2 e_1、e_2 e1e2
  • 基线 O 1 O 2 O_1O_2 O1O2连线
  • 极线(Epipolar line) :极平面同像平面的交线 p 1 e 1 、 p 2 e 2 p_1e_1、p_2e_2 p1e1p2e2,记为 l 1 、 l 2 l_1、l_2 l1l2

记两帧图像间的变换为 T 12 T_{12} T12,实际求解中,特征点 p 1 、 p 2 p_1、p_2 p1p2通过特征匹配得到, P 、 e 1 、 e 2 P、e_1、e_2 Pe1e2未知,待求变换 T 12 T_{12} T12

像素坐标

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

齐次坐标

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

对极约束

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

为简化公式,定义两个矩阵:基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E:
E = t ∧ R F = K − T E K − 1 E=t^\wedge R\qquad F=K^{-T}EK^{-1} E=tRF=KTEK1
则有:
x 2 T E x 1 = 0 p 2 T F p 1 = 0 x_2^TEx_1=0\qquad p_2^TFp_1=0 x2TEx1=0p2TFp1=0
由此,相机位姿估计问题可分为如下两步:

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

本质矩阵

本质矩阵 E = t ∧ R E=t^\wedge R E=tR是一个 3 × 3 3\times 3 3×3的矩阵,满足如下规则约束:

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

八点法求解

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

对于一对匹配的特征点,设其归一化坐标为 x 1 = [ u 1 v 1 1 ] x_1=\begin{bmatrix}u_1&v_1&1\end{bmatrix} x1=[u1v11] x 2 = [ u 2 v 2 1 ] x_2=\begin{bmatrix}u_2&v_2&1\end{bmatrix} x2=[u2v21],则可根据对极约束得到:
[ u 2 v 2 1 ] [ e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ] [ u 1 v 1 1 ] = 0 \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 [u2v21]e1e4e7e2e5e8e3e6e9u1v11=0
对本质矩阵E,将其展开写为向量形式:
e = [ e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ] T \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=[e1e2e3e4e5e6e7e8e9]T
则对极约束可写为关于 e e e的线性形式:
[ u 2 u 1 u 2 v 1 u 2 v 2 u 1 v 2 v 1 v 2 u 1 v 1 1 ] ⋅ e = 0 \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 [u2u1u2v1u2v2u1v2v1v2u1v11]e=0
同样,对所有8对匹配特征点做上述计算,则可得到如下线性方程组:
[ 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 ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ ⋮ 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 ] [ e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ] = 0 \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 u21u11u22u12u28u18u21v11u22v12u28v18u21u22u28v21u11v22u12v28u18v21v11v22v12v28v18v21v22v28u11u12u18v11v12v18111e1e2e3e4e5e6e7e8e9=0
其中, u i v i u^iv^i uivi表示第 i i i对匹配的特征点。当系数矩阵满秩(Rank=8)时,E的各个元素可求解。此时e构成一条线,同尺度等价性一致。

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

SVD分解

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

设本质矩阵E的SVD如下:
E = U Σ V T E=U\Sigma V^T E=UΣVT
根据SVD定义:矩阵 U 、 V U、V UV为正交阵, Σ \Sigma Σ为奇异值矩阵。由本质矩阵的内在性质知: Σ = d i a g ( σ , σ , 0 ) \Sigma=\mathrm{diag}(\sigma, \sigma, 0) Σ=diag(σ,σ,0),则在SVD分解中,任意本质矩阵E具有两个可能的 t 、 R t、R tR与之对应(此处直接给出了最终解,未推导):
t 1 ∧ = U R Z ( π 2 ) Σ U T R 1 = U R Z T ( π 2 ) V T t 2 ∧ = U R Z ( − π 2 ) Σ U T R 2 = U R Z T ( − π 2 ) V T 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\\ t1=URZ(2π)ΣUTR1=URZT(2π)VTt2=URZ(2π)ΣUTR2=URZT(2π)VT
其中, R Z ( π 2 ) R_Z(\frac{\pi}{2}) RZ(2π)表示绕Z轴旋转 π 2 \frac{\pi}{2} 2π得到旋转矩阵。同时由于E和 − - E等价,对任意一个 t t t取负,可得相同结果。则存在四组可能的解:

视觉里程计:2D到2D,对极几何法_第2张图片

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

内在性质

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

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

八点法存在的问题

尺度不确定性

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

初始化纯旋转

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

多于8对点时

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

单应矩阵

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

对于像平面 I 1 、 I 2 I_1、I_2 I1I2上匹配的两特征点 p 1 、 p 2 p_1、p_2 p1p2,若特征点落在某平面P上:
n T P + d = 0 − n T P d = 1 n^TP+d=0\\ -\frac{n^TP}{d}=1 nTP+d=0dnTP=1
其中 n n n为平面的法向量,d为截距。同样在相机平面中,存在投影关系:
p 2 ≃ K ( R P + t ) ≃ K ( R P + t ⋅ 1 ) ≃ K ( R P + t ⋅ ( − n T P d ) ) ≃ K ( R − t n T d ) P ≃ K ( R − t n T d ) K − 1 P 1 \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} p2K(RP+t)K(RP+t1)K(RP+t(dnTP))K(RdtnT)PK(RdtnT)K1P1
由此得到对应匹配特征点间的变换关系:
H = K ( R − t n T d ) K − 1 H=K\Bigl(R-\frac{tn^T}{d}\Bigr)K^{-1} H=K(RdtnT)K1
称矩阵H为单应矩阵,为一个 3 × 3 3\times 3 3×3的矩阵。

p 1 、 p 2 p_1、p_2 p1p2的归一化像素坐标带入:
p 2 ≃ H p 1 [ u 2 v 2 1 ] ≃ [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] [ u 1 v 1 1 ] 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} p2Hp1u2v21h1h4h7h2h5h8h3h6h9u1v11
符号 ≃ \simeq 表示了尺度意义下的相等,故而矩阵H乘以任意非零常数依旧等价于其本身。实际处理中,取 h 9 = 1 h_9=1 h9=1从而展开:
h 1 u 1 + h 2 v 1 + h 3 − h 7 u 1 u 2 − h 8 v 1 u 2 = u 2 h 4 u 1 + h 5 v 1 + h 6 − h 7 u 1 v 2 − h 8 v 1 v 2 = v 2 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 h1u1+h2v1+h3h7u1u2h8v1u2=u2h4u1+h5v1+h6h7u1v2h8v1v2=v2
也即,一堆匹配特征点可以提供两个约束条件,则自由度为8的单应矩阵H只需要4对点即可求解得到:
[ 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 ] [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 ] = [ u 2 1 v 2 1 u 2 2 v 2 2 u 2 3 v 2 3 u 2 4 v 2 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} u110u120u130u140v110v120v130v140101010100u110u120u130u140v110v120v130v1401010101u11u21u11v21u12u22u12v22u13u23u13v23u14u24u14v24v11u21v11v21v12u22v12v22v13u23v13v23v14u24v14v24h1h2h3h4h5h6h7h8=u21v21u22v22u23v23u24v24
计算得到单应矩阵H后,应同本质矩阵一样,进一步分解计算 R 、 t R、t Rt

小结

在2D-2D的情况下,仅知道图像坐标间对应关系:

  • 当特征点在平面上(俯视、仰视),使用H计算得到 R 、 t R、t Rt
  • 否则,使用E\F计算 R 、 t R、t Rt
  • 获得 R 、 t R、t Rt后,采用三角化计算其深度信息。

你可能感兴趣的:(《视觉SLAM十四讲》笔记,计算机视觉,算法,自动驾驶)