1 // Test vgl_rotate_3d
2
3 #include "testlib/testlib_test.h"
4 #include <vgl/algo/vgl_rotation_3d.h>
5 #include "vgl/vgl_plane_3d.h"
6 #include "vgl/vgl_homg_line_3d_2_points.h"
7 #include "vgl/vgl_line_3d_2_points.h"
8 #include "vgl/vgl_line_segment_3d.h"
9 #include "vgl/vgl_distance.h"
10 #include "vnl/vnl_double_3.h"
11 #include "vnl/vnl_double_3x3.h"
12 #include "vnl/vnl_random.h"
13 #include "vnl/vnl_rational.h"
14 #include "vnl/vnl_rational_traits.h"
15 #include "vnl/vnl_math.h" // for sqrt2 and pi/2
16
17 static const double epsilon = 1e-11;
18
19 // Test conversions between various rotation representations
20 static void
test_conversions(const vgl_rotation_3d<double> & rot)21 test_conversions(const vgl_rotation_3d<double> & rot)
22 {
23 vnl_double_3x3 R = rot.as_matrix();
24 vgl_h_matrix_3d<double> H = rot.as_h_matrix_3d();
25 vnl_double_3 rr = rot.as_rodrigues();
26 vnl_double_3 er = rot.as_euler_angles();
27 vnl_quaternion<double> qr = rot.as_quaternion();
28 if (qr.real() < 0.0)
29 qr *= -1.0;
30
31 vnl_quaternion<double> qr_other = vgl_rotation_3d<double>(R).as_quaternion();
32 if (qr_other.real() < 0.0)
33 qr_other *= -1.0;
34 double diff1 = (qr_other - qr).magnitude();
35 TEST_NEAR("Matrix conversion", diff1, 0.0, epsilon);
36 if (diff1 > epsilon)
37 std::cout << "Matrix:\n" << R << std::endl;
38
39 qr_other = vgl_rotation_3d<double>(H).as_quaternion();
40 if (qr_other.real() < 0.0)
41 qr_other *= -1.0;
42 double diff2 = (qr_other - qr).magnitude();
43 TEST_NEAR("3D Homography conversion", diff2, 0.0, epsilon);
44 if (diff2 > epsilon)
45 std::cout << "3D Homography:\n" << H << std::endl;
46
47 qr_other = vgl_rotation_3d<double>(rr).as_quaternion();
48 if (qr_other.real() < 0.0)
49 qr_other *= -1.0;
50 double diff3 = (qr_other - qr).magnitude();
51 TEST_NEAR("Rodrigues conversion", diff3, 0.0, epsilon);
52 if (diff3 > epsilon)
53 std::cout << "Rodrigues: " << rr << std::endl;
54
55 qr_other = vgl_rotation_3d<double>(er[0], er[1], er[2]).as_quaternion();
56 if (qr_other.real() < 0.0)
57 qr_other *= -1.0;
58 double diff4 = (qr_other - qr).magnitude();
59 TEST_NEAR("Euler conversion", diff4, 0.0, epsilon);
60 if (diff4 > epsilon)
61 std::cout << "Euler: Rx=" << er[0] << " Ry=" << er[1] << " Rz=" << er[2] << std::endl;
62 // Test the case of flipping the orientation of a vector (rotation by pi)
63 vgl_vector_3d<double> a(1.0, 1.0, 1.0), aflip(-1.0, -1.0, -1.0);
64 vgl_rotation_3d<double> flip(a, aflip);
65 vgl_vector_3d<double> v = flip * a;
66 vgl_vector_3d<double> null = v + a;
67 double err = std::fabs(null.x()) + std::fabs(null.y()) + std::fabs(null.z());
68 TEST_NEAR("Flip vector", err, 0.0, epsilon);
69 }
70
71
72 // Test that the inverse rotation works as expected
73 static void
test_inverse(const vgl_rotation_3d<double> & rot)74 test_inverse(const vgl_rotation_3d<double> & rot)
75 {
76 vgl_rotation_3d<double> I = rot * rot.inverse();
77 double diff = (I.as_quaternion() - vgl_rotation_3d<double>().as_quaternion()).magnitude();
78 TEST_NEAR("Inverse rotation", diff, 0.0, epsilon);
79 }
80
81 // Test that the transpose or conjugate rotation works as expected
82 static void
test_transpose(const vgl_rotation_3d<double> & rot)83 test_transpose(const vgl_rotation_3d<double> & rot)
84 {
85 vgl_rotation_3d<double> I = rot * rot.transpose();
86 double diff = (I.as_quaternion() - vgl_rotation_3d<double>().as_quaternion()).magnitude();
87 TEST_NEAR("transpose or conjugate rotation", diff, 0.0, epsilon);
88 }
89
90 // Test application of rotation to other vgl objects
91 static void
test_application(const vgl_rotation_3d<double> & rot)92 test_application(const vgl_rotation_3d<double> & rot)
93 {
94 vnl_double_3x3 R = rot.as_matrix();
95
96 vgl_homg_point_3d<double> hpt(100, 50, 200, 2);
97 vnl_double_3 p(hpt.x() / hpt.w(), hpt.y() / hpt.w(), hpt.z() / hpt.w());
98
99 vgl_homg_plane_3d<double> hpl(0, 1, 0, 100);
100
101 vgl_homg_point_3d<double> r_hpt = rot * hpt;
102 vnl_double_3 r_p = R * p;
103
104 vgl_homg_plane_3d<double> r_hpl = rot * hpl;
105
106 TEST_NEAR("Rotated point-plane dist (homg)", vgl_distance(hpt, hpl), vgl_distance(r_hpt, r_hpl), epsilon);
107
108 double diff = (r_p - vnl_double_3(r_hpt.x() / r_hpt.w(), r_hpt.y() / r_hpt.w(), r_hpt.z() / r_hpt.w())).magnitude();
109 TEST_NEAR("Rotated point", diff, 0.0, epsilon);
110
111 vgl_point_3d<double> pt(-30, 0, 75);
112 vgl_point_3d<double> r_pt = rot * pt;
113
114 vgl_plane_3d<double> pl(0, 0, 1, 50);
115 vgl_plane_3d<double> r_pl = rot * pl;
116 TEST_NEAR("Rotated point-plane dist", vgl_distance(pt, pl), vgl_distance(r_pt, r_pl), epsilon);
117
118 vgl_vector_3d<double> v = vgl_point_3d<double>(hpt) - pt;
119 vgl_vector_3d<double> r_v = rot * v;
120 vgl_vector_3d<double> r_v2 = vgl_point_3d<double>(r_hpt) - r_pt;
121 TEST_NEAR("Rotated vector", (r_v - r_v2).length(), 0.0, epsilon);
122
123 vgl_homg_line_3d_2_points<double> hl(hpt, vgl_homg_point_3d<double>(v.x(), v.y(), v.z(), 0));
124 vgl_homg_line_3d_2_points<double> r_hl = rot * hl;
125 std::cout << "rotated hl = " << r_hl << std::endl;
126 // FIXME add test
127
128 vgl_line_3d_2_points<double> l(pt, pt + v);
129 vgl_line_3d_2_points<double> r_l = rot * l;
130 std::cout << "rotated l = " << r_l << std::endl;
131 // FIXME add test
132
133 vgl_line_segment_3d<double> s(pt, pt + v);
134 vgl_line_segment_3d<double> r_s = rot * s;
135 std::cout << "rotated s = " << r_s << std::endl;
136 // FIXME add test
137 }
138
139
140 void
test_rotation_3d()141 test_rotation_3d()
142 {
143 std::cout << "*************************\n"
144 << " Testing vgl_rotation_3d\n"
145 << "*************************\n";
146
147 std::cout << "\n1. Rotation about the x axis over 90 degrees.\n";
148 vgl_rotation_3d<double> rot_id;
149 test_conversions(rot_id);
150 test_inverse(rot_id);
151 test_transpose(rot_id);
152 test_application(rot_id);
153
154 std::cout << "\n2. Rotation about the x axis over 90 degrees.\n";
155
156 vgl_rotation_3d<double> rot_x90(vnl_math::pi_over_2, 0.0, 0.0);
157 test_conversions(rot_x90);
158 test_inverse(rot_x90);
159 test_transpose(rot_x90);
160 test_application(rot_x90);
161
162 vnl_random rnd;
163 vgl_rotation_3d<double> rot_rand(rnd.normal(), rnd.normal(), rnd.normal());
164 std::cout << "\n3. Random rotation: " << rot_rand.as_quaternion() << std::endl;
165 test_conversions(rot_rand);
166 test_inverse(rot_rand);
167 test_transpose(rot_rand);
168 test_application(rot_rand);
169 // test constructor from two vectors
170 vnl_double_3 b(0.0, 0.0, 1.0), ap(vnl_math::sqrt1_2, 0.0, vnl_math::sqrt1_2), // vector of magnitude 1
171 am(-1.0, 0.0, -1.0), // magnitude > 1, to test robustified constructor
172 a1(0.5773502692, -0.5773502692, 0.5773502692), // magnitude 1
173 a2(0, 0, -1); // 180 degree rotation, w/negative z component
174 vgl_rotation_3d<double> r_abp(ap, b);
175 vgl_rotation_3d<double> r_abm(am, b);
176 vgl_rotation_3d<double> r_ab1(a1, b);
177 vgl_rotation_3d<double> r_ab2(a2, b);
178 vnl_double_3 ap_to_b = r_abp * ap, am_to_b = r_abm * am, a1_to_b = r_ab1 * a1, a2_to_b = r_ab2 * a2;
179 double errorp = (b - ap_to_b).squared_magnitude();
180 TEST_NEAR("constructor from 2 vectors: rotate 45d around Y axis", errorp, 0.0, epsilon);
181 double errorm = (b * vnl_math::sqrt2 - am_to_b).squared_magnitude();
182 TEST_NEAR("constructor from 2 vectors: rotate 225d around Y axis", errorm, 0.0, epsilon);
183 double error1 = (b - a1_to_b).squared_magnitude();
184 TEST_NEAR("constructor from 2 vectors: from arbitrary point", error1, 0.0, epsilon);
185 double error2 = (b - a2_to_b).squared_magnitude();
186 TEST_NEAR("constructor from 2 vectors: 180 degree rotation", error2, 0.0, epsilon);
187 vgl_vector_3d<float> ag(1.0f, 1.0f, 0.0f), bg(0.0f, 0.0f, float(vnl_math::sqrt2));
188 vgl_rotation_3d<float> r_abg(ag, bg);
189 vgl_vector_3d<float> ag_to_bg = r_abg * ag;
190 float errorf = (bg - ag_to_bg).sqr_length();
191 TEST_NEAR("constructor from two vgl vectors", errorf, 0.0f, 1e-6f);
192 vnl_vector_fixed<vnl_rational, 3> ai(1L, 1L, 0L), bi(1L, -1L, 0L);
193 vgl_rotation_3d<vnl_rational> r_abi(ai, bi);
194 #define sqr(x) ((x) * (x))
195 error1 = sqr(double(r_abi.as_quaternion()[0])) + sqr(double(r_abi.as_quaternion()[1])) +
196 sqr(double(r_abi.as_quaternion()[2]) + vnl_math::sqrt1_2) +
197 sqr(double(r_abi.as_quaternion()[3]) - vnl_math::sqrt1_2);
198 TEST_NEAR("rotation is 90 deg in XY plane", error1, 0.0, epsilon);
199 #if VXL_INT_64_IS_LONG || VXL_INT_64_IS_LONGLONG
200 // temporary fix for a "bug" (actually just integer overflow) in vnl_rational:
201 vnl_rational sqrthalf(453016774L, 640662461L);
202 r_abi = vnl_quaternion<vnl_rational>(0, 0, -sqrthalf, sqrthalf);
203 #endif // VXL_INT_64_IS_LONG
204 vnl_vector_fixed<vnl_rational, 3> ai_to_bi = r_abi * ai;
205 vnl_rational errori = (bi - ai_to_bi).squared_magnitude();
206 TEST_NEAR("constructor from two rational-coordinate vectors", errori, 0L, epsilon);
207 }
208
209 TESTMAIN(test_rotation_3d);
210