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