-
Notifications
You must be signed in to change notification settings - Fork 6
/
homography.cpp
126 lines (97 loc) · 3.61 KB
/
homography.cpp
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
/*
Copyright (c) 2020, Tharaka Ratnayake, email: tharakasachintha.17@cse.mrt.ac.lk
All rights reserved. https://github.com/tharaka27/ImageStitcherFAST
Some algorithms used in this code referred to:
1. OpenCV: http://opencv.org/
Revision history:
March 30th, 2020: initial version.
*/
#include <math.h>
#include <iostream>
#include "homography.h"
#include "opencv2/core/core.hpp"
// finding the solutions using gaussian elimination method
// ported to cpp from pseudocode in
// http://en.wikipedia.org/wiki/Gaussian_elimination
// input: input pointer to the array
// n : length of the array
//
//**********************************************************************
void homography::gaussian_elimination(double* input, int n) {
double* A = input;
int i = 0;
int j = 0;
int m = n - 1;
while (i < m && j < n) {
// Find pivot in column j, starting in row i:
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) {
//swap rows i and maxi, but do not change the value of i
if (i != maxi)
for (int k = 0; k < n; k++) {
double aux = A[i * n + k];
A[i * n + k] = A[maxi * n + k];
A[maxi * n + k] = aux;
}
//Now A[i,j] will contain the old value of A[maxi,j].
//divide each entry in row i by A[i,j]
double A_ij = A[i * n + j];
for (int k = 0; k < n; k++) {
A[i * n + k] /= A_ij;
}
//Now A[i,j] will have the value 1.
for (int u = i + 1; u < m; u++) {
//subtract A[u,j] * row i from row u
double A_uj = A[u * n + j];
for (int k = 0; k < n; k++) {
A[u * n + k] -= A_uj * A[i * n + k];
}
//Now A[u,j] will be 0, since A[u,j] - A[i,j] * A[u,j] = A[u,j] - 1 * A[u,j] = 0.
}
i++;
}
j++;
}
//back substitution
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];
//A[i*n+j]=0;
}
}
}
//**********************************************************************
// finding the homography using 4 given points
// [ X ] [ h11 , h12, h13] [ x ]
// [ Y ] = [ h21 , h22, h23] [ y ]
// [ 1 ] [ h31 , h32, 1] [ 1 ]
//
// Computer hij values which satisfy above equation
//
// src cv::Point2d 4 source points
// dst cv::Point2d 4 destination points
//
//
// NOTE: Currently gaussian elimination is used. New mechanism
// will be required for optimized algorithm
//**********************************************************************
cv::Mat homography::findHomography_(cv::Point2d src[4], cv::Point2d dst[4]) {
double 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
};
homography::gaussian_elimination(&P[0][0], 9);
cv::Mat A = (cv::Mat_<double>(3, 3) << P[0][8], P[1][8], P[2][8], P[3][8], P[4][8], P[5][8], P[6][8], P[7][8], 1);
return A;
}