1 // This is oxl/mvl/HMatrix2DEuclideanCompute.cxx
2
3 #include "HMatrix2DEuclideanCompute.h"
4
5 #include <cassert>
6 #ifdef _MSC_VER
7 # include "vcl_msvc_warnings.h"
8 #endif
9
10 #include "vnl/vnl_double_2.h"
11 #include "vnl/vnl_double_2x2.h"
12 #include "vnl/vnl_double_3x3.h"
13 #include "vnl/vnl_transpose.h"
14 #include <vnl/algo/vnl_svd.h>
15
16 #include <mvl/HMatrix2D.h>
17 #include <mvl/PairMatchSetCorner.h>
18 #include <mvl/HMatrix2DAffineCompute.h>
19
20 //
21 //
22 //
HMatrix2DEuclideanCompute()23 HMatrix2DEuclideanCompute::HMatrix2DEuclideanCompute() : HMatrix2DCompute() {}
24
25 HMatrix2DEuclideanCompute::~HMatrix2DEuclideanCompute() = default;
26
27 //
28 //
29 //
30 HMatrix2D
compute(PairMatchSetCorner const & matches)31 HMatrix2DEuclideanCompute::compute(PairMatchSetCorner const& matches)
32 {
33 PointArray pts1(matches.count());
34 PointArray pts2(matches.count());
35 matches.extract_matches(pts1, pts2);
36 HMatrix2D H;
37 tmp_fun(pts1,pts2,&H);
38 return H;
39 }
40
41 HMatrix2D
compute(PointArray const & p1,PointArray const & p2)42 HMatrix2DEuclideanCompute::compute(PointArray const& p1,
43 PointArray const& p2)
44 {
45 HMatrix2D H;
46 tmp_fun(p1,p2,&H);
47 return H;
48 }
49
50 bool
compute_p(PointArray const & pts1,PointArray const & pts2,HMatrix2D * H)51 HMatrix2DEuclideanCompute::compute_p(PointArray const& pts1,
52 PointArray const& pts2,
53 HMatrix2D *H)
54 {
55 return tmp_fun(pts1,pts2,H);
56 }
57
58 bool
tmp_fun(PointArray const & pts1,PointArray const & pts2,HMatrix2D * H)59 HMatrix2DEuclideanCompute::tmp_fun(PointArray const& pts1,
60 PointArray const& pts2,
61 HMatrix2D *H)
62 {
63 assert(pts1.size() == pts2.size());
64
65 NonHomg p1(pts1);
66 NonHomg p2(pts2);
67 vnl_double_2 mn1 = ::mean2(p1);
68 vnl_double_2 mn2 = ::mean2(p2);
69 sub_rows(p1,mn1);
70 sub_rows(p2,mn2);
71
72 vnl_double_2x2 scatter = vnl_transpose(p2)*p1;
73 vnl_svd<double> svd(scatter.as_ref()); // size 2x2
74
75 vnl_double_2x2 R = svd.U() * vnl_transpose(svd.V());
76 vnl_double_2 t = mn2 - R * mn1;
77
78 vnl_double_3x3 T;
79 T(0,0) = R(0,0); T(0,1) = R(0,1); T(0,2) = t[0];
80 T(1,0) = R(1,0); T(1,1) = R(1,1); T(1,2) = t[1];
81 T(2,0) = 0.0; T(2,1) = 0.0; T(2,2) = 1.0;
82 H->set(T);
83 return true;
84 }
85