多视图几何--2单应矩阵-2.0从0-1理解并计算单应矩阵

文章目录 1 什么是单应矩阵2 单应矩阵与相机内外参关系的一般形式3理论求解方法4提高精度和稳健性的做法4.1归一化4.2RanSAC 5完整的计算过程6 codes by c plus plus参考

1 什么是单应矩阵

单应矩阵（Homography Matrix）是一个描述两个平面之间点的映射关系的矩阵。在计算机视觉中，它常用于描述同一场景在不同视角下的图像之间的几何变换关系。

2 单应矩阵与相机内外参关系的一般形式

由于一般情况下，我们已知相机的外参（即世界坐标系到相机坐标系的变换关系），因此我们不假设平面的坐标系与世界坐标系对齐。假设平面在相机 C 1 C_1 C1​ 下的法向量为 n \boldsymbol{n} n，平面到相机 C 1 C_1 C1​ 的距离为 d d d，并且对于平面上的点 P C 1 P_{C_1} PC1​​，满足以下关系：

n T P C 1 + d = 0 \boldsymbol{n}^T P_{C_1} + d = 0 nTPC1​​+d=0

其中 n T P C 1 \boldsymbol{n}^T P_{C_1} nTPC1​​ 表示点到法向量的投影。

假设相机 C 1 C_1 C1​ 的外参为 T C 1 W = [ R 1 ∣ t 1 ] T_{C_1 W} = [R_1 | \boldsymbol{t}_1] TC1​W​=[R1​∣t1​]，相机 C 2 C_2 C2​ 的外参为 T C 2 W = [ R 2 ∣ t 2 ] T_{C_2 W} = [R_2 | \boldsymbol{t}_2] TC2​W​=[R2​∣t2​]。根据变换矩阵的性质，可以得到：

T W C 1 = T C 1 W − 1 = [ R 1 t 1 0 T 1 ] − 1 = [ R 1 − 1 − R 1 − 1 t 1 0 T 1 ] (2-5) T_{W C_1} = T_{C_1 W}^{-1} = \begin{bmatrix} R_1 & \boldsymbol{t}_1 \\ \boldsymbol{0}^T & 1 \end{bmatrix}^{-1} = \begin{bmatrix} R_1^{-1} & -R_1^{-1} \boldsymbol{t}_1 \\ \boldsymbol{0}^T & 1 \end{bmatrix} \tag{2-5} TWC1​​=TC1​W−1​=[R1​0T​t1​1​]−1=[R1−1​0T​−R1−1​t1​1​](2-5)

因此，相机 1 到相机 2 的变换矩阵为：

T C 2 C 1 = T C 2 W T W C 1 = [ R 2 t 2 0 T 1 ] [ R 1 − 1 − R 1 − 1 t 1 0 T 1 ] = [ R 2 R 1 − 1 − R 2 R 1 − 1 t 1 + t 2 0 T 1 ] = [ R 21 t 21 0 T 1 ] (2-6) T_{C_2 C_1} = T_{C_2 W} T_{W C_1} = \begin{bmatrix} R_2 & \boldsymbol{t}_2 \\ \boldsymbol{0}^T & 1 \end{bmatrix} \begin{bmatrix} R_1^{-1} & -R_1^{-1} \boldsymbol{t}_1 \\ \boldsymbol{0}^T & 1 \end{bmatrix} = \begin{bmatrix} R_2 R_1^{-1} & -R_2 R_1^{-1} \boldsymbol{t}_1 + \boldsymbol{t}_2 \\ \boldsymbol{0}^T & 1 \end{bmatrix} = \begin{bmatrix} R_{21} & \boldsymbol{t}_{21} \\ \boldsymbol{0}^T & 1 \end{bmatrix} \tag{2-6} TC2​C1​​=TC2​W​TWC1​​=[R2​0T​t2​1​][R1−1​0T​−R1−1​t1​1​]=[R2​R1−1​0T​−R2​R1−1​t1​+t2​1​]=[R21​0T​t21​1​](2-6)

根据相机投影关系，有：

p 1 = 1 Z C 1 K 1 P 1 p 2 = 1 Z C 2 K 2 P 2 (2-7) \begin{aligned} & \boldsymbol{p}_1 = \frac{1}{Z_{C_1}} K_1 P_1 \\ & \boldsymbol{p}_2 = \frac{1}{Z_{C_2}} K_2 P_2 \\ \end{aligned} \tag{2-7} ​p1​=ZC1​​1​K1​P1​p2​=ZC2​​1​K2​P2​​(2-7)

P 1 = Z C 1 K 1 − 1 p 1 (2-8) P_1 = Z_{C_1} K_1^{-1} \boldsymbol{p}_1 \tag{2-8} P1​=ZC1​​K1−1​p1​(2-8)

又 P 1 , P 2 P_1, P_2 P1​,P2​ 为点 P W P_W PW​ 在两个相机坐标系下的坐标，这两个点又可以通过下面的变换联系：

P 2 = R 21 P 1 + t 21 (2-9) P_2 = R_{21} P_1 + \boldsymbol{t}_{21} \tag{2-9} P2​=R21​P1​+t21​(2-9)

联合（2-7）到（2-9），可以得出：

p 2 = 1 Z C 2 K 2 ( R 21 Z C 1 K 1 − 1 p 1 + t 21 ) (2-10) \boldsymbol{p}_2 = \frac{1}{Z_{C_2}} K_2 (R_{21} Z_{C_1} K_1^{-1} \boldsymbol{p}_1 + \boldsymbol{t}_{21}) \tag{2-10} p2​=ZC2​​1​K2​(R21​ZC1​​K1−1​p1​+t21​)(2-10)

对于平面，假设在相机 C 1 C_1 C1​ 坐标系下，对于平面上的点 P 1 P_{1} P1​，满足：

n T P 1 − d = 1 (2-11) \frac{\boldsymbol{n}^T P_1}{-d} = 1 \tag{2-11} −dnTP1​​=1(2-11)

将（2-11）带入公式（2-10）中的 t 21 \boldsymbol{t}_{21} t21​ 可以得到：

t 21 = t 21 n T P 1 − d = t 21 n T − d P 1 = 2 − 8 − t 21 n T d Z C 1 K 1 − 1 p 1 (2-12) \begin{aligned} \boldsymbol{t}_{21} &= \boldsymbol{t}_{21} \frac{\boldsymbol{n}^T P_1}{-d} \\ &= \frac{\boldsymbol{t}_{21} \boldsymbol{n}^T}{-d} P_1 \\ &\overset{2-8}{=} -\frac{\boldsymbol{t}_{21} \boldsymbol{n}^T}{d} Z_{C_1} K_1^{-1} \boldsymbol{p}_1 \end{aligned} \tag{2-12} t21​​=t21​−dnTP1​​=−dt21​nT​P1​=2−8−dt21​nT​ZC1​​K1−1​p1​​(2-12)

将（2-12）代入公式（2-10）中，可以得到：

p 2 = 1 Z C 2 K 2 ( R 21 − t 21 n T d ) Z C 1 K 1 − 1 p 1 = H 21 p 1 (2-13) \begin{aligned} \boldsymbol{p}_2 &= \frac{1}{Z_{C_2}} K_2 \left(R_{21} - \frac{\boldsymbol{t}_{21} \boldsymbol{n}^T}{d}\right) Z_{C_1} K_1^{-1} \boldsymbol{p}_1 \\ &= H_{21} \boldsymbol{p}_1 \end{aligned} \tag{2-13} p2​​=ZC2​​1​K2​(R21​−dt21​nT​)ZC1​​K1−1​p1​=H21​p1​​(2-13)

因此，单应矩阵的表达形式为：

H 21 = Z C 1 Z C 2 K 2 ( R 21 − t 21 n T d ) K 1 − 1 (2-14) H_{21} = \frac{Z_{C_1}}{Z_{C_2}} K_2 \left(R_{21} - \frac{\boldsymbol{t}_{21} \boldsymbol{n}^T}{d}\right) K_1^{-1} \tag{2-14} H21​=ZC2​​ZC1​​​K2​(R21​−dt21​nT​)K1−1​(2-14)

如果将 R 21 R_{21} R21​ 与 t 21 t_{21} t21​ 的展开形式（公式 2-6），则可以得到：

H 21 = Z C 1 Z C 2 K 2 ( R 2 R 1 − 1 − ( − R 2 R 1 − 1 t 1 + t 2 ) n T d ) K 1 − 1 (2-15) H_{21} = \frac{Z_{C_1}}{Z_{C_2}} K_2 \left( R_2 R_1^{-1} - \frac{\left(-R_2 R_1^{-1} \boldsymbol{t}_1 + \boldsymbol{t}_2\right) \boldsymbol{n}^T}{d} \right) K_1^{-1} \tag{2-15} H21​=ZC2​​ZC1​​​K2​(R2​R1−1​−d(−R2​R1−1​t1​+t2​)nT​)K1−1​(2-15)

3理论求解方法

2d射影变换定义 平面射影变换是关于齐次 3 维矢量的一种线性变换,并可用一个非奇异 3 × 3 3 \times 3 3×3 矩阵 H 表示为:

[ x 1 ′ x 2 ′ x 3 ′ ] = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] [ x 1 x 2 x 3 ] (1) \left[\begin{array}{l} x_1^{\prime} \\ x_2^{\prime} \\ x_3^{\prime} \end{array}\right]=\left[\begin{array}{lll} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{array}\right]\left[\begin{array}{l} x_1 \\ x_2 \\ x_3 \end{array}\right]\tag{1} ​x1′​x2′​x3′​​ ​= ​h11​h21​h31​​h12​h22​h32​​h13​h23​h33​​ ​ ​x1​x2​x3​​ ​(1)

或更简洁地表示为 x ′ = H x \mathbf{x}^{\prime}=\mathrm{Hx} x′=Hx. 注意：此方程中的矩阵 H 乘以任意一个非零比例因子不会使射影变换改变. 换句话说 H 是一个齐次矩阵，与点的齐次表示一样，有意义的仅仅是矩阵元素的比率。在 H 的九个元素中有八个独立比率,因此一个射影变换有八个自由度。 将x看成向量，则 x ′ 和 H x x'和Hx x′和Hx同一方向，则其叉乘 x ′ × H x = 0 x'\times Hx=0 x′×Hx=0. 如果将矩阵 H 的第 j j j 行记为 h j T \mathbf{h}^{j {T}} hjT(行向量), 那么由矩阵乘法法则可得：

H x i = ( h 1 T x i h 2 T x i h 3 T x i ) (2) \mathrm{H} \mathbf{x}_i=\left(\begin{array}{l} \mathbf{h}^{1 T} \mathbf{x}_i \\ \mathbf{h}^{2 T} \mathbf{x}_i \\ \mathbf{h}^{3 T} \mathbf{x}_i \end{array}\right) \tag{2} Hxi​= ​h1Txi​h2Txi​h3Txi​​ ​(2)

记 x i ′ = ( x i ′ , y i ′ , w i ′ ) T \mathbf{x}_i^{\prime}=\left(x_i^{\prime}, y_i^{\prime}, w_i^{\prime}\right)^{\mathrm{T}} xi′​=(xi′​,yi′​,wi′​)T, 则叉积定义可以显式地写成：

x i ′ × H x i = ( y i ′ h 3 T x i − w i ′ h 2 T x i w i ′ h 1 T x i − x i ′ h 3 T x i x i ′ h 2 T x i − y i ′ h 1 T x i ) . (3) \mathbf{x}_i^{\prime} \times \mathrm{H} \mathbf{x}_i=\left(\begin{array}{c} y_i^{\prime} \mathbf{h}^{3 T} \mathbf{x}_i-w_i^{\prime} \mathbf{h}^{2 T} \mathbf{x}_i \\ w_i^{\prime} \mathbf{h}^{1 T} \mathbf{x}_i-x_i^{\prime} \mathbf{h}^{3 T} \mathbf{x}_i \\ x_i^{\prime} \mathbf{h}^{2 T} \mathbf{x}_i-y_i^{\prime} \mathbf{h}^{1 T} \mathbf{x}_i \end{array}\right) .\tag{3} xi′​×Hxi​= ​yi′​h3Txi​−wi′​h2Txi​wi′​h1Txi​−xi′​h3Txi​xi′​h2Txi​−yi′​h1Txi​​ ​.(3)

因为对 j = 1 , 2 , 3 , h j T x i = x i T h j j=1,2,3, \mathbf{h}^{jT} \mathbf{x}_i=\mathbf{x}_i^{T} \mathbf{h}^j j=1,2,3,hjTxi​=xiT​hj 皆成立(其结果是一个常数，因此这样是成立的),这就给出关于 H 元素的三个方程,并可以写成下列形式

[ 0 T − w i ′ x i T y i ′ x i T w i ′ x i T 0 T − x i ′ x i T − y i ′ x i T x i ′ x i T 0 T ] [ h 1 h 2 h 3 ] = 0. (4) \left[\begin{array}{ccc} \mathbf{0}^{T} & -w_i^{\prime} \mathbf{x}_i^{T} & y_i^{\prime} \mathbf{x}_i^{T} \\ w_i^{\prime} \mathbf{x}_i^{T} & \mathbf{0}^{T} & -x_i^{\prime} \mathbf{x}_i^{T} \\ -y_i^{\prime} \mathbf{x}_i^{T} & x_i^{\prime} \mathbf{x}_i^{T} & \mathbf{0}^{T} \end{array}\right]\left[\begin{array}{l} \mathbf{h}^1 \\ \mathbf{h}^2 \\ \mathbf{h}^3 \end{array}\right]=\mathbf{0} .\tag{4} ​0Twi′​xiT​−yi′​xiT​​−wi′​xiT​0Txi′​xiT​​yi′​xiT​−xi′​xiT​0T​ ​ ​h1h2h3​ ​=0.(4)

这些方程都有 A i h = 0 A_i \mathbf{h}=\mathbf{0} Ai​h=0 的形式, 其中 A i A_i Ai​ 是 3 × 9 3 \times 9 3×9 的矩阵, h \mathbf{h} h 是由矩阵 H H H 的元素组成的 9 维矢量，由于上述方程组中第一个等式乘以 x i x_i xi​加上第二个等式乘以 y i y_i yi​,再除以 − w i -w_i −wi​可以得到第三个等式，因此上述方程组是线性相关的，其秩为2.因此舍弃第三个方程得到线性无关的方程组： [ 0 T − w i ′ x i T y i ′ x i T w i ′ x i T 0 T − x i ′ x i T ] [ h 1 h 2 h 3 ] = 0. (4) \left[\begin{array}{ccc} \mathbf{0}^{T} & -w_i^{\prime} \mathbf{x}_i^{T} & y_i^{\prime} \mathbf{x}_i^{T} \\ w_i^{\prime} \mathbf{x}_i^{T} & \mathbf{0}^{T} & -x_i^{\prime} \mathbf{x}_i^{T} \end{array}\right]\left[\begin{array}{l} \mathbf{h}^1 \\ \mathbf{h}^2 \\ \mathbf{h}^3 \end{array}\right]=\mathbf{0} .\tag{4} [0Twi′​xiT​​−wi′​xiT​0T​yi′​xiT​−xi′​xiT​​] ​h1h2h3​ ​=0.(4)

H = [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] , \quad \mathrm{H}=\left[\begin{array}{lll} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{array}\right], H= ​h1​h4​h7​​h2​h5​h8​​h3​h6​h9​​ ​, h = ( h 1 h 2 h 3 ) = ( h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ) \mathbf{h}=\left(\begin{array}{l} \mathbf{h}^1 \\ \mathbf{h}^2 \\ \mathbf{h}^3 \end{array}\right)=\left(\begin{array}{l} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \\ h_9 \end{array}\right) h= ​h1h2h3​ ​= ​h1​h2​h3​h4​h5​h6​h7​h8​h9​​ ​

其中 h i h_i hi​ 是 h \mathbf{h} h 的第 i i i 个元素. 将等式(5)展开得： [ 0 0 0 − w i ′ x i − w i ′ y i − w i ′ w i y i ′ x i y i ′ y i y i ′ w i w i ′ x i w i ′ y i w i ′ w i 0 0 0 − x i ′ x i − x i ′ y i − x i ′ w i ] [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] = 0. (6) \left[\begin{array}{ccc} 0&0&0 & -w_i^{\prime} {x}_i & -w_i^{\prime} {y}_i & -w_i^{\prime} {w}_i & y_i^{\prime} {x}_i & y_i^{\prime} {y}_i & y_i^{\prime} {w}_i \\ w_i^{\prime} {x}_i &w_i^{\prime} {y}_i &w_i^{\prime} {w}_i &0&0&0 &-x_i^{\prime}{x}_i &-x_i^{\prime}{y}_i &-x_i^{\prime}{w}_i \\ \end{array}\right] \left[\begin{array}{l} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \\ h_9 \end{array}\right]=\mathbf{0} .\tag{6} [0wi′​xi​​0wi′​yi​​0wi′​wi​​−wi′​xi​0​−wi′​yi​0​−wi′​wi​0​yi′​xi​−xi′​xi​​yi′​yi​−xi′​yi​​yi′​wi​−xi′​wi​​] ​h1​h2​h3​h4​h5​h6​h7​h8​h9​​ ​=0.(6) 即 A h = 0 A \mathbf{h}=\mathbf{0} Ah=0。每个匹配对提供两个方程，单应矩阵一共八个线性无关的元素，因此最少需要四个匹配对才能求解出单应矩阵。 齐次解(Ax=0): 观察等式(6)其实是一个线性方程组求解问题，当有4个匹配点对，即8个方程时，此时是适定方程组即未知量的秩等于系数矩阵的秩，此时可以利用线性方程组高斯消去法、求逆解法或者迭代解法求解单应矩阵元素(参考索尔的《数值分析》)。如果匹配点对超对4个，即线性矩阵秩大于未知量的秩，此时选择最小二乘法则求解齐次线性超定方程组，这里其实一个通用的解法，使用svd求解，系数矩阵 A A A的最小特征值对应的特征向量为单应矩阵元素。 非齐次解(Ax=b): 既然单应矩阵有8个线性无关的元素，我们不妨假设其中一个元素为常数，例如假设最后一个元素 h 9 = 1 h_9=1 h9​=1，等式(6)变为: [ 0 0 0 − w i ′ x i − w i ′ y i − w i ′ w i y i ′ x i y i ′ y i w i ′ x i w i ′ y i w i ′ w i 0 0 0 − x i ′ x i − x i ′ y i ] [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 ] = [ − y i ′ w i x i ′ w i ] . (7) \left[\begin{array}{ccc} 0&0&0 & -w_i^{\prime} {x}_i & -w_i^{\prime} {y}_i & -w_i^{\prime} {w}_i & y_i^{\prime} {x}_i & y_i^{\prime} {y}_i \\ w_i^{\prime} {x}_i &w_i^{\prime} {y}_i &w_i^{\prime} {w}_i &0&0&0 &-x_i^{\prime}{x}_i &-x_i^{\prime}{y}_i \\ \end{array}\right] \left[\begin{array}{l} h_1 \\ h_2 \\ h_3 \\ h_4 \\ h_5 \\ h_6 \\ h_7 \\ h_8 \end{array}\right]= \left[\begin{array}{l} -y_i^{\prime} {w}_i\\ x_i^{\prime}{w}_i \end{array}\right] .\tag{7} [0wi′​xi​​0wi′​yi​​0wi′​wi​​−wi′​xi​0​−wi′​yi​0​−wi′​wi​0​yi′​xi​−xi′​xi​​yi′​yi​−xi′​yi​​] ​h1​h2​h3​h4​h5​h6​h7​h8​​ ​=[−yi′​wi​xi′​wi​​].(7) 这里就是slam14讲公式(7.20)的方程组。使用最小二乘求解非齐次方程组即可。使用非齐次形式存在一个问题，即假设的元素如果为0等式7是不成立的，因此这种方法存在一个缺陷，不推荐这种计算方法。 将上述求解单应矩阵的过程称为DLT(direct linear transform).

4提高精度和稳健性的做法 4.1归一化

归一化的目的主要是降低数据噪声的影响，提高单应矩阵的精度，此外归一化具有相似不变性，即经过归一化求得的单应矩阵与原数据求解的结果是完全等价的，可以看一下我自己的理解. 这里有两种方式： ORB slam方法: μ = 1 N ∑ 1 N x σ = ∑ 1 N ∣ x − μ ∣ N x norm  = x − μ σ \begin{aligned} & \mu=\frac{1}{N} \sum_1^N x \\ & \sigma=\sum_1^N \frac{|x-\mu|}{N} \\ & x_{\text {norm }}=\frac{x-\mu}{\sigma} \end{aligned} ​μ=N1​1∑N​xσ=1∑N​N∣x−μ∣​xnorm ​=σx−μ​​ 这里不清楚是否具有相似不变性(未经过证明)。 MVG方法： 归一化 x x x: 计算一个只包括位移和缩放的相似变换 T T T,将点 x x x变到新的点集 x x x,使得点 x x x的形心位于原点 ( 0 , 0 ) (0,0) (0,0),并且它们到原点的平均距离是 2 \sqrt{2} 2 ​.

4.2RanSAC

将ransac应用于单应矩阵求解可以降低噪点和错误点对对单应矩阵的影响。这里不再介绍ransac方法，具体参考维基百科，在MVG中如下：

5完整的计算过程

将第2节中的方法与第1节的理论结合，得到完整的单应矩阵计算方法：

特别地说明：针对ransac的结果，再利用LM算法最小化双向重投影误差，这里好像有点问题：即LM一般是用来求解非线性最小二乘问题的。重投影误差是最小二乘问题，而双向重投影误差并不是一个最小二乘问题，因此是无法用LM最小化双向重投影误差的，我翻阅了opencv利用LM求解单应矩阵的代码，发现opencv使用的也仅是重投影误差(opencv\calib3d\src\fundam.cpp)。

6 codes by c plus plus

三个函数构成

// #include <iostream> #include<random> #include<opencv2/calib3d.hpp> #include<opencv2/core.hpp> #include<opencv2/stitching.hpp> #include<opencv2/features2d.hpp> #include<opencv2/highgui.hpp> #include <opencv2/flann.hpp> #include <opencv2/imgproc.hpp> #include <opencv2/highgui.hpp> #include <opencv2/features2d/features2d.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/opencv.hpp> #include<Eigen/Core> #include<Eigen/SVD> #include<Eigen/QR> #include<Eigen/Dense> //归一化操作 void normalizePoints2(const std::vector<Eigen::Vector3d>& origin_pts, std::vector<Eigen::Vector3d>& normalized_pts, Eigen::Matrix3d &T) { if (origin_pts.empty()) { return; } //std::cout << "归一化\n"; //如果包含无穷远点怎么处理 //计算形心(质心) Eigen::Vector3d center_pt; center_pt << 0.0, 0.0, 0.0; double sum_x = 0.0, sum_y = 0.0; for (size_t i = 0; i < origin_pts.size(); i++) { sum_x += origin_pts[i](0) ; sum_y += origin_pts[i](1) ; } center_pt(0) = sum_x / double(origin_pts.size()); center_pt(1) = sum_y / double(origin_pts.size()); center_pt(2) = 1.0; double scale = 0.0,value=0.0; for (size_t i = 0; i < origin_pts.size(); i++) { value += std::sqrt(std::pow(origin_pts[i](0) - center_pt(0), 2.0) + std::pow(origin_pts[i](1) - center_pt(1), 2.0)); } value /= double(origin_pts.size()); scale = sqrt(2.0) / value; double tx = -1.0 * scale * center_pt(0); double ty = -1.0 * scale * center_pt(1); //相似变换矩阵 T << scale, 0.0, tx, 0.0, scale, ty, 0.0, 0.0, 1.0; normalized_pts.resize(origin_pts.size()); for (size_t i = 0; i < origin_pts.size(); i++) { normalized_pts[i] = T * origin_pts[i];//T.inverse() } //std::cout << T << std::endl; //验证 double mean_x = 0.0, mean_y = 0.0, dist = 0.0; for (size_t i = 0; i < normalized_pts.size(); i++) { mean_x += normalized_pts[i].x(); mean_y += normalized_pts[i].y(); dist += std::pow((normalized_pts[i].x() * normalized_pts[i].x() + normalized_pts[i].y() * normalized_pts[i].y()), 0.5); } mean_x /= double(normalized_pts.size()); mean_y /= double(normalized_pts.size()); dist /= double(normalized_pts.size()); // std::cout << "归一化结果：" << mean_x << " " << mean_y << " " << dist << std::endl; } //利用svd直接求解H矩阵 void svdComputeH(const std::vector<Eigen::Vector3d>&src_pts,const std::vector<Eigen::Vector3d>&trg_pts, Eigen::Matrix3d&result) { const int rows_num = 2 * src_pts.size(); Eigen::MatrixXd A=Eigen::MatrixXd::Zero(rows_num,9); //std::cout << "A0" << A << std::endl; for (size_t i = 0; i < src_pts.size(); i++) { //i行 A(2*i, 0) = 0.0; A(2 * i, 1) = 0.0; A(2 * i, 2) = 0.0; A(2 * i, 3) = -1.0*src_pts[i].x(); A(2 * i, 4) = -1.0 * src_pts[i].y();A(2 * i, 5) = -1.0 ; A(2 * i, 6) =trg_pts[i].y()* src_pts[i].x(); A(2 * i, 7) = trg_pts[i].y() * src_pts[i].y(); A(2 * i, 8) = trg_pts[i].y() ; //i+1行 A(2 * i + 1, 0) = src_pts[i].x();A(2 * i + 1, 1) = src_pts[i].y();A(2 * i + 1, 2) = 1.0; A(2 * i + 1, 3) = 0.0; A(2 * i + 1, 4) = 0.0; A(2 * i + 1, 5) = 0.0; A(2 * i + 1, 6) =-1.0* trg_pts[i].x() * src_pts[i].x(); A(2 * i + 1, 7) = -1.0 * trg_pts[i].x()*src_pts[i].y(); A(2 * i + 1, 8) = -1.0 * trg_pts[i].x(); } // std::cout << "A" << A << std::endl; Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::MatrixXd V = svd.matrixV(); Eigen::MatrixXd D = svd.singularValues(); //寻找最小奇异值对应的特征向量 double max_num = double(INT32_MAX); Eigen::MatrixXd H; //for (size_t i = 0; i < D.rows(); i++) //{ // if (D(i, 0) < max_num) // { // max_num = D(i, 0); // H = V.col(i); // } //} H = V.col(8); /*std::cout << "D" << D << std::endl; std::cout << "v" << V << std::endl; std::cout << H << std::endl;*/ result << H(0,0), H(1, 0), H(2, 0), H(3, 0), H(4, 0), H(5, 0), H(6, 0), H(7, 0), H(8, 0) ; //std::cout << result << std::endl; } //利用ransac求解H矩阵 void ransacComputeH(std::vector<Eigen::Vector3d>& src_pts, std::vector<Eigen::Vector3d>& trg_pts, Eigen::Matrix3d& result) { int point_size = src_pts.size(); double dist_t = 10;//像素 int max_inlier_num_T = 0.8* point_size; int max_iter_num = 30; int iter_count = 0; //每次迭代的结果对应的索引 std::vector<std::vector<int> >inlier_x_indices{}; std::vector<std::vector<int> >inlier_y_indices{}; std::vector<int> inlier_indices{}; srand(time(NULL)); while(true) { //inlier_indices.clear(); ++iter_count; //随机抽选一定数量点，理论上四个点就可以 std::vector<int>random_indices; std::vector< Eigen::Vector3d> random_x, random_y; for (size_t i = 0; i <8; i++) { int random_num = rand() % point_size;//随机选择一个点 // std::cout << "random_num" << random_num << std::endl; random_indices.push_back(random_num); random_x.push_back(src_pts[random_num]); random_y.push_back(trg_pts[random_num]); } //归一化 std::vector< Eigen::Vector3d> normalized_random_x, normalized_random_y; Eigen::Matrix3d Tx, Ty; normalizePoints2(random_x, normalized_random_x, Tx); normalizePoints2(random_y, normalized_random_y, Ty); Eigen::Matrix3d normalized_random_result; svdComputeH(normalized_random_x, normalized_random_y, normalized_random_result); //逆变换为原始的H Eigen::Matrix3d random_result=Ty.inverse()*normalized_random_result*Tx; std::cout << iter_count<<" " << random_result<< std::endl; //计算残差（距离） int inlier_num = 0; std::vector<int> inlier_indices_tmp{}; for (size_t i = 0; i < point_size; i++) { Eigen::Vector3d src_x= src_pts[i]; Eigen::Vector3d computed_trg_pt = random_result * src_x;//利用计算结果得到的理论值 double computed_trg_pt_x = computed_trg_pt(0) / computed_trg_pt(2); double computed_trg_pt_y = computed_trg_pt(1) / computed_trg_pt(2); double computed_dist = std::pow(std::pow(computed_trg_pt_x - trg_pts[i](0), 2) + std::pow(computed_trg_pt_y - trg_pts[i](1), 2), 0.5); //inlier if (computed_dist<dist_t) { ++inlier_num; inlier_indices_tmp.push_back(i); } } //寻找最大数量的内点集合（最大一致集） if (inlier_indices.size()<inlier_indices_tmp.size()) { inlier_indices = inlier_indices_tmp; } if (iter_count >= max_iter_num || inlier_num > max_inlier_num_T) { break; } } //选择最大内点集重新估计参数 std::vector< Eigen::Vector3d> inlier_x, inlier_y; for (size_t i = 0; i < inlier_indices.size(); i++) { inlier_x.push_back(src_pts[inlier_indices[i]]); inlier_y.push_back(trg_pts[inlier_indices[i]]); } std::vector< Eigen::Vector3d> normalized_inlier_x, normalized_inlier_y; Eigen::Matrix3d Tx, Ty, normalized_inlier_result; normalizePoints2(inlier_x, normalized_inlier_x, Tx); normalizePoints2(inlier_y, normalized_inlier_y, Ty); svdComputeH(normalized_inlier_x, normalized_inlier_y, normalized_inlier_result); result = Ty.inverse() * normalized_inlier_result * Tx; } //图像拼接验证H矩阵的正确性 bool featureDetecteAndMatch2(cv::Mat& img1, cv::Mat& img2,int H_method=0) { using namespace cv; using namespace std; Mat g1(img1, Rect(0, 0, img1.cols, img1.rows)); // init roi Mat g2(img2, Rect(0, 0, img2.cols, img2.rows)); cvtColor(g1, g1, COLOR_BGR2GRAY); cvtColor(g2, g2, COLOR_BGR2GRAY); vector<cv::KeyPoint> keypoints_roi, keypoints_img; /* keypoints found using SIFT */ Mat descriptor_roi, descriptor_img; /* Descriptors for SIFT */ vector<cv::DMatch> matches, good_matches; //cv::Ptr<cv::SIFT> sift = cv::SIFT::create(); //Ptr<cv::ORB>orb_feature = ORB::create(1000); //Ptr<cv::BRISK> brisk_feature = BRISK::create(1000); Ptr<cv::AKAZE> akaze_feature = AKAZE::create(); int i, dist = 80; akaze_feature->detectAndCompute(g1, cv::Mat(), keypoints_roi, descriptor_roi); /* get keypoints of ROI image */ akaze_feature->detectAndCompute(g2, cv::Mat(), keypoints_img, descriptor_img); /* get keypoints of the image */ //FlannBasedMatcher matcher; /* FLANN based matcher to match keypoints */ //matcher.match(descriptor_roi, descriptor_img, matches); //实现描述符之间的匹配 Ptr<DescriptorMatcher> matche = DescriptorMatcher::create("BruteForce-Hamming"); matche->match(descriptor_roi, descriptor_img, matches); double max_dist = 0; double min_dist = 5000; //-- Quick calculation of max and min distances between keypoints for (int i = 0; i < descriptor_roi.rows; i++) { double dist = matches[i].distance; if (dist < min_dist) min_dist = dist; if (dist > max_dist) max_dist = dist; } // 特征点筛选 for (i = 0; i < descriptor_roi.rows; i++) { if (matches[i].distance < 5 * min_dist) { good_matches.push_back(matches[i]); } } printf("%ld no. of matched keypoints in right image\n", good_matches.size()); /* Draw matched keypoints */ Mat img_matches; //绘制匹配 drawMatches(img1, keypoints_roi, img2, keypoints_img, good_matches, img_matches, Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); //cv::namedWindow("matches_image", cv::WINDOW_NORMAL); imshow("matches_image", img_matches); //waitKey(0); vector<Point2f> keypoints1, keypoints2; for (i = 0; i < good_matches.size(); i++) { keypoints1.push_back(keypoints_img[good_matches[i].trainIdx].pt); keypoints2.push_back(keypoints_roi[good_matches[i].queryIdx].pt); } //计算单应矩阵(射影变换矩阵) Mat H=cv::Mat_<double>(3,3); Mat H2=cv::Mat_<double>(3, 3); //利用opencv自带的计算单应 if (H_method==0) { H= findHomography(keypoints1, keypoints2, RANSAC); H2= findHomography(keypoints2, keypoints1, RANSAC); } //利用自己写的计算单应 else if (H_method == 1) { std::vector<Eigen::Vector3d> src_pts, trg_pts; src_pts.resize(keypoints1.size()); trg_pts.resize(keypoints2.size()); for (size_t i = 0; i < keypoints1.size(); i++) { src_pts[i](0) = keypoints1[i].x; src_pts[i](1) = keypoints1[i].y; src_pts[i](2) = 1.0; trg_pts[i](0) = keypoints2[i].x; trg_pts[i](1) = keypoints2[i].y; trg_pts[i](2) = 1.0; } Eigen::Matrix3d H_result, H_result2; ransacComputeH(src_pts, trg_pts, H_result); ransacComputeH(trg_pts ,src_pts, H_result2); for (size_t i = 0; i < 3; ++i) { for(size_t j = 0; j < 3; ++j) { H.at<double>(i, j) = H_result(i, j)*(1.0/ H_result(2, 2)); //*(1.0 / H_result(2, 2)):把最后一个元素变为1，和opencv保持一致 H2.at<double>(i, j) = H_result2(i, j) * (1.0 / H_result2(2, 2)); } } } std::cout << "H:\n" << H << std::endl; Mat stitchedImage; //定义射影变换后的图像(也是拼接结果图像) Mat stitchedImage2; //定义射影变换后的图像(也是拼接结果图像) int mRows = img2.rows; if (img1.rows > img2.rows) { mRows = img1.rows; } int count = 0; for (int i = 0; i < keypoints2.size(); i++) { if (keypoints2[i].x >= img2.cols / 2) count++; } //判断匹配点位置来决定图片是左还是右 if (count / float(keypoints2.size()) >= 0.5) //待拼接img2图像在右边 { cout << "img1 should be left" << endl; vector<Point2f>corners(4); vector<Point2f>corners2(4); corners[0] = Point(0, 0); corners[1] = Point(0, img2.rows); corners[2] = Point(img2.cols, img2.rows); corners[3] = Point(img2.cols, 0); stitchedImage = Mat::zeros(img2.cols + img1.cols, mRows, CV_8UC3); warpPerspective(img2, stitchedImage, H, Size(img2.cols + img1.cols, mRows)); perspectiveTransform(corners, corners2, H); /* circle(stitchedImage, corners2[0], 5, Scalar(0, 255, 0), 2, 8); circle(stitchedImage, corners2[1], 5, Scalar(0, 255, 255), 2, 8); circle(stitchedImage, corners2[2], 5, Scalar(0, 255, 0), 2, 8); circle(stitchedImage, corners2[3], 5, Scalar(0, 255, 0), 2, 8); */ cout << corners2[0].x << ", " << corners2[0].y << endl; cout << corners2[1].x << ", " << corners2[1].y << endl; imshow("temp", stitchedImage); //imwrite("temp.jpg", stitchedImage); Mat half(stitchedImage, Rect(0, 0, img1.cols, img1.rows)); img1.copyTo(half); imshow("result", stitchedImage); } else //待拼接图像img2在左边 { cout << "img2 should be left" << endl; stitchedImage = Mat::zeros(img2.cols + img1.cols, mRows, CV_8UC3); warpPerspective(img1, stitchedImage, H2, Size(img1.cols + img2.cols, mRows)); imshow("temp", stitchedImage); //计算射影变换后的四个端点 vector<Point2f>corners(4); vector<Point2f>corners2(4); corners[0] = Point(0, 0); corners[1] = Point(0, img1.rows); corners[2] = Point(img1.cols, img1.rows); corners[3] = Point(img1.cols, 0); perspectiveTransform(corners, corners2, H2); //射影变换对应端点 /* circle(stitchedImage, corners2[0], 5, Scalar(0, 255, 0), 2, 8); circle(stitchedImage, corners2[1], 5, Scalar(0, 255, 255), 2, 8); circle(stitchedImage, corners2[2], 5, Scalar(0, 255, 0), 2, 8); circle(stitchedImage, corners2[3], 5, Scalar(0, 255, 0), 2, 8); */ cout << corners2[0].x << ", " << corners2[0].y << endl; cout << corners2[1].x << ", " << corners2[1].y << endl; Mat half(stitchedImage, Rect(0, 0, img2.cols, img2.rows)); img2.copyTo(half); imshow("result", stitchedImage); } imwrite("result2.bmp", stitchedImage); return true; } #include<fstream> int main() { //1 比较利用上述代码计算的H矩阵与opencv计算的H矩阵差异 //读取文件 std::vector<cv::Point2d>src_cv_pts, trg_cv_pts; std::vector<Eigen::Vector3d>src_points; std::ifstream fin1("./pts1.txt"); while (true) { if (fin1.eof()) { break; } Eigen::Vector3d pt; double x, y; fin1 >> x >> y; pt(0) = x; pt(1) = y; pt(2) = 1.0; src_points.push_back(pt); cv::Point2d pp; pp.x = x; pp.y = y; src_cv_pts.push_back(pp); } std::vector<Eigen::Vector3d>trg_points, normalized_points; std::ifstream fin2("./pts2.txt"); while (true) { if (fin2.eof()) { break; } Eigen::Vector3d pt; double x, y; fin2 >> x >> y; pt(0) = x; pt(1) = y; pt(2) = 1.0; trg_points.push_back(pt); cv::Point2d pp; pp.x = x; pp.y = y; trg_cv_pts.push_back(pp); } std::cout << "point size:" << src_points.size() << " " << trg_points.size() << std::endl; Eigen::Matrix3d H0,H00; svdComputeH(src_points, trg_points, H0); double scale0 = 1.0 / H0(2, 2); for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { H00(i, j) = H0(i, j) * scale0; } } std::cout << "h0:\n" << H0 << std::endl; std::cout << "h00:\n" << H00 << std::endl; Eigen::Matrix3d HH; ransacComputeH(src_points, trg_points, HH); std::cout << "result:\n" << HH << std::endl; Eigen::Matrix3d hhh; double scale = 1.0 / HH(2, 2); for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { hhh(i, j) = HH(i, j) * scale; } } std::cout << "result2:\n" << hhh << std::endl; cv::Mat opencv_H = cv::findHomography(src_cv_pts, trg_cv_pts, cv::RANSAC); std::cout << "opencv result:" << opencv_H << std::endl; //2利用图像拼接检查H矩阵计算的效果 std::cout << "image stitching\n:"; // cv::Mat img1 = cv::imread("7.jpg"); cv::Mat img2 = cv::imread("8.jpg"); featureDetecteAndMatch2(img1, img2,1); std::cout << "Hello World!\n"; }

原始图像：

图像拼接效果

参考

1 2 3 [4] slam14讲-单应矩阵 [5] 计算机视觉中多视图几何