1 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library.
2
3 // Copyright (c) 2012, 2013 Pierre MOULON.
4
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8
9 #include "openMVG/geometry/rigid_transformation3D_srt.hpp"
10 #include "CppUnitLite/TestHarness.h"
11 #include "testing/testing.h"
12 #include <iostream>
13
14 using namespace openMVG;
15 using namespace openMVG::geometry;
16 using namespace std;
17
TEST(SRT_precision,Experiment_ScaleOnly)18 TEST(SRT_precision, Experiment_ScaleOnly) {
19
20 int nbPoints = 10;
21 Mat x1 = Mat::Random(3,nbPoints);
22 Mat x2 = x1;
23
24 double scale = 2;
25 Mat3 rot = Mat3::Identity();
26 Vec3 t(0,0,0);
27
28 for (int i=0; i < nbPoints; ++i)
29 {
30 Vec3 pt = x1.col(i);
31 x2.col(i) = (scale * rot * pt + t);
32 }
33
34 // Compute the Similarity transform
35 double Sc;
36 Mat3 Rc;
37 Vec3 tc;
38 FindRTS(x1, x2, &Sc, &tc, &Rc);
39 Refine_RTS(x1,x2,&Sc,&tc,&Rc);
40
41 std::cout << "\n"
42 << "Scale " << Sc << "\n"
43 << "Rot \n" << Rc << "\n"
44 << "t " << tc.transpose();
45 }
46
TEST(SRT_precision,Experiment_ScaleAndRot)47 TEST(SRT_precision, Experiment_ScaleAndRot) {
48
49 int nbPoints = 10;
50 Mat x1 = Mat::Random(3,nbPoints);
51 Mat x2 = x1;
52
53 double scale = 2;
54 Mat3 rot = (Eigen::AngleAxis<double>(.2, Vec3::UnitX())
55 * Eigen::AngleAxis<double>(.3, Vec3::UnitY())
56 * Eigen::AngleAxis<double>(.6, Vec3::UnitZ())).toRotationMatrix();
57 Vec3 t(0,0,0);
58
59 for (int i=0; i < nbPoints; ++i)
60 {
61 Vec3 pt = x1.col(i);
62 x2.col(i) = (scale * rot * pt + t);
63 }
64
65 // Compute the Similarity transform
66 double Sc;
67 Mat3 Rc;
68 Vec3 tc;
69 FindRTS(x1, x2, &Sc, &tc, &Rc);
70 Refine_RTS(x1,x2,&Sc,&tc,&Rc);
71
72 std::cout << "\n"
73 << "Scale " << Sc << "\n"
74 << "Rot \n" << Rc << "\n"
75 << "t " << tc.transpose();
76
77 std::cout << "\nGT\n"
78 << "Scale " << scale << "\n"
79 << "Rot \n" << rot << "\n"
80 << "t " << t.transpose();
81 }
82
TEST(SRT_precision,Experiment_ScaleRotTranslation)83 TEST(SRT_precision, Experiment_ScaleRotTranslation) {
84
85 int nbPoints = 10;
86 Mat x1 = Mat::Random(3,nbPoints);
87 Mat x2 = x1;
88
89 double scale = 2;
90 Mat3 rot = (Eigen::AngleAxis<double>(.2, Vec3::UnitX())
91 * Eigen::AngleAxis<double>(.3, Vec3::UnitY())
92 * Eigen::AngleAxis<double>(.6, Vec3::UnitZ())).toRotationMatrix();
93 Vec3 t(0.5,-0.3,.38);
94
95 for (int i=0; i < nbPoints; ++i)
96 {
97 Vec3 pt = x1.col(i);
98 x2.col(i) = (scale * rot * pt + t);
99 }
100
101 // Compute the Similarity transform
102 double Sc;
103 Mat3 Rc;
104 Vec3 tc;
105 FindRTS(x1, x2, &Sc, &tc, &Rc);
106 Refine_RTS(x1,x2,&Sc,&tc,&Rc);
107
108 std::cout << "\n"
109 << "Scale " << Sc << "\n"
110 << "Rot \n" << Rc << "\n"
111 << "t " << tc.transpose();
112
113 std::cout << "\nGT\n"
114 << "Scale " << scale << "\n"
115 << "Rot \n" << rot << "\n"
116 << "t " << t.transpose();
117 }
118
119 /* ************************************************************************* */
main()120 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
121 /* ************************************************************************* */
122