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
|
#include "autostitch.h"
#include "meowpp/math/Matrix.h"
#include "meowpp/math/Vector.h"
#include <utility>
using namespace meow;
double MyRansacCheck::threshold = 5.0;
meow::Usage MyRansacCheck::usage(){
Usage usg;
usg.optionAdd('t',
"Threshold for RANSAC",
"<floating point>", stringPrintf("%.10f", threshold),
false);
return usg;
}
bool MyRansacCheck::usage(Usage const& usg){
threshold = inRange(0.0000001, 1000.0,
atof(usg.optionValue('t', 0).c_str()));
return true;
}
MyRansacCheck::MyRansacCheck(){
}
MyRansacCheck::MyRansacCheck(MyRansacCheck const& __rc):
_from(__rc._from),
_to(__rc._to){
}
MyRansacCheck::MyRansacCheck(std::vector<Vector<double> > const* __from,
std::vector<Vector<double> > const* __to):
_from(__from),
_to(__to){
}
MyRansacCheck::~MyRansacCheck(){
}
std::pair<Vector3D<double>, Vector3D<double> > MyRansacCheck::vCalc(
std::vector<FeaturePointIndexPair> const& __sample
) const{
Matrix<double> m(6, 7, 0.0);
for(size_t i = 0; i < 3u; i++){
m(i * 2 , 0, (*_from)[__sample[i].from.second](0));
m(i * 2 , 1, (*_from)[__sample[i].from.second](1));
m(i * 2 , 2, 1.0);
m(i * 2 , 6, (*_to)[__sample[i].to.second](0));
m(i * 2 + 1, 3, (*_from)[__sample[i].from.second](0));
m(i * 2 + 1, 4, (*_from)[__sample[i].from.second](1));
m(i * 2 + 1, 5, 1.0);
m(i * 2 + 1, 6, (*_to)[__sample[i].to.second](1));
}
m.triangulared();
Vector<double> x(6, 0.0);
for(ssize_t i = 5; i >= 0; i--){
double sum = 0;
for(size_t j = i + 1; j < 6u; j++){
sum += x(j) * m(i, j);
}
x.entry(i, (m(i, 6) - sum) / m(i, i));
}
Vector3D<double> vX(x(0), x(1), x(2));
Vector3D<double> vY(x(3), x(4), x(5));
return std::pair<Vector3D<double>, Vector3D<double> >(vX, vY);
}
void MyRansacCheck::rememberVCalc(std::vector<FeaturePointIndexPair>
const& __sample){
std::pair<Vector3D<double>, Vector3D<double> > p(vCalc(__sample));
_vX = p.first;
_vY = p.second;
}
bool MyRansacCheck::ok(FeaturePointIndexPair const& __m) const{
Vector2D<double> from(
(*_from)[__m.from.second](0),
(*_from)[__m.from.second](1));
Vector2D<double> me(
(*_to)[__m.to.second](0),
(*_to)[__m.to.second](1));
Vector2D<double> me2(to(from));
return ((me - me2).length2() <= threshold);
}
double MyRansacCheck::operator()(std::vector<FeaturePointIndexPair>
const& __sample,
std::vector<FeaturePointIndexPair>
const& __data) const{
for(size_t i = 0, I = __sample.size(); i < I; i++){
for(size_t j = 0, J = __sample.size(); j < J; j++){
if(i == j) continue;
if(__sample[i].from.second == __sample[j].from.second) return -1;
if(__sample[i].to .second == __sample[j].to .second) return -1;
}
}
((MyRansacCheck*)this)->rememberVCalc(__sample);
size_t ret = 0;
for(size_t i = 0, I = __data.size(); i < I; i++){
if(ok(__data[i])){
ret++;
}
}
return 0.001 + ret;
}
Vector2D<double> MyRansacCheck::to(Vector2D<double> const& __v) const{
Vector3D<double> v(__v(0), __v(1), 1);
return Vector2D<double>(v.dot(_vX), v.dot(_vY));
}
|
