(01)ORB-SLAM2源码无死角解析-(21) 基本矩阵Fundamental→本质矩阵Essential 分解恢复 Rt
讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
通过前面的博客，我们可以知道如下两个公式(如果为同一相机,则其相机内参 K 1 = K 2 \mathbf K_1=\mathbf K_2 K1=K2):
E = t ∧ R = t × R p 0 T E p 1 = 0 (01) \tag{01} \color{blue} \mathbf E=\mathbf t ^{\wedge} \mathbf R=\mathbf t\times \mathbf R~~~~~~~~~\color{blue} p_0^T \mathbf Ep_1=0 E=t∧R=t×R p0TEp1=0(01) F = ( K 0 − T E K 1 − 1 ) v o T F v 1 = 0 (02) \tag{02} \color{blue} \mathbf F=(\mathbf K_0^{-T} \mathbf E\mathbf K_1^{-1}) ~~~~~~~~~~ \color{blue} v_o^T\mathbf Fv_1=0 F=(K0−TEK1−1) voTFv1=0(02)其上的 p 0 , p 1 p_0,p_1 p0,p1 是图像坐标， v 0 , v 1 v_0,v_1 v0,v1 是像素坐标; E \mathbf E E 表示本质矩阵 Essential, F \mathbf F F 表示基本矩阵 Fundamental。一般在程序的运行过程中，相机内参是移植的，另外在 ORB-SLAM2 源码中 K 1 = K 2 \mathbf K_1=\mathbf K_2 K1=K2，因为是同一相机。也就是已知相机内参的情况下，一般会先计算出本质矩阵，然后通过本质矩阵求解 R t \mathbf R\mathbf t Rt。求解本质矩阵的公式如下(同一相机)：
E = K T F K (03) \tag{03} \color{blue} \mathbf E= \mathbf K^T \mathbf F \mathbf K E=KTFK(03)
二、反对称矩阵性质
再进行公式推导之前，我们需要了解一下反对称矩阵的一些性质(这里只列举部分→与后续公式推导相关的)。
性质 ( 1 ) : \color{red}{性质(1):} 性质(1): 当 A \mathbf A A为 n n n 实反对称矩阵时，那么存在 X T A X = 0 \mathbf{ X^TAX=0} XTAX=0, 其中 X \mathbf X X 为 n n n 阶矩阵( ∀ X ∈ R n \forall \mathbf X \in R^{n} ∀X∈Rn)。
性质 ( 2 ) : \color{red}{性质(2):} 性质(2): 实反对称矩阵的特征值为 0 或者虚数。
性质 ( 3 ) : \color{red}{性质(3):} 性质(3): 若 A \mathbf A A为 n n n 阶实反对称矩阵，则存在矩阵 n n n 阶正交矩阵 Γ \mathbf \Gamma Γ 使得：
Γ T A Γ = [ 0 m 1 − m 1 0 ⋱ 0 m r − m r 0 ⋱ 0 m n − m n 0 ] , \mathbf \Gamma^T \mathbf A \mathbf \Gamma=\left[\begin{array}{cccccccc} 0 & m_{1} & & & & & & \\ -m_{1} & 0 & & & & & & \\ & & \ddots & & & & & \\ & & & 0 & m_{r} & & & \\ & & & -m_{r} & 0 & & & \\ & & & & & \ddots & & \\ & & & & & & 0 & m_{n} \\ & & & & & & -m_{n} & 0 \end{array}\right], ΓTAΓ=⎣ ⎡0−m1m10⋱0−mrmr0⋱0−mnmn0⎦ ⎤, 上式中 ± b i \pm b_i ±bi 是 A \mathbf A A 的全部非零特征值， i ∈ { 1 , 2 , 3 ⋯ } i \in \{1,2,3 \cdots \} i∈{1,2,3⋯}。 A \mathbf A A 的秩 R ( A ) = 2 r R(\mathbf A)=2r R(A)=2r。其上矩阵 Γ T A Γ \mathbf \Gamma^T \mathbf A \mathbf \Gamma ΓTAΓ 称为实反对称矩阵 A \mathbf A A 在正交相似下的标准型。
性质 ( 4 ) : \color{red}{性质(4):} 性质(4):
三、基础推导
首先这里我们作如下假设(偏移矩阵与旋转矩阵):
t = ( t 1 , t 2 , t 3 ) R = ( r 1 , r 2 , r 3 ) (04) \tag{04} \color{blue} \mathbf t=(t_1,t_2,t_3)~~~~~~~~~~~~~\mathbf R=(\mathbf r_1,\mathbf r_2,\mathbf r_3) t=(t1,t2,t3) R=(r1,r2,r3)(04)那么我们根据公式(1)可得： E = t ∧ R = ( t ∧ r 1 , t ∧ r 2 , t ∧ r 3 ) (05) \tag{05} \color{blue} \mathbf E=\mathbf t^{\wedge} \mathbf R=\mathbf(\mathbf t^{\wedge} \mathbf r_1,\mathbf t^{\wedge}\mathbf r_2,\mathbf t^{\wedge}\mathbf r_3) E=t∧R=(t∧r1,t∧r2,t∧r3)(05)根据前面博客的推论，我们知道 , E ,\mathbf E ,E 的奇异矩阵必是 d i a g ( σ 1 , σ 2 , 0 ) = ( t 1 2 + t 2 2 + t 3 2 , t 1 2 + t 2 2 + t 3 2 , 0 ) diag(\sigma_{1}, \sigma_{2}, 0)=(t_{1}^{2}+t_{2}^{2}+t_{3}^{2}, t_{1}^{2}+t_{2}^{2}+t_{3}^{2}, 0) diag(σ1,σ2,0)=(t12+t22+t32,t12+t22+t32,0)。另外根据反对真矩阵的性质: 反对称矩阵矩阵一定正交于 d i a g ( m 1 D , m 2 D , m 3 D , 0 , 0 , ⋯ 0 ) diag(m_1D,m_2D,m_3D,0,0,\cdots0) diag(m1D,m2D,m3D,0,0,⋯0), D D D 的注解如下：
D = ( 0 1 − 1 0 ) , m i ≠ 0 , i = 1 , 2 , 3 ⋯ (06) \tag{06} \color{blue} \mathbf D=\left(\begin{array}{cc} 0 & 1 \\ -1 & 0 \end{array}\right), m_{i} \neq 0, i=1, 2, 3\cdots D=(0−110),mi=0,i=1,2,3⋯(06)根据反对称 性质(3) ，然后等式的两边都进行转置: t ∧ = Γ N Γ T \mathbf t^{\wedge}=\mathbf {\Gamma N\Gamma}^T t∧=ΓNΓT，这里的 Γ \mathbf \Gamma Γ 为正交矩阵， N \mathbf N N的格式如下： N = [ 0 − m 0 m 0 0 0 0 0 ] (07) \tag{07} \color{blue} \mathbf N=\left[\begin{array}{ccc} 0 & -m & 0 \\ m & 0 & 0 \\ 0 & 0 & 0 \end{array}\right] N=⎣ ⎡0m0−m00000⎦ ⎤(07)把 t ∧ = Γ N Γ T \mathbf t^{\wedge}=\mathbf \Gamma \mathbf N\mathbf \Gamma^T t∧=ΓNΓT 代入 E = t ∧ R \mathbf E =\mathbf t^{\wedge} \mathbf R E=t∧R 可得：
E = t ∧ R = Γ N Γ T R (08) \tag{08} \color{blue} \mathbf E =\mathbf t^{\wedge} \mathbf R=\mathbf {\Gamma N \Gamma}^T\mathbf R E=t∧R=ΓNΓTR(08) E T E = R T Γ N T N Γ T R (09) \tag{09} \color{blue} \mathbf {E}^T{\mathbf E}=\mathbf {R}^T \mathbf \Gamma \mathbf N^T\mathbf N\mathbf \Gamma^T\mathbf R ETE=RTΓNTNΓTR(09)又: N T N = d i a g ( m 2 , m 2 , 0 ) (10) \tag{10} \color{blue} \mathbf N^T\mathbf N=diag(m^2,m^2,0) NTN=diag(m2,m2,0)(10)这个时候可以很明显的看到 d i a g ( m 2 , m 2 , 0 ) diag(m^2,m^2,0) diag(m2,m2,0) 与奇异值矩阵是十分相似的，根据奇异值分解的原理 A = U Σ V T \mathbf A=\mathbf U \mathbf \Sigma \mathbf V^{T} A=UΣVT，如果 A T A = V Σ 2 V T \mathbf A^T\mathbf A=\mathbf V \mathbf \Sigma^2 \mathbf V^{T} ATA=VΣ2VT，特征向量组成 V \mathbf V V矩阵； 如果 A A T = U Σ 2 U T \mathbf A\mathbf A^T=\mathbf U\mathbf \Sigma^2 \mathbf U^{T} AAT=UΣ2UT，特征向量组成 U \mathbf U U 矩阵。那么我们现在对本质 E T E \mathbf E^T\mathbf E ETE 进行特征值分解： E T E = V Σ 2 V T = R T Γ N T N Γ T R = （ R T Γ ) ( N T N ) ( Γ T R ) (11) \tag{11} \color{blue} \mathbf E^T\mathbf E=\mathbf V \mathbf \Sigma^2 \mathbf V^{T}=\mathbf {R}^T \mathbf \Gamma \mathbf N^T\mathbf N\mathbf \Gamma^T\mathbf R=（\mathbf {R}^T \mathbf \Gamma) (\mathbf N^T\mathbf N) (\mathbf \Gamma^T\mathbf R) ETE=VΣ2VT=RTΓNTNΓTR=（RTΓ)(NTN)(ΓTR)(11)可以知道 E T E \mathbf E^T\mathbf E ETE 的特征值为 N T N = d i a g ( m 2 , m 2 , 0 ) \mathbf N^T\mathbf N=diag(m^2,m^2,0) NTN=diag(m2,m2,0), 其对应的特征向量为 V = R T Γ = ( V 1 , V 2 , V 3 ) \mathbf V=\mathbf R^T \mathbf \Gamma=(V_1,V_2,V_3) V=RTΓ=(V1,V2,V3)。这里的 V \mathbf V V 也是。这样我们得到 Σ = d i a g ( ∣ m ∣ , ∣ m ∣ , 0 ) (12) \tag{12} \color{blue} \mathbf \Sigma=diag(|m|,|m|,0) Σ=diag(∣m∣,∣m∣,0)(12) N T N = Σ 2 (13) \tag{13} \color{blue} \mathbf N^T\mathbf N= \mathbf \Sigma^2 NTN=Σ2(13)
对于(07)式子,其可以写成 N = W Σ \mathbf N=\mathbf W \mathbf \Sigma N=WΣ:(这里的 Σ \mathbf \Sigma Σ 为正交矩阵) N = W Σ = [ 0 − 1 0 1 0 0 0 0 1 ] [ m 0 0 0 m 0 0 0 0 ] (14) \tag{14} \color{blue} \mathbf N=\mathbf W \mathbf \Sigma=\left[\begin{array}{ccc} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & 0 \end{array}\right] N=WΣ=⎣ ⎡010−100001⎦ ⎤⎣ ⎡m000m0000⎦ ⎤(14) 注意 : \color{red}{注意:} 注意:其上的 Σ \mathbf \Sigma Σ 为正交矩阵，那么他的逆就等于他的转置。后续会利用到这个性质。另外，可以发现，上式还可以写成: N = − W T Σ = [ 0 1 0 − 1 0 0 0 0 − 1 ] [ m 0 0 0 m 0 0 0 0 ] (15) \tag{15} \color{blue} \mathbf N=-\mathbf W^T \mathbf \Sigma=\left[\begin{array}{ccc} 0 & 1 & 0\\ -1 & 0 & 0 \\ 0 & 0 & -1 \end{array}\right]\left[\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & 0 \end{array}\right] N=−WTΣ=⎣ ⎡0−1010000−1⎦ ⎤⎣ ⎡m000m0000⎦ ⎤(15)那么我们就要就要注意了，后续再计算 R t \mathbf R \mathbf t Rt的时候，其结果是否受 W \mathbf W W 影响，如果与其相关，则我们需要分情况进行讨论。另外，需要注意到， Σ = d i a g ( ∣ m ∣ , ∣ m ∣ , 0 ) \mathbf \Sigma=diag(|m|,|m|,0) Σ=diag(∣m∣,∣m∣,0) 因为奇异值是大于0的。但是 m 的正负值是不确定的，所以我们需要分两种情况来进行讨论，即 m>0 与 m<0 小时
四、情况一(m>0)
情形一: 以(14)式为基准( N = W Σ \mathbf N= \mathbf W \mathbf \Sigma N=WΣ)
根据前面的推导 N = W Σ \mathbf N= \mathbf W \mathbf \Sigma N=WΣ。结合我们前面的 V = R T Γ = ( V 1 , V 2 , V 3 ) \mathbf V=\mathbf R^T \mathbf \Gamma=(V_1,V_2,V_3) V=RTΓ=(V1,V2,V3), 以及 (08) 式可以得出：
E V = Γ N Γ T R V = Γ N Γ T R R T Γ = Γ N = Γ W Σ (16) \tag{16} \color{blue} \mathbf E \mathbf V=\mathbf {\Gamma N \Gamma}^T\mathbf R \mathbf V= \mathbf {\Gamma N \Gamma}^T\mathbf R \mathbf R^T \mathbf \Gamma=\mathbf \Gamma \mathbf N=\mathbf \Gamma \mathbf {W \Sigma} EV=ΓNΓTRV=ΓNΓTRRTΓ=ΓN=ΓWΣ(16)另外结合 E = U Σ V T \mathbf E=\mathbf U \mathbf \Sigma \mathbf V^{T} E=UΣVT,两边同时乘 V \mathbf V V, 可以得到 E V = U Σ \mathbf E \mathbf V= \mathbf U \Sigma EV=UΣ。那么结合(16)式可以得到：
E V = U Σ = Γ N = Γ W Σ (17) \tag{17} \color{blue} \mathbf E \mathbf V= \mathbf U \Sigma=\mathbf \Gamma \mathbf N=\mathbf \Gamma \mathbf {W \Sigma} EV=UΣ=ΓN=ΓWΣ(17) U = Γ W ⟹ Γ = U W T \color{blue} \mathbf U=\mathbf \Gamma \mathbf W~~\Longrightarrow ~~\mathbf \Gamma = \mathbf U\mathbf W^T U=ΓW ⟹ Γ=UWT 前面通过(11)式得到 V = R T Γ \mathbf V=\mathbf R^T \mathbf \Gamma V=RTΓ，结合上式可以得到如下结论( W − 1 = W T \mathbf W^{-1}=\mathbf W^T W−1=WT，正交矩阵性质)： R = Γ V T = U W T V T (18) \tag{18} \color{blue} \mathbf R= \mathbf \Gamma \mathbf V^T=\mathbf U\mathbf W^T\mathbf V^T R=ΓVT=UWTVT(18)
我们前面已经假设 t ∧ = Γ N Γ T \mathbf t^{\wedge}=\mathbf {\Gamma N\Gamma}^T t∧=ΓNΓT，以及(17)式中 U = Γ W \mathbf U=\mathbf \Gamma \mathbf W U=ΓW, 那么进一步推导可以的获得: t ∧ = Γ N Γ T = U W T N W U T = U W T W Σ W U T = U Σ W U T (19) \tag{19} \color{blue} \mathbf t^{\wedge}=\mathbf {\Gamma N\Gamma}^T=\mathbf U \mathbf W^T \mathbf N \mathbf WU^T=\mathbf U \mathbf W^T \mathbf W \mathbf \Sigma \mathbf WU^T=\mathbf U \mathbf \Sigma \mathbf WU^T t∧=ΓNΓT=UWTNWUT=UWTWΣWUT=UΣWUT(19)
情形二: 以(15)式为基准( N = − W T Σ \mathbf N= -\mathbf W^T \mathbf \Sigma N=−WTΣ)
根据上面的推导，我们知道情形一与情形二他们主要在于 N \mathbf N N 的区别，进一步其实是在于 W \mathbf W W 与 − W T -\mathbf W^T −WT 的区别，所以只需要把情形一中的 W \mathbf W W 替换成 − W T -\mathbf W^T −WT 即可，故:
R = U W T V T = U ( − W T ) T V T = − U W V T (20) \tag{20} \color{blue} \mathbf R= \mathbf U\mathbf W^T\mathbf V^T= \mathbf U (-\mathbf W^T)^T\mathbf V^T= -\mathbf U \mathbf W \mathbf V^T R=UWTVT=U(−WT)TVT=−UWVT(20) t ∧ = U Σ W U T = U Σ ( − W T ) U T = − U Σ W T U T \color{blue} \mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W\mathbf U^T=\mathbf U \mathbf \Sigma (-\mathbf W^T)\mathbf U^T=-\mathbf U \mathbf \Sigma \mathbf W^T\mathbf U^T t∧=UΣWUT=UΣ(−WT)UT=−UΣWTUT
五、情况一(m<0)
情形一: 以(14)式为基准( N = W Σ \mathbf N= \mathbf W \mathbf \Sigma N=WΣ)
首先我们来思考一下，根据情况一的情形一中的(18)与(19)式子，我们可以的推导如下： N = W Σ = [ 0 − 1 0 1 0 0 0 0 1 ] [ m 0 0 0 m 0 0 0 0 ] (21) \tag{21} \color{blue} \mathbf N=\mathbf W \mathbf \Sigma=\left[\begin{array}{ccc} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ccc} m & 0 & 0 \\ 0 & m & 0 \\ 0 & 0 & 0 \end{array}\right] N=WΣ=⎣ ⎡010−100001⎦ ⎤⎣ ⎡m000m0000⎦ ⎤(21) R = U W T V T \color{blue} \mathbf R=\mathbf U\mathbf W^T\mathbf V^T R=UWTVT t ∧ = U Σ W U T \color{blue} \mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W\mathbf U^T t∧=UΣWUT
因为这里的 m 已经小于0了，所以我们需要在其前面加负号，也就是
Σ = [ − m 0 0 0 − m 0 0 0 0 ] (21) \tag{21} \color{blue} \mathbf \Sigma = \left[\begin{array}{ccc} -m & 0 & 0 \\ 0 & -m & 0 \\ 0 & 0 & 0 \end{array}\right] Σ=⎣ ⎡−m000−m0000⎦ ⎤(21)那么我们复原一下 N \mathbf N N, 看其是否发生改变。如下： N = W Σ = [ 0 − 1 0 1 0 0 0 0 1 ] [ − m 0 0 0 − m 0 0 0 0 ] = [ 0 m 0 − m 0 0 0 0 0 ] (23) \tag{23} \color{blue} \mathbf N=\mathbf W \mathbf \Sigma=\left[\begin{array}{ccc} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ccc} -m & 0 & 0 \\ 0 & -m & 0 \\ 0 & 0 & 0 \end{array}\right]=\left[\begin{array}{ccc} 0 & m & 0 \\ -m & 0 & 0 \\ 0 & 0 & 0 \end{array}\right] N=WΣ=⎣ ⎡010−100001⎦ ⎤⎣ ⎡−m000−m0000⎦ ⎤=⎣ ⎡0−m0m00000⎦ ⎤(23)可以很明显的看到，此时的 N \mathbf N N 相对于 (07)式子 的 N \mathbf N N 已经发生了变化，需要乘 -1 之后才相等。如果我们使用上式的 Σ \mathbf \Sigma Σ 根据(08) 式复原,其得到 − E -\mathbf E −E ，而不是 E \mathbf E E。所以，当 m < 0 的时候(与 Σ \mathbf \Sigma Σ 相关才添加负号)，
R = U W T V T (24) \tag{24} \color{blue} \mathbf R=\mathbf U\mathbf W^T\mathbf V^T R=UWTVT(24) t ∧ = − U Σ W U T \color{blue} \mathbf t^{\wedge}=-\mathbf U \mathbf \Sigma \mathbf WU^T t∧=−UΣWUT
情形二: 以(14)式为基准( N = W Σ \mathbf N= \mathbf W \mathbf \Sigma N=WΣ)
针对于该情形，只需要把 情况一(m>0)中情形二，也就是式(20)结果中的 t ∧ \mathbf t^{\wedge} t∧ 变号即可。如下：
R = − U W V T (25) \tag{25} \color{blue} \mathbf R= -\mathbf U \mathbf W \mathbf V^T R=−UWVT(25) t ∧ = U Σ W T U T \color{blue} \mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W^T\mathbf U^T t∧=UΣWTUT
六、求解偏移矩阵t
根据上述的推导，我们共获得了4组解，从解可以看出，只需要对 E \mathbf E E 做奇异值分解，即可获得 U , Σ , V \mathbf U, \mathbf \Sigma, \mathbf V U,Σ,V 从而轻松求解出 R \mathbf R R 以及 , t ∧ \mathbf t^{\wedge} t∧。由于本质矩阵 E \mathbf E E 具备尺度等价性质，那么我们先对 E \mathbf E E 进行特征值分解，分解之后获得 U , Σ , V \mathbf U, \mathbf \Sigma, \mathbf V U,Σ,V， 然后令其中的 Σ = d i a g ( 1 , 1 , 0 ) \mathbf \Sigma=diag(1,1,0) Σ=diag(1,1,0) 再重新与 U , V \mathbf U,\mathbf V U,V 组合，根据前面的公式求解 R \mathbf R R 以及 , t ∧ \mathbf t^{\wedge} t∧。
那么现在我们就需要根据 t ∧ \mathbf t^{\wedge} t∧ 求解出 t \mathbf t t，进一步分析，我们以(19)式中的 t ∧ = U Σ W U T \mathbf t^{\wedge}=\mathbf U \mathbf \Sigma \mathbf W\mathbf U^T t∧=UΣWUT 为例进行分析，其中的 U = ( u 1 , u 2 , u 3 ) \mathbf U=(u_1,u_2,u_3) U=(u1,u2,u3)。那么: t ∧ = [ u ⃗ 1 , u ⃗ 2 , u ⃗ 3 ] [ 1 1 0 ] [ 0 − 1 0 1 0 0 0 0 1 ] [ u 1 T u 2 T u 3 T ] = u 2 u 1 T − u 1 u 2 T (26) \tag{26} \color{blue} \mathbf {t}^{\wedge}=\left[\vec{u}_{1}, \vec{u}_{2}, \vec{u}_{3}\right]\left[\begin{array}{ccc} 1 & \\ & 1 & \\ & & 0 \end{array}\right]\left[\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}\right]\left[\begin{array}{l} {u}_{1}^T \\ {u}_{2}^T \\ {u}_{3}^T \end{array}\right]=u_2u_1^T-u_1u_2^T t∧=[u1,u2,u3]⎣ ⎡110⎦ ⎤⎣ ⎡010−100001⎦ ⎤⎣ ⎡u1Tu2Tu3T⎦ ⎤=u2u1T−u1u2T(26)令 u 1 , u 2 , u 3 u_1,u_2,u_3 u1,u2,u3 构成右手坐标系，即 d e t ( U ) = 1 det(U)=1 det(U)=1，又如果 x,y向量正交，则x y内积为0，即x的转置乘y为0，根据该性质我们可以得出: t ∧ u 1 = u 2 t ∧ u 2 = − u 1 t ∧ u 3 = 0 (27) \tag{27} \color{blue} \mathbf t^{\wedge}u_1=u_2~~~~~~~\mathbf t^{\wedge}u_2=-u_1~~~~~~~\mathbf t^{\wedge}u_3=0 t∧u1=u2 t∧u2=−u1 t∧u3=0(27)
然后再根据性质 a,b向量叉乘，获得垂直与a,b的新向量，进行如下推导：
{ ( t − u 3 ) ∧ u 1 = t ∧ u 1 + u 3 ∧ u 1 = u 2 − u 3 × u 1 = u 2 − u 2 = 0 ( t − u 3 ) ∧ u 2 = t ∧ u 2 + u 3 ∧ u 2 = − u 1 + u 3 × u 2 = − u 1 + u 1 = 0 ( t − u 3 ) ∧ u 3 = t ∧ u 3 + u 3 ∧ u 3 = 0 − u 3 × u 3 = 0 − 0 = 0 (28) \tag{28} \color{blue} \left\{\begin{array}{l} (\mathbf t-u_3)^{\wedge}u_1=\mathbf t^{\wedge}u_1+u_3^{\wedge}u_1=u_2-u_3\times u_1=u_2-u_2=\mathbf0\\ (\mathbf t-u_3)^{\wedge}u_2=\mathbf t^{\wedge}u_2+u_3^{\wedge}u_2=-u_1+u_3\times u_2=-u_1+u_1=\mathbf0\\ (\mathbf t-u_3)^{\wedge}u_3=\mathbf t^{\wedge}u_3+u_3^{\wedge}u_3=\mathbf0-u_3\times u_3=\mathbf0-\mathbf0=\mathbf0\\ \end{array}\right. ⎩ ⎨ ⎧(t−u3)∧u1=t∧u1+u3∧u1=u2−u3×u1=u2−u2=0(t−u3)∧u2=t∧u2+u3∧u2=−u1+u3×u2=−u1+u1=0(t−u3)∧u3=t∧u3+u3∧u3=0−u3×u3=0−0=0(28)
又因为 u 1 , u 2 , u 3 u_1,u_2,u_3 u1,u2,u3 为非零列列向量，故只有 t − u 3 = 0 \mathbf t-u_3=0 t−u3=0 时，才能满足上述要求，所以 t = u 3 \mathbf t=u_3 t=u3。由此可见， t \mathbf t t 为 E E E 左奇异值矩阵的最后一列。
七、源码注释
其代码的主调函数为 Initializer.cc文件中的 Initializer::ReconstructF() 函数，该函数调用 DecomposeE(E21,R1,R2,t) 函数，该为核心部分，展示如下：
/**
* @brief 分解Essential矩阵得到R,t
* 分解E矩阵将得到4组解，这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t]
* 参考：Multiple View Geometry in Computer Vision - Result 9.19 p259
* @param[in] E 本质矩阵
* @param[in & out] R1 旋转矩阵1
* @param[in & out] R2 旋转矩阵2
* @param[in & out] t 平移向量，另外一个取相反数
*/
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{
// 对本质矩阵进行奇异值分解
//准备存储对本质矩阵进行奇异值分解的结果
cv::Mat u,w,vt;
//对本质矩阵进行奇异值分解
cv::SVD::compute(E,w,u,vt);
// 左奇异值矩阵U的最后一列就是t，对其进行归一化
u.col(2).copyTo(t);
t=t/cv::norm(t);
// 构造一个绕Z轴旋转pi/2的旋转矩阵W，按照下式组合得到旋转矩阵 R1 = u*W*vt
//计算完成后要检查一下旋转矩阵行列式的数值，使其满足行列式为1的约束
cv::Mat W(3,3,CV_32F,cv::Scalar(0));
W.at<float>(0,1)=-1;
W.at<float>(1,0)=1;
W.at<float>(2,2)=1;
//计算
R1 = u*W*vt;
//旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反
if(cv::determinant(R1)<0)
R1=-R1;
float a = R1.at<float>(0,0);
// 同理将矩阵W取转置来按照相同的公式计算旋转矩阵R2 = u*W.t()*vt
R2 = u*W.t()*vt;
//旋转矩阵有行列式为1的约束
if(cv::determinant(R2)<0)
R2=-R2;
}
} //namespace ORB_SLAM
主调函数 Initializer::ReconstructF() 注释如下：
/**
* @brief 从基础矩阵F中求解位姿R，t及三维点
* F分解出E，E有四组解，选择计算的有效三维点（在摄像头前方、投影误差小于阈值、视差角大于阈值）最多的作为最优的解
* @param[in] vbMatchesInliers 匹配好的特征点对的Inliers标记
* @param[in] F21 从参考帧到当前帧的基础矩阵
* @param[in] K 相机的内参数矩阵
* @param[in & out] R21 计算好的相机从参考帧到当前帧的旋转
* @param[in & out] t21 计算好的相机从参考帧到当前帧的平移
* @param[in & out] vP3D 三角化测量之后的特征点的空间坐标
* @param[in & out] vbTriangulated 特征点三角化成功的标志
* @param[in] minParallax 认为三角化有效的最小视差角
* @param[in] minTriangulated 最小三角化点数量
* @return true 成功初始化
* @return false 初始化失败
*/
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
// Step 1 统计有效匹配点个数，并用 N 表示
// vbMatchesInliers 中存储匹配点对是否是有效
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i]) N++;
// Step 2 根据基础矩阵和相机的内参数矩阵计算本质矩阵
cv::Mat E21 = K.t()*F21*K;
// 定义本质矩阵分解结果，形成四组解,分别是：
// (R1, t) (R1, -t) (R2, t) (R2, -t)0
cv::Mat R1, R2, t;
// Step 3 从本质矩阵求解两个R解和两个t解，共四组解
// 不过由于两个t解互为相反数，因此这里先只获取一个
// 虽然这个函数对t有归一化，但并没有决定单目整个SLAM过程的尺度.
// 因为 CreateInitialMapMonocular 函数对3D点深度会缩放，然后反过来对 t 有改变.
//注意下文中的符号“'”表示矩阵的转置
// |0 -1 0|
// E = U Sigma V' let W = |1 0 0|
// |0 0 1|
// 得到4个解 E = [R|t]
// R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3
DecomposeE(E21,R1,R2,t);
cv::Mat t1=t;
cv::Mat t2=-t;
// Reconstruct with the 4 hyphoteses and check
// Step 4 分别验证求解的4种R和t的组合，选出最佳组合
// 原理：若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合
// 实现：根据计算的解组合成为四种情况,并依次调用 Initializer::CheckRT() 进行检查,得到可以进行三角化测量的点的数目
// 定义四组解分别在对同一匹配点集进行三角化测量之后的特征点空间坐标
vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
// 定义四组解分别对同一匹配点集的有效三角化结果，True or False
vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
// 定义四种解对应的比较大的特征点对视差角
float parallax1,parallax2, parallax3, parallax4;
// Step 4.1 使用同样的匹配点分别检查四组解，记录当前计算的3D点在摄像头前方且投影误差小于阈值的个数，记为有效3D点个数
int nGood1 = CheckRT(R1,t1, //当前组解
mvKeys1,mvKeys2, //参考帧和当前帧中的特征点
mvMatches12, vbMatchesInliers, //特征点的匹配关系和Inliers标记
K, //相机的内参数矩阵
vP3D1, //存储三角化以后特征点的空间坐标
4.0*mSigma2, //三角化测量过程中允许的最大重投影误差
vbTriangulated1, //参考帧中被成功进行三角化测量的特征点的标记
parallax1); //认为某对特征点三角化测量有效的比较大的视差角
int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
// Step 4.2 选取最大可三角化测量的点的数目
int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
// 重置变量，并在后面赋值为最佳R和T
R21 = cv::Mat();
t21 = cv::Mat();
// Step 4.3 确定最小的可以三角化的点数
// 在0.9倍的内点数 和 指定值minTriangulated =50 中取最大的，也就是说至少50个
int nMinGood = max(static_cast<int>(0.9*N), minTriangulated);
// 统计四组解中重建的有效3D点个数 > 0.7 * maxGood 的解的数目
// 如果有多个解同时满足该条件，认为结果太接近，nsimilar++，nsimilar>1就认为有问题了，后面返回false
int nsimilar = 0;
if(nGood1>0.7*maxGood)
nsimilar++;
if(nGood2>0.7*maxGood)
nsimilar++;
if(nGood3>0.7*maxGood)
nsimilar++;
if(nGood4>0.7*maxGood)
nsimilar++;
// Step 4.4 四个结果中如果没有明显的最优结果，或者没有足够数量的三角化点，则返回失败
// 条件1: 如果四组解能够重建的最多3D点个数小于所要求的最少3D点个数（mMinGood），失败
// 条件2: 如果存在两组及以上的解能三角化出 >0.7*maxGood的点，说明没有明显最优结果，失败
if(maxGood<nMinGood || nsimilar>1)
{
return false;
}
// Step 4.5 选择最佳解记录结果
// 条件1: 有效重建最多的3D点，即maxGood == nGoodx，也即是位于相机前方的3D点个数最多
// 条件2: 三角化视差角 parallax 必须大于最小视差角 minParallax，角度越大3D点越稳定
//看看最好的good点是在哪种解的条件下发生的
if(maxGood==nGood1)
{
//如果该种解下的parallax大于函数参数中给定的最小值
if(parallax1>minParallax)
{
// 存储3D坐标
vP3D = vP3D1;
// 获取特征点向量的三角化测量标记
vbTriangulated = vbTriangulated1;
// 存储相机姿态
R1.copyTo(R21);
t1.copyTo(t21);
// 结束
return true;
}
}else if(maxGood==nGood2)
{
if(parallax2>minParallax)
{
vP3D = vP3D2;
vbTriangulated = vbTriangulated2;
R2.copyTo(R21);
t1.copyTo(t21);
return true;
}
}else if(maxGood==nGood3)
{
if(parallax3>minParallax)
{
vP3D = vP3D3;
vbTriangulated = vbTriangulated3;
R1.copyTo(R21);
t2.copyTo(t21);
return true;
}
}else if(maxGood==nGood4)
{
if(parallax4>minParallax)
{
vP3D = vP3D4;
vbTriangulated = vbTriangulated4;
R2.copyTo(R21);
t2.copyTo(t21);
return true;
}
}
// 如果有最优解但是不满足对应的parallax>minParallax，那么返回false表示求解失败
return false;
}
八、结语
从上述的分析中，我们知道从 Essential 中恢复 Rt 存在四种解，那么那一组解才是最合适的呢?源码中是对每一个解进行分析，检测点是不是都在两个相机的前方，并且统计重投影误差较小的点的个数，找出4个解中统计得到点数最多的的那个解作为最终的解。其代码具体的实现，我们接下会进行讲解。
本文内容来自计算机视觉life ORB-SLAM2 课程课件