1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
| //
| // Created by Scheaven on 2020/5/22.
| //
|
| #include "homography_util.h"
|
| static void gaussian_elimination(float *input, int n)
| {
| float * A = input;
| int i = 0;
| int j = 0;
| //m = 8 rows, n = 9 cols
| int m = n-1;
| while (i < m && j < n)
| {
| int maxi = i;
| for(int k = i+1; k < m; k++)
| {
| if(fabs(A[k * n + j]) > fabs(A[maxi * n + j]))
| {
| maxi = k;
| }
| }
| if (A[maxi * n + j] != 0)
| {
| if(i != maxi)
| for(int k = 0; k < n; k++)
| {
| float aux = A[i * n + k];
| A[i * n + k] = A[maxi * n + k];
| A[maxi * n + k] = aux;
| }
| float A_ij = A[i * n + j];
| for(int k = 0; k < n; k++)
| {
| A[i * n + k] /= A_ij;
| }
| for(int u = i+1; u< m; u++)
| {
| float A_uj = A[u * n + j];
| for(int k = 0; k <n; k++)
| {
| A[u * n + k] -= A_uj * A[i * n + k];
| }
| }
| i++;
| }
| j++;
| }
|
| for(int i = m-2; i >= 0; i--)
| {
| for(int j = i+1; j < n-1; j++)
| {
| A[i * n + m] -= A[i * n + j] * A[j * n + m];
| }
| }
|
| }
|
| // 创建单应性矩阵
| void creatHomography(vector<SPoint2f> src, vector<SPoint2f> dst,SHomogtaphy *SH_pointer,int j_index)
| {
| float P[8][9]=
| {
| {-src[0].x, -src[0].y, -1, 0, 0, 0, src[0].x * dst[0].x, src[0].y * dst[0].x, -dst[0].x }, // h11
| { 0, 0, 0, -src[0].x, -src[0].y, -1, src[0].x * dst[0].y, src[0].y * dst[0].y, -dst[0].y }, // h12
|
| {-src[1].x, -src[1].y, -1, 0, 0, 0, src[1].x * dst[1].x, src[1].y * dst[1].x, -dst[1].x }, // h13
| { 0, 0, 0, -src[1].x, -src[1].y, -1, src[1].x * dst[1].y, src[1].y * dst[1].y, -dst[1].y }, // h21
|
| {-src[2].x, -src[2].y, -1, 0, 0, 0, src[2].x * dst[2].x, src[2].y * dst[2].x, -dst[2].x }, // h22
| { 0, 0, 0, -src[2].x, -src[2].y, -1, src[2].x * dst[2].y, src[2].y * dst[2].y, -dst[2].y }, // h23
|
| {-src[3].x, -src[3].y, -1, 0, 0, 0, src[3].x * dst[3].x, src[3].y * dst[3].x, -dst[3].x }, // h31
| { 0, 0, 0, -src[3].x, -src[3].y, -1, src[3].x * dst[3].y, src[3].y * dst[3].y, -dst[3].y }, // h32
| };
|
| gaussian_elimination(&P[0][0], 9);
| float aux_H[]={ P[0][8], P[1][8], P[2][8], // h11 h21 0 h31
| P[3][8], P[4][8], P[5][8], // h12 h22 0 h32
| P[6][8], P[7][8], 1}; // h13 h23 0 h33
|
| for(int i = 0; i < 3; i++)
| {
| for(int j = 0; j < 3; j++)
| {
| // homography[i * 3 + j] = aux_H[i * 3 + j];
| SH_pointer->matrix[j_index].homography[i][j] = aux_H[i * 3 + j];
| }
| }
| }
|
|
