讲解关于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 与 m0)
情形一: 以(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
五、情况一(m0)中情形二，也就是式(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∧=[u 1,u 2,u 3]⎣ ⎡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(0,1)=-1;
W.at(1,0)=1;
W.at(2,2)=1;
//计算
R1 = u*W*vt;
//旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反
if(cv::determinant(R1)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(maxGood1)
{
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 课程课件
