-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathransac.cpp
222 lines (179 loc) · 7.94 KB
/
ransac.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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
#include "ransac.h"
ransac::ransac ()
{
}
Mat<double> ransac::ransac_metoda ( std::vector<pary> zbior_punktow_kluczowych, int ile_krokow, double max_error, int jaka_transformacja )
{
std::cout << "ransac in progres "<<jaka_transformacja<< "\n";
Mat<double> best_model;
double best_score = 0;
for ( int i = 0; i < ile_krokow; i++ )
{
//std::cout << i;
Mat <double> model;
do
{
if ( jaka_transformacja == 0 )
{
std::vector<pary> wybrane_ptk = sasiad.kilka_losowych_punktow ( zbior_punktow_kluczowych, 3);
model = oblicz_model_afiniczny ( wybrane_ptk );
}
else
{
std::vector<pary> wybrane_ptk = sasiad.kilka_losowych_punktow ( zbior_punktow_kluczowych, 4 );
model = oblicz_model_perspektywistyczny ( wybrane_ptk );
}
} while ( model.empty () );
double wynik = 0;
for ( auto &element : zbior_punktow_kluczowych )
{
double blad = model_blad ( model, element );
if ( blad < max_error )
wynik++;
}
if ( wynik > best_score )
{
best_score = wynik;
best_model = model;
std::cout << "model zmiana\n";
}
}
return best_model;
}
double ransac::model_blad ( Mat<double> model, pary para )
{
double wynik = para.pierwszy.transformata ( model ).odleglosc ( para.drugi );
return wynik;
}
Mat<double> ransac::oblicz_model_afiniczny ( std::vector<pary> zbior_punktow_kluczowych )
{
Mat <double> maciez ( 6, 6, fill::zeros );
maciez ( 0, 0 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_x;
maciez ( 0, 1 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_y;
maciez ( 0, 2 ) = 1;
maciez ( 1, 0 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_x;
maciez ( 1, 1 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_y;
maciez ( 1, 2 ) = 1;
maciez ( 2, 0 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_x;
maciez ( 2, 1 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_y;
maciez ( 2, 2 ) = 1;
maciez ( 3, 3 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_x;
maciez ( 3, 4 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_y;
maciez ( 3, 5 ) = 1;
maciez ( 4, 3 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_x;
maciez ( 4, 4 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_y;
maciez ( 4, 5 ) = 1;
maciez ( 5, 3 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_x;
maciez ( 5, 4 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_y;
maciez ( 5, 5 ) = 1;
Mat <double> maciez_uv ( 6, 1 );
maciez_uv ( 0, 0 ) = zbior_punktow_kluczowych[0].drugi.wspolrzedna_x;
maciez_uv ( 1, 0 ) = zbior_punktow_kluczowych[1].drugi.wspolrzedna_x;
maciez_uv ( 2, 0 ) = zbior_punktow_kluczowych[2].drugi.wspolrzedna_x;
maciez_uv ( 3, 0 ) = zbior_punktow_kluczowych[0].drugi.wspolrzedna_y;
maciez_uv ( 4, 0 ) = zbior_punktow_kluczowych[1].drugi.wspolrzedna_y;
maciez_uv ( 5, 0 ) = zbior_punktow_kluczowych[2].drugi.wspolrzedna_y;
Mat <double> wynik;
Mat <double> mac_I ( 6, 6, fill::eye );
try
{
Mat <double> inversed = solve ( maciez, mac_I );
wynik = inversed * maciez_uv;
}
catch ( const std::exception& e )
{
std::cout << "\n";
std::cout << e.what ();
}
Mat <double> wynik2 ( 3, 3, fill::zeros );
wynik2 ( 0, 0 ) = wynik ( 0, 0 );
wynik2 ( 0, 1 ) = wynik ( 1, 0 );
wynik2 ( 0, 2 ) = wynik ( 2, 0 );
wynik2 ( 1, 0 ) = wynik ( 3, 0 );
wynik2 ( 1, 1 ) = wynik ( 4, 0 );
wynik2 ( 1, 2 ) = wynik ( 5, 0 );
wynik2 ( 2, 2 ) = 1;
return wynik2;
}
Mat<double> ransac::oblicz_model_perspektywistyczny ( std::vector<pary> zbior_punktow_kluczowych )
{
Mat <double> maciez ( 8, 8, fill::zeros );
maciez ( 0, 0 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_x;
maciez ( 0, 1 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_y;
maciez ( 0, 2 ) = 1;
maciez ( 1, 0 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_x;
maciez ( 1, 1 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_y;
maciez ( 1, 2 ) = 1;
maciez ( 2, 0 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_x;
maciez ( 2, 1 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_y;
maciez ( 2, 2 ) = 1;
maciez ( 3, 0 ) = zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_x;
maciez ( 3, 1 ) = zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_y;
maciez ( 3, 2 ) = 1;
maciez ( 4, 3 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_x;
maciez ( 4, 4 ) = zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_y;
maciez ( 4, 5 ) = 1;
maciez ( 5, 3 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_x;
maciez ( 5, 4 ) = zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_y;
maciez ( 5, 5 ) = 1;
maciez ( 6, 3 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_x;
maciez ( 6, 4 ) = zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_y;
maciez ( 6, 5 ) = 1;
maciez ( 7, 3 ) = zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_x;
maciez ( 7, 4 ) = zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_y;
maciez ( 7, 5 ) = 1;
maciez ( 0, 6 ) = -zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[0].drugi.wspolrzedna_x;
maciez ( 0, 7 ) = -zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[0].drugi.wspolrzedna_x;
maciez ( 1, 6 ) = -zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[1].drugi.wspolrzedna_x;
maciez ( 1, 7 ) = -zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[1].drugi.wspolrzedna_x;
maciez ( 2, 6 ) = -zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[2].drugi.wspolrzedna_x;
maciez ( 2, 7 ) = -zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[2].drugi.wspolrzedna_x;
maciez ( 3, 6 ) = -zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[3].drugi.wspolrzedna_x;
maciez ( 3, 7 ) = -zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[3].drugi.wspolrzedna_x;
maciez ( 4, 6 ) = -zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[0].drugi.wspolrzedna_y;
maciez ( 4, 7 ) = -zbior_punktow_kluczowych[0].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[0].drugi.wspolrzedna_y;
maciez ( 5, 6 ) = -zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[1].drugi.wspolrzedna_y;
maciez ( 5, 7 ) = -zbior_punktow_kluczowych[1].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[1].drugi.wspolrzedna_y;
maciez ( 6, 6 ) = -zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[2].drugi.wspolrzedna_y;
maciez ( 6, 7 ) = -zbior_punktow_kluczowych[2].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[2].drugi.wspolrzedna_y;
maciez ( 7, 6 ) = -zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_x*zbior_punktow_kluczowych[3].drugi.wspolrzedna_y;
maciez ( 7, 7 ) = -zbior_punktow_kluczowych[3].pierwszy.wspolrzedna_y*zbior_punktow_kluczowych[3].drugi.wspolrzedna_y;
Mat <double> maciez_uv ( 8, 1 );
maciez_uv ( 0, 0 ) = zbior_punktow_kluczowych[0].drugi.wspolrzedna_x;
maciez_uv ( 1, 0 ) = zbior_punktow_kluczowych[1].drugi.wspolrzedna_x;
maciez_uv ( 2, 0 ) = zbior_punktow_kluczowych[2].drugi.wspolrzedna_x;
maciez_uv ( 3, 0 ) = zbior_punktow_kluczowych[3].drugi.wspolrzedna_x;
maciez_uv ( 4, 0 ) = zbior_punktow_kluczowych[0].drugi.wspolrzedna_y;
maciez_uv ( 5, 0 ) = zbior_punktow_kluczowych[1].drugi.wspolrzedna_y;
maciez_uv ( 6, 0 ) = zbior_punktow_kluczowych[2].drugi.wspolrzedna_y;
maciez_uv ( 7, 0 ) = zbior_punktow_kluczowych[3].drugi.wspolrzedna_y;
// std::cout << maciez << "\n";
//std::cout << det ( maciez );
//std::getchar ();
Mat <double> wynik(8,1, fill::zeros);
Mat <double> mac_I ( 8, 8, fill::eye );
try
{
Mat <double> inversed = solve ( maciez, mac_I );
wynik = inversed * maciez_uv;
}
catch ( const std::exception& e )
{
std::cout << "\n";
std::cout << e.what ();
}
Mat <double> wynik2 ( 3, 3, fill::zeros );
wynik2 ( 0, 0 ) = wynik ( 0, 0 );
wynik2 ( 0, 1 ) = wynik ( 1, 0 );
wynik2 ( 0, 2 ) = wynik ( 2, 0 );
wynik2 ( 1, 0 ) = wynik ( 3, 0 );
wynik2 ( 1, 1 ) = wynik ( 4, 0 );
wynik2 ( 1, 2 ) = wynik ( 5, 0 );
wynik2 ( 2, 0 ) = wynik ( 6, 0 );
wynik2 ( 2, 1 ) = wynik ( 7, 0 );
wynik2 ( 2, 2 ) = 1;
return wynik2;
}
ransac::~ransac ()
{
}