(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
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过前面的文章，知道了如何求解Homography 以及 Fundamental 矩阵。但是这是几个中间过程而已，求解 Homography 以及 Fundamental 是为了进一步求解旋转矩阵 $R$ 以及偏移矩阵 $t$, 通过 Homography 求解 $Rt$ 主要有以下两个方式：
$FaugerasSVD-baseddecomposition$：Motion and Structure from Motion in a Piecewise Planar Environment
$ZhangSVD-baseddecomposition$：D Reconstruction Based on Homography Mapping

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

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

 

二、适用场景

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

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

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

 

三、公式推导

首先设三维空间的一个点 $\mathbf{X}$, 以及该点再图像平面的坐标为 $\mathbf{x}$(假设焦距 f 为1),如下：
$$\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\text{\hspace{1em}(1)}$$ 那么根据相机的成像原理(可以参考图一),其存在如下关系：$$\frac{X}{x}=\frac{Y}{y}=Z\text{\hspace{1em}(2)}$$$令第一个相机坐标为世界坐标$，设相机投影模型(E表示单位矩阵,R|T表示把旋转矩阵与偏移矩阵按列拼接起来)
$$P_1=K_1[E|0]P_2=K_2[R|t]\text{\hspace{1em}(3)}$$设3D点所在的世界平面方程方程，与法向量 $\mathbf{n}$ 如下(单位向量)，其中的 $d$ 表示坐标系原点到平面的距离，$$平面方程：aX+bY+cZ=d$$$$法向量：\mathbf{n}={\left(\begin{array}{lll}a& b& c\end{array}\right)}^T$$$$根据以上两个式子:{\mathbf{n}}^T\mathbf{X}=d$$那么带入平面上的点 $\mathbf{X}$ ，其平面方程可以是表示为:$$\frac{1}{d}{\mathbf{n}}^T\mathbf{X}=1\text{\hspace{1em}(4)}$$那么我们设平面上的点 ${\mathbf{X}}_1$满足上式(4)(以第一个相机坐标为世界坐标)，也就是：
$$1=\frac{1}{d}{\mathbf{n}}^T{\mathbf{X}}_1\text{\hspace{1em}(5)}$$对于两个相机坐标系点的关系 如下:$${\mathbf{X}}_2=R{\mathbf{X}}_1+\mathbf{t}\text{\hspace{1em}(6)}$$那么我们把(5)式乘到(6)式的 $\mathbf{t}$ 后面，可以得到如下：$${\mathbf{X}}_2=\left(R+\frac{1}{d}{\mathbf{tn}}^T\right){\mathbf{X}}_1\text{\hspace{1em}(7)}$$根据单应性变换，对图像齐次坐标 $\mathbf{x}={\left(\begin{array}{lll}x& y& 1\end{array}\right)}^T$和齐次单应矩阵 $H$ 那么两个图像的坐标满足 $s{\mathbf{x}}_2=H{\mathbf{x}}_1$ ,这里的 $s$ 可以理解为缩放，根据相机投影模型：$${\mathbf{x}}_1=K_1{\mathbf{X}}_1{\mathbf{x}}_2=K_2{\mathbf{X}}_2\text{\hspace{1em}(8)}$$我们可以推导出$${\mathbf{X}}_2=K_2^{-1}H{K}_1{\mathbf{X}}_1\text{\hspace{1em}(9)}$$

根据 (7), (9) 我们可以得到(如果是同一个相机，这里的 ${\mathbf{K}}_1={\mathbf{K}}_2$)$$A=dR+{\mathbf{tn}}^T=d{K}_2^{-1}H{K}_1\text{\hspace{1em}(10)}$$上面的公式我们可以求得 $H$ 矩阵与 $Rt$ 的对应关系(略过)，对 $A$ 进行奇异值分解，如下 $$A=U\Lambda V^T\Lambda =\mathrm{diag}\left(d_1,d_2,d_3\right)\text{\hspace{1em}(11)}$$另外由于 $U,V$ 满足 $\left(其中U^{T}U=V^{T}V=E\right)$，可以得到：$$\Lambda =U^{T}AV=d{U}^{T}RV+\left(U^T\mathbf{t}\right){\left(V^T\mathbf{n}\right)}^T\text{\hspace{1em}(12)}$$$U,V$为正交矩阵，也就是说其行列式为 $\pm 1$，即 $s=\det (U)=\det (V)$, 且 $s^2=1$， 令:$$\{\begin{array}{l}{R}^{\prime }=s{U}^{T}RV\\ {\mathbf{t}}^{\prime }=U^T\mathbf{t}\\ {\mathbf{n}}^{\prime }=V^T\mathbf{n}\\ d^{\prime }=sd\\ s=\det (U)\det (V)\end{array}⟹\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}\text{\hspace{1em}(13)}$$ 其上 $V_T$ 是行列式为 $\pm 1$ 的正交矩阵，那么其也是一个旋转矩阵(可以参考该片博客末尾史上最简SLAM零基础解读(1) - 旋转平移矩阵→欧式变换推导),该矩阵作用于单位法向量 $n$,也就是对齐进行旋转，也就是说 $n^{\prime }$ 也是单位向量。取基底 ${\mathbf{e}}_1=(1,0，0{)}^T,{\mathbf{e}}_1=(1,0，0{)}^T,{\mathbf{e}}_1=(1,0，0{)}^T$ ，设单位向量 ${\mathbf{n}}^{\prime }=(x_1,x_2,x_3{)}^T$, 则 ${\mathbf{n}}^{\prime }\mathbf{e}=(x_1{\mathbf{e}}_1+x_2{\mathbf{e}}_2+x_3{\mathbf{e}}_3)$ 根据式子 (13) 可以得到：$$\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}{lll}{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]\text{\hspace{1em}(14)}$$$$\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}{lll}{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]\text{\hspace{1em}(15)}$$即可推导出如下3式：
$$\begin{array}{rl}{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{array}\text{\hspace{1em}(16)}$$
注意到 $n$ 是单位法向量，$V$ 是旋转矩阵，则 $n\prime$ 也是单位法向量，则有${\Sigma }_i^3=x_i^2=1$。对(16)的三个式子消去t′得

$$\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}\text{\hspace{1em}(17)}$$根据前面类似的结论，可以知道 $R^{\prime }$ 为旋转矩阵，满足 $R^{\prime T}{R}^{\prime }=E$。也就是 $||R^{\prime }X||=||X||$，对上式两边取二f范数可得：$$\{\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}\text{\hspace{1em}(18)}$$上式是一个关于$x_1^2、x_2^2、x_3^2$ 的一个齐次线性方程，又根据 $n^{\prime }=(x_1,x_2,x_3{)}^T$ 为单位向量，也就是 $x_1^2+x_2^2+x_3^2={\sum }_{i=1}^3{x}_i^2=1$，且系数矩阵的秩小于3，需要求该线性方程的非零解，其行列式必定为0，也就有：$$\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\text{\hspace{1em}(19)}$$ 
情形一:($d^{\prime }=\pm d_1$)
如果 $d=\pm d_1$ 带入(18)式，其为一解为 $x_1=x_2=x_3=0$，很明显其不满足 ${\sum }_{i=1}^3{x}_i^2=1$, 说明其解为无效解。
 
情形二:($d^{\prime }=\pm d_3$)
如果 $d=\pm d_3$ 带入(18)式，其为一解为 $x_1=x_2=x_3=0$，很明显其不满足 ${\sum }_{i=1}^3{x}_i^2=1$, 说明其解为无效解。

 

四、齐次方程求解

根据上面我们已经讨论了情形一与情形二，都是比较简单的，但是很明显都是无效解的，倒是还有一种情形没有讨论，那就是($d=\pm d_2$),针对于这该情形，会出现好几种情况，现在分别对其进行讨论,当 $d^{\prime }=\pm d_2$ 时，带入(18）式，并且根据 ${\sum }_{i=1}^3{x}_i^2=1$ 可以获得(前面令 ${\mathbf{n}}^{\prime }=(x_1,x_2,x_3)$)
$$\{\begin{array}{l}{x}_1={\epsilon }_1\sqrt{\frac{d_1^2-d_2^2}{d_1^2-d_3^2}}\\ x_2=0\\ x_3={\epsilon }_3\sqrt{\frac{d_2^2-d_3^2}{d_1^1-d_3^2}}\end{array}{\epsilon }_1,{\epsilon }_3=\pm 1\text{\hspace{1em}(20)}$$然后我们在此基础上，再根据 $d_1\ge d_2\ge d_3$ 分多种情况进行讨论。主要分为三种情况：
$$\begin{gathered} (1)d_1\ne d_2\ne d_3 \\ (2)d_1=d_2\ne d_3或者d_1\ne d_2=d_3 \\ (3)d_1=d_2=d_3\text{\hspace{1em}(21)} \end{gathered}$$ 
情况一：($d^{\prime }=d_2>0$)，满足该条件下，进行细致讨论
$(1)当d_1\ne d_2\ne d_3时$
        由于$d^{\prime }=d_2、x_2=0$,带入(16)中的第二个等式中，可以得到 $e_2=R^{\prime }{e}_2$ 可以得知，$R^{\prime }$ 是沿着轴 $e_2$ 旋转的，那么就有：
$$R^{\prime }=\left(\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right)$$其代入(17)式中的第三个式子，可得：$$\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)$$结合式子(20)可解得：$$\{\begin{array}{l}\sin \theta =\frac{d_1-d_3}{d_2}{x}_1{x}_3={\epsilon }_1{\epsilon }_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}$$由于$n^{\prime T}{n}^{\prime }=1$, (15)式两边同时乘 $n^{\prime }$ 然后位移可以得到:$${\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)$$结合之前式可以推导得出：$${\mathbf{t}}^{\prime }=\left(d_1-d_3\right)\left(\begin{array}{c}{x}_1\\ 0\\ x_3\end{array}\right)$$
 
$(2)当d_1=d_2\ne d_3时$
        根据式子(20)，这时有$x_1=x_2=0,x_3=\pm 1$, 可得 ${\mathbf{n}}^{\prime }=(x_1,x_2,x_3)=(0,0,\pm 1{)}^T$, 结合式子(16), 解为：$$\{\begin{array}{l}{R}^{\prime }=E\\ {\mathbf{t}}^{\prime }=\left(d_3-d_1\right){\mathbf{n}}^{\prime }\end{array}$$
$(3)当d_1=d_2=d_3时$
        这时$x_1、x_2、x_3$ 未定义，根据式(16),(17)可得:
$$\{\begin{array}{l}{R}^{\prime }=E\\ {\mathbf{t}}^{\prime }=0\end{array}$$

 
情况二：($d^{\prime }=-d_2<0$), 满足该条件下，进行细致讨论

$(1)当d_1\ne d_2\ne d_3时$
        把 $d^{\prime }=-d_2$ 代入(16)的第二个等式得 $-e_2=R^{\prime }{e}_2$, 此时 $R^{\prime }$ 可以表示为$$R^{\prime }=\left(\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& -1& 0\\ \sin \theta & 0& -\cos \theta \end{array}\right)$$该式代入(17)式中的第三个等式，得：$$\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)$$结合式(20)可以解得：$$\{\begin{array}{l}\sin \theta =\frac{d_1+d_3}{d_2}{x}_1{x}_3={\epsilon }_1{\epsilon }_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_3{x}_1^2-d_1{x}_3^2}{d_2}=\frac{d_1{d}_3-d_2^2}{\left(d_1-d_3\right)d_2}\end{array}$$上述推出得式子，带入到(15)式中可得：$${\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)=\left(d_1+d_3\right)\left(\begin{array}{c}{x}_1\\ 0\\ x_3\end{array}\right)$$

 
$(2)当d_1=d_2\ne d_3时$
        结合式(20)，这时有$x_1=x_2=0,x_3=\pm 1$, 可得 ${\mathbf{n}}^{\prime }=(x_1,x_2,x_3)=(0,0,\pm 1{)}^T$,结合式子(16)，解为$$\{\begin{array}{c}{R}^{\prime }=\left[\begin{array}{ccc}-1& 0& 0\\ 0& -1& 0\\ 0& 0& 1\end{array}\right]\\ {\mathbf{t}}^{\prime }=\left(d_3+d_1\right){\mathbf{n}}^{\prime }\end{array}$$ 
$(3)当d_1=d_2=d_3时$
        根据公式(13)有 $-d^{\prime }E=d^{\prime }{R}^{\prime }+{\mathbf{t}}^{\prime }{\mathbf{n}}^{\prime T}$, 也就有，对于与 ${\mathbf{n}}^{\prime }$ 垂直(在平面内) 的向量 $\mathbf{x}$, 有$R^{\prime }\mathbf{x}=-\mathbf{x}$, 根据 householder 变换得到：$$\{\begin{array}{l}{R}^{\prime }=-E+2{\mathbf{n}}^{\prime }{\mathbf{n}}^{\prime T}\\ {\mathbf{t}}^{\prime }=-2d^{\prime }{\mathbf{n}}^{\prime }\end{array}$$
 

五、方程解梳理

根据上面的分析，其一共是八组解，其中情形一:($d^{\prime }=\pm d_1$)，与 情形二:($d^{\prime }=\pm d_3$) 都属于无效解。另外六种解分成两种情况，分别为情况一：($d^{\prime }=d_2>0$)，情况二：($d^{\prime }=-d_2<0$), 每种情况三种解。

我们从另外一个角度来分析，为什么其是八组解呢。首先我们回顾(20)式, 其中包含了 ${\epsilon }_1,{\epsilon }_3=\pm 1$,也就是共 4 种组合，然后结合前面的分析，可以知道 $d^{\prime }$ 可能存在两种情况，分别为 $d^{\prime }=d_2>0,d^{\prime }=-d_2<0$。所以最终解的组合为 4x2=8，但是我们在应用的时候，确不一定能得到这么多组解，为什么呢?

我们来回顾一下之前是式子(10)，如下所示(如果为同一相机则 ${\mathbf{K}}_1={\mathbf{K}}_2$)：$$A=dR+{\mathbf{tn}}^T=d{K}_2^{-1}H{K}_1\text{\hspace{1em}(10)}$$在实际执行代码的过程种，其上的 $A$ 是确定的，因为 $s=\det (U)\det (V)=1$, $K_1,K_2$ 为两个相机的内参，$X_1$ 为相机一种的世界坐标，又 ${\mathbf{x}}_1=K_1{\mathbf{X}}_1⟹{\mathbf{X}}_1={\mathbf{x}}_1{K}_1^{-1}$, 也就是 ${\mathbf{X}}_1$ 可以由相机内参以及相机坐标转换而来。所以 $A$ 在代码执行过程中，其是一个确定的值。

另外 $s=\det (U)\det (V)$ 是确定的，所以$d^{\prime }=sd$ 也是确定的，也就是代码执行过程中，是知道 $d^{\prime }$ 是否大于0，这样就能够排除 4 组解了。 假设四组解对应的法向量 $\mathbf{n}$ 为 ${\mathbf{n}}_1,{-\mathbf{n}}_1,{\mathbf{n}}_2,{-\mathbf{n}}_2,$, 根据(4) 式，以及考虑到 $Z_1>0$, 有：$$\frac{{\mathbf{n}}^T{\mathbf{X}}_1}{d{Z}_1}=\frac{{\mathbf{n}}^T{\mathbf{x}}_1}{d}>0$$所以剩下的四组解中，只有两组是有效的，另外在论文中提到：当观测到的点不是全部靠近一个相机光心而远离另外一个相机光心，那么只有一个解满足所有的点。如果有 $d^{\prime }>0$, 对于上述两个解 (${\mathbf{n}}_1^{\prime },{\mathbf{t}}_1^{\prime }$) 和 (${\mathbf{n}}_2^{\prime },{\mathbf{t}}_2^{\prime }$)，这两个解有如下关系，也就是两个解是相同的($本人没有明白所以然$),如下：$$\{\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}$$

$注意$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<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                      cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &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<iend; i++)
        if(vbMatchesInliers[i])
            N++;

    // We recover 8 motion hypotheses using the method of Faugeras et al.
    // Motion and structure from motion in a piecewise planar environment.
    // International Journal of Pattern Recognition and Artificial Intelligence, 1988

    // 参考SLAM十四讲第二版p170-p171
    // H = K * (R - t * n / d) * K_inv
    // 其中: K表示内参数矩阵
    //       K_inv 表示内参数矩阵的逆
    //       R 和 t 表示旋转和平移向量
    //       n 表示平面法向量
    // 令 H = K * A * K_inv
    // 则 A = k_inv * H * k

    cv::Mat invK = K.inv();
    cv::Mat A = invK*H21*K;

    // 对矩阵A进行SVD分解
    // A 等待被进行奇异值分解的矩阵
    // w 奇异值矩阵
    // U 奇异值分解左矩阵
    // Vt 奇异值分解右矩阵，注意函数返回的是转置
    // cv::SVD::FULL_UV 全部分解
    // A = U * w * Vt
    cv::Mat U,w,Vt,V;
    cv::SVD::compute(A, w, U, Vt, cv::SVD::FULL_UV);

    // 根据文献eq(8)，计算关联变量
    V=Vt.t();

    // 计算变量s = det(U) * det(V)
    // 因为det(V)==det(Vt), 所以 s = det(U) * det(Vt)
    float s = cv::determinant(U)*cv::determinant(Vt);
    
    // 取得矩阵的各个奇异值
    float d1 = w.at<float>(0);
    float d2 = w.at<float>(1);
    float d3 = w.at<float>(2);

    // SVD分解正常情况下特征值di应该是正的，且满足d1>=d2>=d3
    if(d1/d2<1.00001 || d2/d3<1.00001) {
        return false;
    }


    // 在ORBSLAM中没有对奇异值 d1 d2 d3按照论文中描述的关系进行分类讨论, 而是直接进行了计算
    // 定义8中情况下的旋转矩阵、平移向量和空间向量
    vector<cv::Mat> vR, vt, vn;
    vR.reserve(8);
    vt.reserve(8);
    vn.reserve(8);

    // Step 1.1 讨论 d' > 0 时的 4 组解
    // 根据论文eq.(12)有
    // x1 = e1 * sqrt((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3))
    // x2 = 0
    // x3 = e3 * sqrt((d2 * d2 - d2 * d2) / (d1 * d1 - d3 * d3))
    // 令 aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3))
    //    aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3))
    // 则
    // x1 = e1 * aux1
    // x3 = e3 * aux2

    // 因为 e1,e2,e3 = 1 or -1
    // 所以有x1和x3有四种组合
    // x1 =  {aux1,aux1,-aux1,-aux1}
    // x3 =  {aux3,-aux3,aux3,-aux3}

    float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
    float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
    float x1[] = {aux1,aux1,-aux1,-aux1};
    float x3[] = {aux3,-aux3,aux3,-aux3};


    // 根据论文eq.(13)有
    // sin(theta) = e1 * e3 * sqrt(( d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)) /(d1 + d3)/d2
    // cos(theta) = (d2* d2 + d1 * d3) / (d1 + d3) / d2 
    // 令  aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2)
    // 则  sin(theta) = e1 * e3 * aux_stheta
    //     cos(theta) = (d2*d2+d1*d3)/((d1+d3)*d2)
    // 因为 e1 e2 e3 = 1 or -1
    // 所以 sin(theta) = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}
    float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
    float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
    float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};

    // 计算旋转矩阵 R'
    //根据不同的e1 e3组合所得出来的四种R t的解
    //      | ctheta      0   -aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0    aux_stheta|       | aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       | aux3|

    //      | ctheta      0    aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      |-aux_stheta  0    ctheta    |       |-aux3|

    //      | ctheta      0   -aux_stheta|       |-aux1|
    // Rp = |    0        1       0      |  tp = |  0  |
    //      | aux_stheta  0    ctheta    |       | aux3|
    // 开始遍历这四种情况中的每一种
    for(int i=0; i<4; i++)
    {
        //生成Rp，就是eq.(8) 的 R'
        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
        Rp.at<float>(0,0)=ctheta;
        Rp.at<float>(0,2)=-stheta[i];
        Rp.at<float>(2,0)=stheta[i];        
        Rp.at<float>(2,2)=ctheta;

        // eq.(8) 计算R
        cv::Mat R = s*U*Rp*Vt;

        // 保存
        vR.push_back(R);

        // eq. (14) 生成tp 
        cv::Mat tp(3,1,CV_32F);
        tp.at<float>(0)=x1[i];
        tp.at<float>(1)=0;
        tp.at<float>(2)=-x3[i];
        tp*=d1-d3;

        // 这里虽然对t有归一化，并没有决定单目整个SLAM过程的尺度
        // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变
        // eq.(8)恢复原始的t
        cv::Mat t = U*tp;
        vt.push_back(t/cv::norm(t));

        // 构造法向量np
        cv::Mat np(3,1,CV_32F);
        np.at<float>(0)=x1[i];
        np.at<float>(1)=0;
        np.at<float>(2)=x3[i];

        // eq.(8) 恢复原始的法向量
        cv::Mat n = V*np;
        //看PPT 16页的图，保持平面法向量向上
        if(n.at<float>(2)<0)
            n=-n;
        // 添加到vector
        vn.push_back(n);
    }
    
    // Step 1.2 讨论 d' < 0 时的 4 组解
    float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
    // cos_theta项
    float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
    // 考虑到e1,e2的取值，这里的sin_theta有两种可能的解
    float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};

    // 对于每种由e1 e3取值的组合而形成的四种解的情况
    for(int i=0; i<4; i++)
    {
        // 计算旋转矩阵 R'
        cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
        Rp.at<float>(0,0)=cphi;
        Rp.at<float>(0,2)=sphi[i];
        Rp.at<float>(1,1)=-1;
        Rp.at<float>(2,0)=sphi[i];
        Rp.at<float>(2,2)=-cphi;

        // 恢复出原来的R
        cv::Mat R = s*U*Rp*Vt;
        // 然后添加到vector中
        vR.push_back(R);

        // 构造tp
        cv::Mat tp(3,1,CV_32F);
        tp.at<float>(0)=x1[i];
        tp.at<float>(1)=0;
        tp.at<float>(2)=x3[i];
        tp*=d1+d3;

        // 恢复出原来的t
        cv::Mat t = U*tp;
        // 归一化之后加入到vector中,要提供给上面的平移矩阵都是要进行过归一化的
        vt.push_back(t/cv::norm(t));

        // 构造法向量np
        cv::Mat np(3,1,CV_32F);
        np.at<float>(0)=x1[i];
        np.at<float>(1)=0;
        np.at<float>(2)=x3[i];

        // 恢复出原来的法向量
        cv::Mat n = V*np;
        // 保证法向量指向上方
        if(n.at<float>(2)<0)
            n=-n;
        // 添加到vector中
        vn.push_back(n);
    }

    // 最好的good点
    int bestGood = 0;
    // 其次最好的good点
    int secondBestGood = 0;    
    // 最好的解的索引，初始值为-1
    int bestSolutionIdx = -1;
    // 最大的视差角
    float bestParallax = -1;
    // 存储最好解对应的，对特征点对进行三角化测量的结果
    vector<cv::Point3f> bestP3D;
    // 最佳解所对应的，那些可以被三角化测量的点的标记
    vector<bool> bestTriangulated;

    // Instead of applying the visibility constraints proposed in the WFaugeras' paper (which could fail for points seen with low parallax)
    // We reconstruct all hypotheses and check in terms of triangulated points and parallax
    
    // Step 2. 对 8 组解进行验证，并选择产生相机前方最多3D点的解为最优解
    for(size_t i=0; i<8; i++)
    {
        // 第i组解对应的比较大的视差角
        float parallaxi;
        // 三角化测量之后的特征点的空间坐标
        vector<cv::Point3f> vP3Di;
        // 特征点对是否被三角化的标记
        vector<bool> vbTriangulatedi;
    
        // 调用 Initializer::CheckRT(), 计算good点的数目
        int nGood = CheckRT(vR[i],vt[i],                    //当前组解的旋转矩阵和平移向量
                            mvKeys1,mvKeys2,                //特征点
                            mvMatches12,vbMatchesInliers,   //特征匹配关系以及Inlier标记
                            K,                              //相机的内参数矩阵
                            vP3Di,                          //存储三角化测量之后的特征点空间坐标的
                            4.0*mSigma2,                    //三角化过程中允许的最大重投影误差
                            vbTriangulatedi,                //特征点是否被成功进行三角测量的标记
                            parallaxi);                     // 这组解在三角化测量的时候的比较大的视差角
        
        // 更新历史最优和次优的解
        // 保留最优的和次优的解.保存次优解的目的是看看最优解是否突出
        if(nGood>bestGood)
        {
            // 如果当前组解的good点数是历史最优，那么之前的历史最优就变成了历史次优
            secondBestGood = bestGood;
            // 更新历史最优点
            bestGood = nGood;
            // 最优解的组索引为i（就是当前次遍历）
            bestSolutionIdx = i;
            // 更新变量
            bestParallax = parallaxi;
            bestP3D = vP3Di;
            bestTriangulated = vbTriangulatedi;
        }
        // 如果当前组的good计数小于历史最优但却大于历史次优
        else if(nGood>secondBestGood)
        {
            // 说明当前组解是历史次优点，更新之
            secondBestGood = nGood;
        }
    }



    // Step 3 选择最优解。要满足下面的四个条件
    // 1. good点数最优解明显大于次优解，这里取0.75经验值
    // 2. 视角差大于规定的阈值
    // 3. good点数要大于规定的最小的被三角化的点数量
    // 4. good数要足够多，达到总数的90%以上
    if(secondBestGood<0.75*bestGood &&      
       bestParallax>=minParallax &&
       bestGood>minTriangulated && 
       bestGood>0.9*N)
    {
        // 从最佳的解的索引访问到R，t
        vR[bestSolutionIdx].copyTo(R21);
        vt[bestSolutionIdx].copyTo(t21);
        // 获得最佳解时，成功三角化的三维点，以后作为初始地图点使用
        vP3D = bestP3D;
        // 获取特征点的被成功进行三角化的标记
        vbTriangulated = bestTriangulated;

        //返回真，找到了最好的解
        return true;
    }
    return false;
}

 

七、结语

该篇博客讲解的内容比较多，然后难度也比较大。所以需要大家好好琢磨，慢慢消化。另外上述的代码中，有一个地方还没有进行详细的讲解，那就是其上的 CheckRT() 函数，主要是关于三角化的知识，在后面的博客中，

 
 
本文内容来自计算机视觉life ORB-SLAM2 课程课件