(01)ORB-SLAM2源码无死角解析-(20) 分解Homography,恢复Rt→Faugeras SVD-based decomposition

讲解关于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→官方认证  

一、前言

通过前面的文章，知道了如何求解Homography 以及 Fundamental 矩阵。但是这是几个中间过程而已，求解 Homography 以及 Fundamental 是为了进一步求解旋转矩阵 R R R 以及偏移矩阵 t t t, 通过 Homography 求解 R t Rt Rt 主要有以下两个方式： F a u g e r a s S V D − b a s e d d e c o m p o s i t i o n \color{blue}Faugeras SVD-based decomposition FaugerasSVD−baseddecomposition：Motion and Structure from Motion in a Piecewise Planar Environment Z h a n g S V D − b a s e d d e c o m p o s i t i o n \color{blue} Zhang SVD-based decomposition ZhangSVD−baseddecomposition：D Reconstruction Based on Homography Mapping

该篇博客主要讲解上面之中的 Faugeras SVD-based decomposition。在进行详细讲解之前，大家需要了解以下相机成像的原理，这里简单给个图示，不再进行详细的讲解

图一：相机成像原理→单孔模型  

二、适用场景

根据两帧图像恢复相机 R t Rt Rt 的方式有很多种，比如前面说到本质矩阵，基本矩阵，以及该博客讲解的单应矩阵，这么多的方式中，选用那种才是最合适的呢? 我们先来说一下 Homography：

1、两帧图像中的特征点共面(下左图所示)
2、相机发生纯旋转，即偏移矩阵t=0(下右图所示)

以上两种情况就适合从单应矩阵 Homography 进行求解。具体缘由在接下来的第二篇博客中有具体的介绍。

 

三、公式推导

首先设三维空间的一个点 X \mathbf{X} X, 以及该点再图像平面的坐标为 x \mathbf x x(假设焦距 f 为1),如下： X = ( X Y Z ) T                           x = ( x y 1 ) T (1) \tag{1} \color{blue} \mathbf{X}=\left(\begin{array}{lll} X & Y & Z \end{array}\right)^{T}~~~~~~~~~~~~~~~~~~~~~~~~~\mathbf{x}=\left(\begin{array}{lll} x & y & 1 \end{array}\right)^{T} X=(X​Y​Z​)T                         x=(x​y​1​)T(1) 那么根据相机的成像原理(可以参考图一),其存在如下关系： X x = Y y = Z (2) \tag{2} \color{blue} \frac{X}{x}=\frac{Y}{y}=Z xX​=yY​=Z(2) 令第一个相机坐标为世界坐标 \color{red}令第一个相机坐标为世界坐标 令第一个相机坐标为世界坐标，设相机投影模型(E表示单位矩阵,R|T表示把旋转矩阵与偏移矩阵按列拼接起来) P 1 = K 1 [ E ∣ 0 ]              P 2 = K 2 [ R ∣ t ] (3) \tag{3} \color{blue} P_1=K_1[E|0]~~~~~~~~~~~~P_2=K_2[R|t] P1​=K1​[E∣0]            P2​=K2​[R∣t](3)设3D点所在的世界平面方程方程，与法向量 n \mathbf n n 如下(单位向量)，其中的 d d d 表示坐标系原点到平面的距离， 平面方程：      a X + b Y + c Z = d \color{blue} 平面方程：~~~~~a X+b Y+c Z=d 平面方程：     aX+bY+cZ=d 法向量： n = ( a b c ) T \color{blue} 法向量：\mathbf n = \left(\begin{array}{lll} a & b & c \end{array}\right)^{T} 法向量：n=(a​b​c​)T 根据以上两个式子 :        n T X = d \color{blue} 根据以上两个式子:~~~~~~\mathbf {n}^T \mathbf{X}=d 根据以上两个式子:      nTX=d那么带入平面上的点 X \mathbf{X} X ，其平面方程可以是表示为: 1 d n T X = 1 (4) \tag{4} \color{blue} \frac{1}{d} \mathbf{n}^{T} \mathbf{X}=1 d1​nTX=1(4)那么我们设平面上的点 X 1 \mathbf{X_1} X1​满足上式(4)(以第一个相机坐标为世界坐标)，也就是： 1 = 1 d n T X 1 (5) \tag{5} \color{blue} 1 = \frac{1}{d} \mathbf{n}^{T} \mathbf{X_1} 1=d1​nTX1​(5)对于两个相机坐标系点的关系 如下: X 2 = R X 1 + t (6) \tag{6} \color{blue} \mathbf{X}_{2}=R \mathbf{X}_{1}+\mathbf{t} X2​=RX1​+t(6)那么我们把(5)式乘到(6)式的 t \mathbf t t 后面，可以得到如下： X 2 = ( R + 1 d t n T ) X 1 (7) \tag{7} \color{blue} \mathbf{X}_{2}=\left(R+\frac{1}{d} \mathbf{t n}^{T}\right) \mathbf{X}_{1} X2​=(R+d1​tnT)X1​(7)根据单应性变换，对图像齐次坐标 x = ( x y 1 ) T \mathbf{x}=\left(\begin{array}{lll}x & y & 1\end{array}\right)^{T} x=(x​y​1​)T和齐次单应矩阵 H H H 那么两个图像的坐标满足 s x 2 = H x 1 s\mathbf x_2=H\mathbf x_1 sx2​=Hx1​ ,这里的 s s s 可以理解为缩放，根据相机投影模型： x 1 = K 1 X 1             x 2 = K 2 X 2 (8) \tag{8} \color{blue} \mathbf{x}_{1}=K_{1} \mathbf{X}_{1}~~~~~~~~~~~\mathbf{x}_{2}=K_{2} \mathbf{X}_{2} x1​=K1​X1​           x2​=K2​X2​(8)我们可以推导出 X 2 = K 2 − 1 H K 1 X 1 (9) \tag{9} \color{blue} \mathbf{X}_{2}=K_{2}^{-1} H K_{1} \mathbf{X}_{1} X2​=K2−1​HK1​X1​(9)

根据 (7), (9) 我们可以得到(如果是同一个相机，这里的 K 1 = K 2 \mathbf K_1=\mathbf K_2 K1​=K2​) A = d R + t n T = d K 2 − 1 H K 1 (10) \tag{10} \color{blue} A=dR+\mathbf{t n}^{T}=dK_{2}^{-1} H K_{1} A=dR+tnT=dK2−1​HK1​(10)上面的公式我们可以求得 H H H 矩阵与 R t Rt Rt 的对应关系(略过)，对 A A A 进行奇异值分解，如下 A = U Λ V T           Λ = diag ⁡ ( d 1 , d 2 , d 3 ) (11) \tag{11} \color{blue} A=U \Lambda V^{T}~~~~~~~~~\Lambda=\operatorname{diag}\left(d_{1}, d_{2}, d_{3}\right) A=UΛVT         Λ=diag(d1​,d2​,d3​)(11)另外由于 U , V U,V U,V 满足 ( 其中 U T U = V T V = E ) \left(其中 U^{T} U=V^{T} V=E\right) (其中UTU=VTV=E)，可以得到： Λ = U T A V = d U T R V + ( U T t ) ( V T n ) T (12) \tag{12} \color{blue} \Lambda=U^{T} A V=d U^{T} R V+\left(U^{T} \mathbf{t}\right)\left(V^{T} \mathbf{n}\right)^{T} Λ=UTAV=dUTRV+(UTt)(VTn)T(12) U , V U,V U,V为正交矩阵，也就是说其行列式为 ± 1 \pm 1 ±1，即 s = det ⁡ ( U ) = det ⁡ ( V ) s=\operatorname{det}(U)= \operatorname{det}(V) s=det(U)=det(V), 且 s 2 = 1 s^2=1 s2=1， 令: { R ′ = s U T R V t ′ = U T t n ′ = V T n d ′ = s d s = det ⁡ ( U ) det ⁡ ( V ) ⟹             Λ = [ d 1 0 0 0 d 2 0 0 0 d 3 ] = d ′ R ′ + t ′ n ′ T (13) \tag{13} \color{blue} \left\{\begin{array}{l} R^{\prime}=s U^{T} R V \\ \mathbf{t}^{\prime}=U^{T} \mathbf{t} \\ \mathbf{n}^{\prime}=V^{T} \mathbf{n} \\ d^{\prime}=s d \\ s=\operatorname{det}(U) \operatorname{det}(V) \end{array}\right. \Longrightarrow~~~~~~~~~~~ \Lambda=\left[\begin{array}{ccc} d_{1} & 0 & 0 \\ 0 & d_{2} & 0 \\ 0 & 0 & d_{3} \end{array}\right]=d^{\prime} R^{\prime}+\mathbf t^{\prime} \mathbf{n}^{\prime T} ⎩ ⎨ ⎧​R′=sUTRVt′=UTtn′=VTnd′=sds=det(U)det(V)​⟹           Λ=⎣ ⎡​d1​00​0d2​0​00d3​​⎦ ⎤​=d′R′+t′n′T(13) 其上 V T V_T VT​ 是行列式为 ± 1 \pm 1 ±1 的正交矩阵，那么其也是一个旋转矩阵(可以参考该片博客末尾史上最简SLAM零基础解读(1) - 旋转平移矩阵→欧式变换推导),该矩阵作用于单位法向量 n n n,也就是对齐进行旋转，也就是说 n ′ n' n′ 也是单位向量。取基底 e 1 = ( 1 , 0 ， 0 ) T , e 1 = ( 1 , 0 ， 0 ) T , e 1 = ( 1 , 0 ， 0 ) T \mathbf e_1=(1,0，0)^T,\mathbf e_1=(1,0，0)^T,\mathbf e_1=(1,0，0)^T e1​=(1,0，0)T,e1​=(1,0，0)T,e1​=(1,0，0)T ，设单位向量 n ′ = ( x 1 , x 2 , x 3 ) T \mathbf n'=(x_1, x_2, x_3)^T n′=(x1​,x2​,x3​)T, 则 n ′ e = ( x 1 e 1 + x 2 e 2 + x 3 e 3 ) \mathbf n'\mathbf e=(x_{1} \mathbf{e}_{1}+x_{2} \mathbf{e}_{2}+x_{3} \mathbf{e}_{3}) n′e=(x1​e1​+x2​e2​+x3​e3​) 根据式子 (13) 可以得到： [ d 1 e 1 d 2 e 2 d 3 e 3 ] = [ d ′ R ′ e 1 d ′ R ′ e 2 d ′ R ′ e 3 ] + [ t ′ x 1 e 1 t ′ x 2 e 2 t ′ x 3 e 3 ] (14) \tag{14} \color{blue} \left[\begin{array}{lll} d_{1} \mathbf{e}_{1} & d_{2} \mathbf{e}_{2} & d_{3} \mathbf{e}_{3} \end{array}\right]=\left[\begin{array}{llll} d^{\prime} R^{\prime} \mathbf{e}_{1} & d^{\prime} R^{\prime} \mathbf{e}_{2} & d^{\prime} R^{\prime} \mathbf{e}_{3} \end{array}\right]+\left[\begin{array}{lll} \mathbf{t}^{\prime} x_{1} \mathbf e_1 & \mathbf{t}^{\prime} x_{2} \mathbf e_2 & \mathbf{t}^{\prime} x_{3} \mathbf e_3 \end{array}\right] [d1​e1​​d2​e2​​d3​e3​​]=[d′R′e1​​d′R′e2​​d′R′e3​​]+[t′x1​e1​​t′x2​e2​​t′x3​e3​​](14) [ d 1 e 1 d 2 e 2 d 3 e 3 ] = [ d ′ R ′ e 1 d ′ R ′ e 2 d ′ R ′ e 3 ] + [ t ′ x 1 t ′ x 2 t ′ x 3 ] (15) \tag{15} \color{blue} \left[\begin{array}{lll} d_{1} \mathbf{e}_{1} & d_{2} \mathbf{e}_{2} & d_{3} \mathbf{e}_{3} \end{array}\right]=\left[\begin{array}{llll} d^{\prime} R^{\prime} \mathbf{e}_{1} & d^{\prime} R^{\prime} \mathbf{e}_{2} & d^{\prime} R^{\prime} \mathbf{e}_{3} \end{array}\right]+\left[\begin{array}{lll} \mathbf{t}^{\prime} x_{1} & \mathbf{t}^{\prime} x_{2} & \mathbf{t}^{\prime} x_{3} \end{array}\right] [d1​e1​​d2​e2​​d3​e3​​]=[d′R′e1​​d′R′e2​​d′R′e3​​]+[t′x1​​t′x2​​t′x3​​](15)即可推导出如下3式： d 1 e 1 = d ′ R ′ e 1 + t ′ x 1 d 2 e 2 = d ′ R ′ e 2 + t ′ x 2 d 3 e 3 = d ′ R ′ e 3 + t ′ x 3 (16) \tag{16} \color{blue} \begin{aligned} d_{1} \mathbf{e}_{1} &=d^{\prime} R^{\prime} \mathbf{e}_{1}+\mathbf{t}^{\prime} x_{1} \\ d_{2} \mathbf{e}_{2} &=d^{\prime} R^{\prime} \mathbf{e}_{2}+\mathbf{t}^{\prime} x_{2} \\ d_{3} \mathbf{e}_{3} &=d^{\prime} R^{\prime} \mathbf{e}_{3}+\mathbf{t}^{\prime} x_{3} \end{aligned} d1​e1​d2​e2​d3​e3​​=d′R′e1​+t′x1​=d′R′e2​+t′x2​=d′R′e3​+t′x3​​(16) 注意到 n n n 是单位法向量， V V V 是旋转矩阵，则 n ′ n′ n′ 也是单位法向量，则有 Σ i 3 = x i 2 = 1 Σ^3_i=x^2_i=1 Σi3​=xi2​=1。对(16)的三个式子消去t′得

d ′ R ′ ( x 2 e 1 − x 1 e 2 ) = d 1 x 2 e 1 − d 2 x 1 e 2 d ′ R ′ ( x 3 e 2 − x 2 e 3 ) = d 2 x 3 e 2 − d 3 x 2 e 3 d ′ R ′ ( x 1 e 3 − x 3 e 1 ) = d 3 x 1 e 3 − d 1 x 3 e 1 (17) \tag{17} \color{blue} \begin{array}{l} d^{\prime} R^{\prime}\left(x_{2} \mathbf{e}_{1}-x_{1} \mathbf{e}_{2}\right)=d_{1} x_{2} \mathbf{e}_{1}-d_{2} x_{1} \mathbf{e}_{2} \\ d^{\prime} R^{\prime}\left(x_{3} \mathbf{e}_{2}-x_{2} \mathbf{e}_{3}\right)=d_{2} x_{3} \mathbf{e}_{2}-d_{3} x_{2} \mathbf{e}_{3} \\ d^{\prime} R^{\prime}\left(x_{1} \mathbf{e}_{3}-x_{3} \mathbf{e}_{1}\right)=d_{3} x_{1} \mathbf{e}_{3}-d_{1} x_{3} \mathbf{e}_{1} \end{array} d′R′(x2​e1​−x1​e2​)=d1​x2​e1​−d2​x1​e2​d′R′(x3​e2​−x2​e3​)=d2​x3​e2​−d3​x2​e3​d′R′(x1​e3​−x3​e1​)=d3​x1​e3​−d1​x3​e1​​(17)根据前面类似的结论，可以知道 R ′ R' R′ 为旋转矩阵，满足 R ′ T R ′ = E R'^TR'=E R′TR′=E。也就是 ∣ ∣ R ′ X ∣ ∣ = ∣ ∣ X ∣ ∣ ||R'X||=||X|| ∣∣R′X∣∣=∣∣X∣∣，对上式两边取二f范数可得： { ( d ′ 2 − d 2 2 ) x 1 2 + ( d ′ 2 − d 1 2 ) x 2 2 = 0 ( d ′ 2 − d 3 2 ) x 2 2 + ( d ′ 2 − d 2 2 ) x 3 2 = 0 ( d ′ 2 − d 1 2 ) x 3 2 + ( d ′ 2 − d 3 2 ) x 1 2 = 0 (18) \tag{18} \color{blue} \left\{\begin{array}{l} \left(d^{\prime 2}-d_{2}^{2}\right) x_{1}^{2}+\left(d^{\prime 2}-d_{1}^{2}\right) x_{2}^{2}=0 \\ \left(d^{\prime 2}-d_{3}^{2}\right) x_{2}^{2}+\left(d^{\prime 2}-d_{2}^{2}\right) x_{3}^{2}=0 \\ \left(d^{\prime 2}-d_{1}^{2}\right) x_{3}^{2}+\left(d^{\prime 2}-d_{3}^{2}\right) x_{1}^{2}=0 \end{array}\right. ⎩ ⎨ ⎧​(d′2−d22​)x12​+(d′2−d12​)x22​=0(d′2−d32​)x22​+(d′2−d22​)x32​=0(d′2−d12​)x32​+(d′2−d32​)x12​=0​(18)上式是一个关于 x 1 2 、 x 2 2 、 x 3 2 x_1^2、x_2^2、x_3^2 x12​、x22​、x32​ 的一个齐次线性方程，又根据 n ′ = ( x 1 , x 2 , x 3 ) T n'=(x_1, x_2, x_3)^T n′=(x1​,x2​,x3​)T 为单位向量，也就是 x 1 2 + x 2 2 + x 3 2 = ∑ i = 1 3 x i 2 = 1 x_1^2+x_2^2+x_3^2=\sum_{i=1}^3x_i^2=1 x12​+x22​+x32​=∑i=13​xi2​=1，且系数矩阵的秩小于3，需要求该线性方程的非零解，其行列式必定为0，也就有： ( d ′ 2 − d 1 2 ) ( d ′ 2 − d 2 2 ) ( d ′ 2 − d 3 2 ) = 0 (19) \tag{19} \color{blue} \left(d^{\prime 2}-d_{1}^{2}\right)\left(d^{\prime 2}-d_{2}^{2}\right)\left(d^{\prime 2}-d_{3}^{2}\right)=0 (d′2−d12​)(d′2−d22​)(d′2−d32​)=0(19)  情形一:( d ′ = ± d 1 \color{red}d'=\pm d_1 d′=±d1​) 如果 d = ± d 1 d=\pm d_1 d=±d1​ 带入(18)式，其为一解为 x 1 = x 2 = x 3 = 0 x_1=x_2=x_3=0 x1​=x2​=x3​=0，很明显其不满足 ∑ i = 1 3 x i 2 = 1 \sum_{i=1}^3x_i^2=1 ∑i=13​xi2​=1, 说明其解为无效解。   情形二:( d ′ = ± d 3 \color{red}d'=\pm d_3 d′=±d3​) 如果 d = ± d 3 d=\pm d_3 d=±d3​ 带入(18)式，其为一解为 x 1 = x 2 = x 3 = 0 x_1=x_2=x_3=0 x1​=x2​=x3​=0，很明显其不满足 ∑ i = 1 3 x i 2 = 1 \sum_{i=1}^3x_i^2=1 ∑i=13​xi2​=1, 说明其解为无效解。

 

四、齐次方程求解

根据上面我们已经讨论了情形一与情形二，都是比较简单的，但是很明显都是无效解的，倒是还有一种情形没有讨论，那就是( d = ± d 2 d=\pm d_2 d=±d2​),针对于这该情形，会出现好几种情况，现在分别对其进行讨论,当 d ′ = ± d 2 d'=\pm d_2 d′=±d2​ 时，带入(18）式，并且根据 ∑ i = 1 3 x i 2 = 1 \sum_{i=1}^3x_i^2=1 ∑i=13​xi2​=1 可以获得(前面令 n ′ = ( x 1 , x 2 , x 3 ) \mathbf n'=(x_1,x_2,x_3) n′=(x1​,x2​,x3​)) { x 1 = ε 1 d 1 2 − d 2 2 d 1 2 − d 3 2 x 2 = 0 x 3 = ε 3 d 2 2 − d 3 2 d 1 1 − d 3 2             ε 1 , ε 3 = ± 1 (20) \tag{20} \color{blue} \left\{\begin{array}{l} x_{1}=\varepsilon_{1} \sqrt{\frac{d_{1}^{2}-d_{2}^{2}}{d_{1}^{2}-d_{3}^{2}}} \\ x_{2}=0 \\ x_{3}=\varepsilon_{3} \sqrt{\frac{d_{2}^{2}-d_{3}^{2}}{d_{1}^{1}-d_{3}^{2}}} \end{array}~~~~~~~~~~~ \varepsilon_{1}, \varepsilon_{3}=\pm 1\right. ⎩ ⎨ ⎧​x1​=ε1​d12​−d32​d12​−d22​​ ​x2​=0x3​=ε3​d11​−d32​d22​−d32​​ ​​           ε1​,ε3​=±1(20)然后我们在此基础上，再根据 d 1 ≥ d 2 ≥ d 3 d_1\geq d_2\geq d_3 d1​≥d2​≥d3​ 分多种情况进行讨论。主要分为三种情况： ( 1 ) d 1 ≠ d 2 ≠ d 3 ( 2 ) d 1 = d 2 ≠ d 3 或者 d 1 ≠ d 2 = d 3 ( 3 ) d 1 = d 2 = d 3 (21) \tag{21} \color{blue} (1)d_1≠d_2≠d_3\\ (2)d_1=d_2≠d_3 或者d_1≠d_2=d_3\\(3)d_1=d_2=d_3 (1)d1​=d2​=d3​(2)d1​=d2​=d3​或者d1​=d2​=d3​(3)d1​=d2​=d3​(21)  情况一：( d ′ = d 2 > 0 \color{red}d'=d_2>0 d′=d2​>0)，满足该条件下，进行细致讨论 ( 1 ) 当 d 1 ≠ d 2 ≠ d 3 时 \color{blue} (1)当d_1≠d_2≠d_3时 (1)当d1​=d2​=d3​时         由于 d ′ = d 2 、 x 2 = 0 d'=d_2、 x_2=0 d′=d2​、x2​=0,带入(16)中的第二个等式中，可以得到 e 2 = R ′ e 2 e_2=R'e_2 e2​=R′e2​ 可以得知， R ′ R' R′ 是沿着轴 e 2 e_2 e2​ 旋转的，那么就有： R ′ = ( cos ⁡ θ 0 − sin ⁡ θ 0 1 0 sin ⁡ θ 0 cos ⁡ θ ) R^{\prime}=\left(\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right) R′=⎝ ⎛​cosθ0sinθ​010​−sinθ0cosθ​⎠ ⎞​其代入(17)式中的第三个式子，可得： ( cos ⁡ θ 0 − sin ⁡ θ 0 1 0 sin ⁡ θ 0 cos ⁡ θ ) ( − x 3 0 x 1 ) = ( − d 1 x 3 0 d 3 x 1 ) \left(\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right)\left(\begin{array}{c} -x_{3} \\ 0 \\ x_{1} \end{array}\right)=\left(\begin{array}{c} -d_{1} x_{3} \\ 0 \\ d_{3} x_{1} \end{array}\right) ⎝ ⎛​cosθ0sinθ​010​−sinθ0cosθ​⎠ ⎞​⎝ ⎛​−x3​0x1​​⎠ ⎞​=⎝ ⎛​−d1​x3​0d3​x1​​⎠ ⎞​结合式子(20)可解得： { sin ⁡ θ = d 1 − d 3 d 2 x 1 x 3 = ε 1 ε 3 ( d 1 2 − d 2 2 ) ( d 2 2 − d 3 2 ) ( d 1 − d 3 ) d 2 cos ⁡ θ = d 1 x 3 2 − d 3 x 1 2 d 2 = d 2 2 + d 1 d 3 ( d 1 + d 3 ) d 2 \left\{\begin{array}{l} \sin \theta=\frac{d_{1}-d_{3}}{d_{2}} x_{1} x_{3}=\varepsilon_{1} \varepsilon_{3} \frac{\sqrt{\left(d_{1}^{2}-d_{2}^{2}\right)\left(d_{2}^{2}-d_{3}^{2}\right)}}{\left(d_{1}-d_{3}\right) d_{2}} \\ \cos \theta=\frac{d_{1} x_{3}^{2}-d_{3} x_{1}^{2}}{d_{2}}=\frac{d_{2}^{2}+d_{1} d_{3}}{\left(d_{1}+d_{3}\right) d_{2}} \end{array}\right. ⎩ ⎨ ⎧​sinθ=d2​d1​−d3​​x1​x3​=ε1​ε3​(d1​−d3​)d2​(d12​−d22​)(d22​−d32​) ​​cosθ=d2​d1​x32​−d3​x12​​=(d1​+d3​)d2​d22​+d1​d3​​​由于 n ′ T n ′ = 1 n'^Tn'=1 n′Tn′=1, (15)式两边同时乘 n ′ n' n′ 然后位移可以得到: t ′ = ( d 1 0 0 0 d 2 0 0 0 d 3 ) ( x 1 0 x 3 ) − d 2 ( cos ⁡ θ 0 − sin ⁡ θ 0 1 0 sin ⁡ θ 0 cos ⁡ θ ) ( x 1 0 x 3 ) \mathbf{t}^{\prime}=\left(\begin{array}{ccc} d_{1} & 0 & 0 \\ 0 & d_{2} & 0 \\ 0 & 0 & d_{3} \end{array}\right)\left(\begin{array}{c} x_{1} \\ 0 \\ x_{3} \end{array}\right)-d_{2}\left(\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right)\left(\begin{array}{c} x_{1} \\ 0 \\ x_{3} \end{array}\right) t′=⎝ ⎛​d1​00​0d2​0​00d3​​⎠ ⎞​⎝ ⎛​x1​0x3​​⎠ ⎞​−d2​⎝ ⎛​cosθ0sinθ​010​−sinθ0cosθ​⎠ ⎞​⎝ ⎛​x1​0x3​​⎠ ⎞​结合之前式可以推导得出： t ′ = ( d 1 − d 3 ) ( x 1 0 x 3 ) \mathbf{t}^{\prime}=\left(d_{1}-d_{3}\right)\left(\begin{array}{c} x_{1} \\ 0 \\ x_{3} \end{array}\right) t′=(d1​−d3​)⎝ ⎛​x1​0x3​​⎠ ⎞​   ( 2 ) 当 d 1 = d 2 ≠ d 3 时 \color{blue} (2)当d_1=d_2≠d_3时 (2)当d1​=d2​=d3​时         根据式子(20)，这时有 x 1 = x 2 = 0 , x 3 = ± 1 x_1=x_2=0, x_3=\pm1 x1​=x2​=0,x3​=±1, 可得 n ′ = ( x 1 , x 2 , x 3 ) = ( 0 , 0 , ± 1 ) T \mathbf n'=(x_1,x_2,x_3)=(0,0,\pm1)^T n′=(x1​,x2​,x3​)=(0,0,±1)T, 结合式子(16), 解为： { R ′ = E t ′ = ( d 3 − d 1 ) n ′ \left\{\begin{array}{l} R^{\prime}=E \\ \mathbf{t}^{\prime}=\left(d_{3}-d_{1}\right) \mathbf{n}^{\prime} \end{array}\right. {R′=Et′=(d3​−d1​)n′​ ( 3 ) 当 d 1 = d 2 = d 3 时 \color{blue} (3)当d_1=d_2=d_3时 (3)当d1​=d2​=d3​时         这时 x 1 、 x 2 、 x 3 x_1、x_2、x_3 x1​、x2​、x3​ 未定义，根据式(16),(17)可得: { R ′ = E t ′ = 0 \left\{\begin{array}{l} R^{\prime}=E \\ \mathbf{t}^{\prime}=\mathbf{0} \end{array}\right. {R′=Et′=0​

  情况二：( d ′ = − d 2 < 0 \color{red}d'=- d_20 d′=d2​>0)，情况二：( d ′ = − d 2 < 0 \color{red}d'=- d_20,d'=- d_20,d′=−d2​ 0 Z_1>0 Z1​>0, 有： n T X 1 d Z 1 = n T x 1 d > 0 \color{blue} \frac{\mathbf{n}^{T} \mathbf{X}_{1}}{d Z_{1}}=\frac{\mathbf{n}^{T} \mathbf{x}_{1}}{d}>0 dZ1​nTX1​​=dnTx1​​>0所以剩下的四组解中，只有两组是有效的，另外在论文中提到：当观测到的点不是全部靠近一个相机光心而远离另外一个相机光心，那么只有一个解满足所有的点。如果有 d ′ > 0 d'>0 d′>0, 对于上述两个解 ( n 1 ′ , t 1 ′ \mathbf{n}'_1,\mathbf{t}'_1 n1′​,t1′​) 和 ( n 2 ′ , t 2 ′ \mathbf{n}'_2,\mathbf{t}'_2 n2′​,t2′​)，这两个解有如下关系，也就是两个解是相同的( 本人没有明白所以然 \color{red}本人没有明白所以然 本人没有明白所以然),如下： { t 2 ′ = ( d 1 − d 3 ) n 1 ′ n 2 ′ = t 1 ′ d 1 − d 3 \color{blue} \left\{\begin{array}{l} \mathbf{t}_{2}^{\prime}=\left(d_{1}-d_{3}\right) \mathbf{n}_{1}^{\prime} \\ \mathbf{n}_{2}^{\prime}=\frac{\mathbf{t}_{1}^{\prime}}{d_{1}-d_{3}} \end{array}\right. {t2′​=(d1​−d3​)n1′​n2′​=d1​−d3​t1′​​​

注意 \color{red}注意 注意ORB-SLAM2的代码，是在求出所有的8个解之后，对每一个解进行分析，检测点是不是都在两个相机的前方，并且统计重投影误差较小的点的个数，找出8个解中统计得到点数最多的的那个解作为最终的解。其代码实现如下。

 

六、代码注释

位于 src/Initializer.cc 文件中的 Initializer::ReconstructH() 函数

/**
 * @brief 用H矩阵恢复R, t和三维点
 * H矩阵分解常见有两种方法：Faugeras SVD-based decomposition 和 Zhang SVD-based decomposition
 * 代码使用了Faugeras SVD-based decomposition算法，参考文献
 * Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988 
 * 
 * @param[in] vbMatchesInliers          匹配点对的内点标记
 * @param[in] H21                       从参考帧到当前帧的单应矩阵
 * @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::ReconstructH(vector &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                      cv::Mat &R21, cv::Mat &t21, vector &vP3D, vector &vbTriangulated, float minParallax, int minTriangulated)
{

    // 目的 ：通过单应矩阵H恢复两帧图像之间的旋转矩阵R和平移向量T
    // 参考 ：Motion and structure from motion in a piecewise plannar environment.
    //        International Journal of Pattern Recognition and Artificial Intelligence, 1988
    // https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment
    
    // 流程:
    //      1. 根据H矩阵的奇异值d'= d2 或者 d' = -d2 分别计算 H 矩阵分解的 8 组解
    //        1.1 讨论 d' > 0 时的 4 组解
    //        1.2 讨论 d' < 0 时的 4 组解
    //      2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解

    // 统计匹配的特征点对中属于内点(Inlier)或有效点个数
    int N=0;
    for(size_t i=0, iend = vbMatchesInliers.size() ; i=d2>=d3
    if(d1/d2