多视图几何--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] TC1W=[R1∣t1]，相机 C 2 C_2 C2 的外参为 T C 2 W = [ R 2 ∣ t 2 ] T_{C_2 W} = [R_2 | \boldsymbol{t}_2] TC2W=[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=TC1W−1=[R10Tt11]−1=[R1−10T−R1−1t11](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} TC2C1=TC2WTWC1=[R20Tt21][R1−10T−R1−1t11]=[R2R1−10T−R2R1−1t1+t21]=[R210Tt211](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=ZC11K1P1p2=ZC21K2P2(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=ZC1K1−1p1(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=R21P1+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=ZC21K2(R21ZC1K1−1p1+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=−dt21nTP1=2−8−dt21nTZC1K1−1p1(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=ZC21K2(R21−dt21nT)ZC1K1−1p1=H21p1(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=ZC2ZC1K2(R21−dt21nT)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=ZC2ZC1K2(R2R1−1−d(−R2R1−1t1+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′ = h11h21h31h12h22h32h13h23h33 x1x2x3 (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= h1Txih2Txih3Txi (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′h2Txiwi′h1Txi−xi′h3Txixi′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=xiThj 皆成立(其结果是一个常数，因此这样是成立的),这就给出关于 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′xiT0Txi′xiTyi′xiT−xi′xiT0T h1h2h3 =0.(4)
这些方程都有
A
i
h
=
0
A_i \mathbf{h}=\mathbf{0}
Aih=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′xiT0Tyi′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=
h1h4h7h2h5h8h3h6h9
,
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
=
h1h2h3h4h5h6h7h8h9
其中
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′xi0wi′yi0wi′wi−wi′xi0−wi′yi0−wi′wi0yi′xi−xi′xiyi′yi−xi′yiyi′wi−xi′wi]
h1h2h3h4h5h6h7h8h9
=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′xi0wi′yi0wi′wi−wi′xi0−wi′yi0−wi′wi0yi′xi−xi′xiyi′yi−xi′yi]
h1h2h3h4h5h6h7h8
=[−yi′wixi′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}
μ=N11∑Nxσ=1∑NN∣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] 计算机视觉中多视图几何