blob: a8351d50be5097949d8d693bc9038f89e7d07280 [file] [log] [blame]
Keir Mierle8ebb0732012-04-30 23:09:08 -07001// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9// this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11// this list of conditions and the following disclaimer in the documentation
12// and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14// used to endorse or promote products derived from this software without
15// specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: sameeragarwal@google.com (Sameer Agarwal)
30
31#include <cmath>
32#include <limits>
33#include <string>
34
35#include <glog/logging.h>
36#include "gmock/gmock.h"
37#include "gtest/gtest.h"
38#include "ceres/stringprintf.h"
39#include "ceres/test_util.h"
40#include "ceres/internal/eigen.h"
41#include "ceres/internal/port.h"
42#include "ceres/jet.h"
43#include "ceres/rotation.h"
44
45namespace ceres {
46namespace internal {
47
48double RandDouble() {
49 double r = rand();
50 return r / RAND_MAX;
51}
52
53// A tolerance value for floating-point comparisons.
54static double const kTolerance = numeric_limits<double>::epsilon() * 10;
55
56// Looser tolerance used for for numerically unstable conversions.
57static double const kLooseTolerance = 1e-9;;
58
59// Use as:
60// double quaternion[4];
61// EXPECT_THAT(quaternion, IsNormalizedQuaternion());
62MATCHER(IsNormalizedQuaternion, "") {
63 if (arg == NULL) {
64 *result_listener << "Null quaternion";
65 return false;
66 }
67
68 double norm2 = arg[0] * arg[0] + arg[1] * arg[1] +
69 arg[2] * arg[2] + arg[3] * arg[3];
70 if (fabs(norm2 - 1.0) > kTolerance) {
71 *result_listener << "squared norm is " << norm2;
72 return false;
73 }
74
75 return true;
76}
77
78// Use as:
79// double expected_quaternion[4];
80// double actual_quaternion[4];
81// EXPECT_THAT(actual_quaternion, IsNearQuaternion(expected_quaternion));
82MATCHER_P(IsNearQuaternion, expected, "") {
83 if (arg == NULL) {
84 *result_listener << "Null quaternion";
85 return false;
86 }
87
88 for (int i = 0; i < 4; i++) {
89 if (fabs(arg[i] - expected[i]) > kTolerance) {
90 *result_listener << "component " << i << " should be " << expected[i];
91 return false;
92 }
93 }
94
95 return true;
96}
97
98// Use as:
99// double expected_axis_angle[4];
100// double actual_axis_angle[4];
101// EXPECT_THAT(actual_axis_angle, IsNearAngleAxis(expected_axis_angle));
102MATCHER_P(IsNearAngleAxis, expected, "") {
103 if (arg == NULL) {
104 *result_listener << "Null axis/angle";
105 return false;
106 }
107
108 for (int i = 0; i < 3; i++) {
109 if (fabs(arg[i] - expected[i]) > kTolerance) {
110 *result_listener << "component " << i << " should be " << expected[i];
111 return false;
112 }
113 }
114
115 return true;
116}
117
118// Use as:
119// double matrix[9];
120// EXPECT_THAT(matrix, IsOrthonormal());
121MATCHER(IsOrthonormal, "") {
122 if (arg == NULL) {
123 *result_listener << "Null matrix";
124 return false;
125 }
126
127 for (int c1 = 0; c1 < 3; c1++) {
128 for (int c2 = 0; c2 < 3; c2++) {
129 double v = 0;
130 for (int i = 0; i < 3; i++) {
131 v += arg[i + 3 * c1] * arg[i + 3 * c2];
132 }
133 double expected = (c1 == c2) ? 1 : 0;
134 if (fabs(expected - v) > kTolerance) {
135 *result_listener << "Columns " << c1 << " and " << c2
136 << " should have dot product " << expected
137 << " but have " << v;
138 return false;
139 }
140 }
141 }
142
143 return true;
144}
145
146// Use as:
147// double matrix1[9];
148// double matrix2[9];
149// EXPECT_THAT(matrix1, IsNear3x3Matrix(matrix2));
150MATCHER_P(IsNear3x3Matrix, expected, "") {
151 if (arg == NULL) {
152 *result_listener << "Null matrix";
153 return false;
154 }
155
156 for (int i = 0; i < 9; i++) {
157 if (fabs(arg[i] - expected[i]) > kTolerance) {
158 *result_listener << "component " << i << " should be " << expected[i];
159 return false;
160 }
161 }
162
163 return true;
164}
165
166// Transforms a zero axis/angle to a quaternion.
167TEST(Rotation, ZeroAngleAxisToQuaternion) {
168 double axis_angle[3] = { 0, 0, 0 };
169 double quaternion[4];
170 double expected[4] = { 1, 0, 0, 0 };
171 AngleAxisToQuaternion(axis_angle, quaternion);
172 EXPECT_THAT(quaternion, IsNormalizedQuaternion());
173 EXPECT_THAT(quaternion, IsNearQuaternion(expected));
174}
175
176// Test that exact conversion works for small angles.
177TEST(Rotation, SmallAngleAxisToQuaternion) {
178 // Small, finite value to test.
179 double theta = 1.0e-2;
180 double axis_angle[3] = { theta, 0, 0 };
181 double quaternion[4];
182 double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
183 AngleAxisToQuaternion(axis_angle, quaternion);
184 EXPECT_THAT(quaternion, IsNormalizedQuaternion());
185 EXPECT_THAT(quaternion, IsNearQuaternion(expected));
186}
187
188// Test that approximate conversion works for very small angles.
189TEST(Rotation, TinyAngleAxisToQuaternion) {
190 // Very small value that could potentially cause underflow.
191 double theta = pow(numeric_limits<double>::min(), 0.75);
192 double axis_angle[3] = { theta, 0, 0 };
193 double quaternion[4];
194 double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
195 AngleAxisToQuaternion(axis_angle, quaternion);
196 EXPECT_THAT(quaternion, IsNormalizedQuaternion());
197 EXPECT_THAT(quaternion, IsNearQuaternion(expected));
198}
199
200// Transforms a rotation by pi/2 around X to a quaternion.
201TEST(Rotation, XRotationToQuaternion) {
202 double axis_angle[3] = { M_PI / 2, 0, 0 };
203 double quaternion[4];
204 double expected[4] = { M_SQRT1_2, M_SQRT1_2, 0, 0 };
205 AngleAxisToQuaternion(axis_angle, quaternion);
206 EXPECT_THAT(quaternion, IsNormalizedQuaternion());
207 EXPECT_THAT(quaternion, IsNearQuaternion(expected));
208}
209
210// Transforms a unit quaternion to an axis angle.
211TEST(Rotation, UnitQuaternionToAngleAxis) {
212 double quaternion[4] = { 1, 0, 0, 0 };
213 double axis_angle[3];
214 double expected[3] = { 0, 0, 0 };
215 QuaternionToAngleAxis(quaternion, axis_angle);
216 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
217}
218
219// Transforms a quaternion that rotates by pi about the Y axis to an axis angle.
220TEST(Rotation, YRotationQuaternionToAngleAxis) {
221 double quaternion[4] = { 0, 0, 1, 0 };
222 double axis_angle[3];
223 double expected[3] = { 0, M_PI, 0 };
224 QuaternionToAngleAxis(quaternion, axis_angle);
225 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
226}
227
228// Transforms a quaternion that rotates by pi/3 about the Z axis to an axis
229// angle.
230TEST(Rotation, ZRotationQuaternionToAngleAxis) {
231 double quaternion[4] = { sqrt(3) / 2, 0, 0, 0.5 };
232 double axis_angle[3];
233 double expected[3] = { 0, 0, M_PI / 3 };
234 QuaternionToAngleAxis(quaternion, axis_angle);
235 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
236}
237
238// Test that exact conversion works for small angles.
239TEST(Rotation, SmallQuaternionToAngleAxis) {
240 // Small, finite value to test.
241 double theta = 1.0e-2;
242 double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
243 double axis_angle[3];
244 double expected[3] = { theta, 0, 0 };
245 QuaternionToAngleAxis(quaternion, axis_angle);
246 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
247}
248
249// Test that approximate conversion works for very small angles.
250TEST(Rotation, TinyQuaternionToAngleAxis) {
251 // Very small value that could potentially cause underflow.
252 double theta = pow(numeric_limits<double>::min(), 0.75);
253 double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
254 double axis_angle[3];
255 double expected[3] = { theta, 0, 0 };
256 QuaternionToAngleAxis(quaternion, axis_angle);
257 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
258}
259
260static const int kNumTrials = 10000;
261
262// Takes a bunch of random axis/angle values, converts them to quaternions,
263// and back again.
264TEST(Rotation, AngleAxisToQuaterionAndBack) {
265 srand(5);
266 for (int i = 0; i < kNumTrials; i++) {
267 double axis_angle[3];
268 // Make an axis by choosing three random numbers in [-1, 1) and
269 // normalizing.
270 double norm = 0;
271 for (int i = 0; i < 3; i++) {
272 axis_angle[i] = RandDouble() * 2 - 1;
273 norm += axis_angle[i] * axis_angle[i];
274 }
275 norm = sqrt(norm);
276
277 // Angle in [-pi, pi).
278 double theta = M_PI * 2 * RandDouble() - M_PI;
279 for (int i = 0; i < 3; i++) {
280 axis_angle[i] = axis_angle[i] * theta / norm;
281 }
282
283 double quaternion[4];
284 double round_trip[3];
285 // We use ASSERTs here because if there's one failure, there are
286 // probably many and spewing a million failures doesn't make anyone's
287 // day.
288 AngleAxisToQuaternion(axis_angle, quaternion);
289 ASSERT_THAT(quaternion, IsNormalizedQuaternion());
290 QuaternionToAngleAxis(quaternion, round_trip);
291 ASSERT_THAT(round_trip, IsNearAngleAxis(axis_angle));
292 }
293}
294
295// Takes a bunch of random quaternions, converts them to axis/angle,
296// and back again.
297TEST(Rotation, QuaterionToAngleAxisAndBack) {
298 srand(5);
299 for (int i = 0; i < kNumTrials; i++) {
300 double quaternion[4];
301 // Choose four random numbers in [-1, 1) and normalize.
302 double norm = 0;
303 for (int i = 0; i < 4; i++) {
304 quaternion[i] = RandDouble() * 2 - 1;
305 norm += quaternion[i] * quaternion[i];
306 }
307 norm = sqrt(norm);
308
309 for (int i = 0; i < 4; i++) {
310 quaternion[i] = quaternion[i] / norm;
311 }
312
313 double axis_angle[3];
314 double round_trip[4];
315 QuaternionToAngleAxis(quaternion, axis_angle);
316 AngleAxisToQuaternion(axis_angle, round_trip);
317 ASSERT_THAT(round_trip, IsNormalizedQuaternion());
318 ASSERT_THAT(round_trip, IsNearQuaternion(quaternion));
319 }
320}
321
322// Transforms a zero axis/angle to a rotation matrix.
323TEST(Rotation, ZeroAngleAxisToRotationMatrix) {
324 double axis_angle[3] = { 0, 0, 0 };
325 double matrix[9];
326 double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
327 AngleAxisToRotationMatrix(axis_angle, matrix);
328 EXPECT_THAT(matrix, IsOrthonormal());
329 EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
330}
331
332TEST(Rotation, NearZeroAngleAxisToRotationMatrix) {
333 double axis_angle[3] = { 1e-24, 2e-24, 3e-24 };
334 double matrix[9];
335 double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
336 AngleAxisToRotationMatrix(axis_angle, matrix);
337 EXPECT_THAT(matrix, IsOrthonormal());
338 EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
339}
340
341// Transforms a rotation by pi/2 around X to a rotation matrix and back.
342TEST(Rotation, XRotationToRotationMatrix) {
343 double axis_angle[3] = { M_PI / 2, 0, 0 };
344 double matrix[9];
345 // The rotation matrices are stored column-major.
346 double expected[9] = { 1, 0, 0, 0, 0, 1, 0, -1, 0 };
347 AngleAxisToRotationMatrix(axis_angle, matrix);
348 EXPECT_THAT(matrix, IsOrthonormal());
349 EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
350 double round_trip[3];
351 RotationMatrixToAngleAxis(matrix, round_trip);
352 EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle));
353}
354
355// Transforms an axis angle that rotates by pi about the Y axis to a
356// rotation matrix and back.
357TEST(Rotation, YRotationToRotationMatrix) {
358 double axis_angle[3] = { 0, M_PI, 0 };
359 double matrix[9];
360 double expected[9] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 };
361 AngleAxisToRotationMatrix(axis_angle, matrix);
362 EXPECT_THAT(matrix, IsOrthonormal());
363 EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
364
365 double round_trip[3];
366 RotationMatrixToAngleAxis(matrix, round_trip);
367 EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle));
368}
369
370TEST(Rotation, NearPiAngleAxisRoundTrip) {
371 double in_axis_angle[3];
372 double matrix[9];
373 double out_axis_angle[3];
374
375 srand(5);
376 for (int i = 0; i < kNumTrials; i++) {
377 // Make an axis by choosing three random numbers in [-1, 1) and
378 // normalizing.
379 double norm = 0;
380 for (int i = 0; i < 3; i++) {
381 in_axis_angle[i] = RandDouble() * 2 - 1;
382 norm += in_axis_angle[i] * in_axis_angle[i];
383 }
384 norm = sqrt(norm);
385
386 // Angle in [pi - kMaxSmallAngle, pi).
387 const double kMaxSmallAngle = 1e-2;
388 double theta = M_PI - kMaxSmallAngle * RandDouble();
389
390 for (int i = 0; i < 3; i++) {
391 in_axis_angle[i] *= (theta / norm);
392 }
393 AngleAxisToRotationMatrix(in_axis_angle, matrix);
394 RotationMatrixToAngleAxis(matrix, out_axis_angle);
395
396 for (int i = 0; i < 3; ++i) {
397 EXPECT_NEAR(out_axis_angle[i], in_axis_angle[i], kLooseTolerance);
398 }
399 }
400}
401
402TEST(Rotation, AtPiAngleAxisRoundTrip) {
403 // A rotation of M_PI about the X axis;
404 static const double kMatrix[3][3] = {
405 {1.0, 0.0, 0.0},
406 {0.0, -1.0, 0.0},
407 {0.0, 0.0, -1.0}
408 };
409
410 double in_matrix[9];
411 // Fill it from kMatrix in col-major order.
412 for (int j = 0, k = 0; j < 3; ++j) {
413 for (int i = 0; i < 3; ++i, ++k) {
414 in_matrix[k] = kMatrix[i][j];
415 }
416 }
417
418 const double expected_axis_angle[3] = { M_PI, 0, 0 };
419
420 double out_matrix[9];
421 double axis_angle[3];
422 RotationMatrixToAngleAxis(in_matrix, axis_angle);
423 AngleAxisToRotationMatrix(axis_angle, out_matrix);
424
425 LOG(INFO) << "AngleAxis = " << axis_angle[0] << " " << axis_angle[1]
426 << " " << axis_angle[2];
427 LOG(INFO) << "Expected AngleAxis = " << M_PI << " 0 0";
428 double out_rowmajor[3][3];
429 for (int j = 0, k = 0; j < 3; ++j) {
430 for (int i = 0; i < 3; ++i, ++k) {
431 out_rowmajor[i][j] = out_matrix[k];
432 }
433 }
434 LOG(INFO) << "Rotation:";
435 LOG(INFO) << "EXPECTED | ACTUAL";
436 for (int i = 0; i < 3; ++i) {
437 string line;
438 for (int j = 0; j < 3; ++j) {
439 StringAppendF(&line, "%g ", kMatrix[i][j]);
440 }
441 line += " | ";
442 for (int j = 0; j < 3; ++j) {
443 StringAppendF(&line, "%g ", out_rowmajor[i][j]);
444 }
445 LOG(INFO) << line;
446 }
447
448 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected_axis_angle));
449 EXPECT_THAT(out_matrix, IsNear3x3Matrix(in_matrix));
450}
451
452// Transforms an axis angle that rotates by pi/3 about the Z axis to a
453// rotation matrix.
454TEST(Rotation, ZRotationToRotationMatrix) {
455 double axis_angle[3] = { 0, 0, M_PI / 3 };
456 double matrix[9];
457 // This is laid-out row-major on the screen but is actually stored
458 // column-major.
459 double expected[9] = { 0.5, sqrt(3) / 2, 0, // Column 1
460 -sqrt(3) / 2, 0.5, 0, // Column 2
461 0, 0, 1 }; // Column 3
462 AngleAxisToRotationMatrix(axis_angle, matrix);
463 EXPECT_THAT(matrix, IsOrthonormal());
464 EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
465 double round_trip[3];
466 RotationMatrixToAngleAxis(matrix, round_trip);
467 EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle));
468}
469
470// Takes a bunch of random axis/angle values, converts them to rotation
471// matrices, and back again.
472TEST(Rotation, AngleAxisToRotationMatrixAndBack) {
473 srand(5);
474 for (int i = 0; i < kNumTrials; i++) {
475 double axis_angle[3];
476 // Make an axis by choosing three random numbers in [-1, 1) and
477 // normalizing.
478 double norm = 0;
479 for (int i = 0; i < 3; i++) {
480 axis_angle[i] = RandDouble() * 2 - 1;
481 norm += axis_angle[i] * axis_angle[i];
482 }
483 norm = sqrt(norm);
484
485 // Angle in [-pi, pi).
486 double theta = M_PI * 2 * RandDouble() - M_PI;
487 for (int i = 0; i < 3; i++) {
488 axis_angle[i] = axis_angle[i] * theta / norm;
489 }
490
491 double matrix[9];
492 double round_trip[3];
493 AngleAxisToRotationMatrix(axis_angle, matrix);
494 ASSERT_THAT(matrix, IsOrthonormal());
495 RotationMatrixToAngleAxis(matrix, round_trip);
496
497 for (int i = 0; i < 3; ++i) {
498 EXPECT_NEAR(round_trip[i], axis_angle[i], kLooseTolerance);
499 }
500 }
501}
502
503// Transposes a 3x3 matrix.
504static void Transpose3x3(double m[9]) {
505 std::swap(m[1], m[3]);
506 std::swap(m[2], m[6]);
507 std::swap(m[5], m[7]);
508}
509
510// Convert Euler angles from radians to degrees.
511static void ToDegrees(double ea[3]) {
512 for (int i = 0; i < 3; ++i)
513 ea[i] *= 180.0 / M_PI;
514}
515
516// Compare the 3x3 rotation matrices produced by the axis-angle
517// rotation 'aa' and the Euler angle rotation 'ea' (in radians).
518static void CompareEulerToAngleAxis(double aa[3], double ea[3]) {
519 double aa_matrix[9];
520 AngleAxisToRotationMatrix(aa, aa_matrix);
521 Transpose3x3(aa_matrix); // Column to row major order.
522
523 double ea_matrix[9];
524 ToDegrees(ea); // Radians to degrees.
525 const int kRowStride = 3;
526 EulerAnglesToRotationMatrix(ea, kRowStride, ea_matrix);
527
528 EXPECT_THAT(aa_matrix, IsOrthonormal());
529 EXPECT_THAT(ea_matrix, IsOrthonormal());
530 EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix));
531}
532
533// Test with rotation axis along the x/y/z axes.
534// Also test zero rotation.
535TEST(EulerAnglesToRotationMatrix, OnAxis) {
536 int n_tests = 0;
537 for (double x = -1.0; x <= 1.0; x += 1.0) {
538 for (double y = -1.0; y <= 1.0; y += 1.0) {
539 for (double z = -1.0; z <= 1.0; z += 1.0) {
540 if ((x != 0) + (y != 0) + (z != 0) > 1)
541 continue;
542 double axis_angle[3] = {x, y, z};
543 double euler_angles[3] = {x, y, z};
544 CompareEulerToAngleAxis(axis_angle, euler_angles);
545 ++n_tests;
546 }
547 }
548 }
549 CHECK_EQ(7, n_tests);
550}
551
552// Test that a random rotation produces an orthonormal rotation
553// matrix.
554TEST(EulerAnglesToRotationMatrix, IsOrthonormal) {
555 srand(5);
556 for (int trial = 0; trial < kNumTrials; ++trial) {
557 double ea[3];
558 for (int i = 0; i < 3; ++i)
559 ea[i] = 360.0 * (RandDouble() * 2.0 - 1.0);
560 double ea_matrix[9];
561 ToDegrees(ea); // Radians to degrees.
562 EulerAnglesToRotationMatrix(ea, 3, ea_matrix);
563 EXPECT_THAT(ea_matrix, IsOrthonormal());
564 }
565}
566
567// Tests using Jets for specific behavior involving auto differentiation
568// near singularity points.
569
570typedef Jet<double, 3> J3;
571typedef Jet<double, 4> J4;
572
573J3 MakeJ3(double a, double v0, double v1, double v2) {
574 J3 j;
575 j.a = a;
576 j.v[0] = v0;
577 j.v[1] = v1;
578 j.v[2] = v2;
579 return j;
580}
581
582J4 MakeJ4(double a, double v0, double v1, double v2, double v3) {
583 J4 j;
584 j.a = a;
585 j.v[0] = v0;
586 j.v[1] = v1;
587 j.v[2] = v2;
588 j.v[3] = v3;
589 return j;
590}
591
592
593bool IsClose(double x, double y) {
594 EXPECT_FALSE(isnan(x));
595 EXPECT_FALSE(isnan(y));
596 double absdiff = fabs(x - y);
597 if (x == 0 || y == 0) {
598 return absdiff <= kTolerance;
599 }
600 double reldiff = absdiff / max(fabs(x), fabs(y));
601 return reldiff <= kTolerance;
602}
603
604template <int N>
605bool IsClose(const Jet<double, N> &x, const Jet<double, N> &y) {
606 if (IsClose(x.a, y.a)) {
607 for (int i = 0; i < N; i++) {
608 if (!IsClose(x.v[i], y.v[i])) {
609 return false;
610 }
611 }
612 }
613 return true;
614}
615
616template <int M, int N>
617void ExpectJetArraysClose(const Jet<double, N> *x, const Jet<double, N> *y) {
618 for (int i = 0; i < M; i++) {
619 if (!IsClose(x[i], y[i])) {
620 LOG(ERROR) << "Jet " << i << "/" << M << " not equal";
621 LOG(ERROR) << "x[" << i << "]: " << x[i];
622 LOG(ERROR) << "y[" << i << "]: " << y[i];
623 Jet<double, N> d, zero;
624 d.a = y[i].a - x[i].a;
625 for (int j = 0; j < N; j++) {
626 d.v[j] = y[i].v[j] - x[i].v[j];
627 }
628 LOG(ERROR) << "diff: " << d;
629 EXPECT_TRUE(IsClose(x[i], y[i]));
630 }
631 }
632}
633
634// Log-10 of a value well below machine precision.
635static const int kSmallTinyCutoff =
636 static_cast<int>(2 * log(numeric_limits<double>::epsilon())/log(10.0));
637
638// Log-10 of a value just below values representable by double.
639static const int kTinyZeroLimit =
640 static_cast<int>(1 + log(numeric_limits<double>::min())/log(10.0));
641
642// Test that exact conversion works for small angles when jets are used.
643TEST(Rotation, SmallAngleAxisToQuaternionForJets) {
644 // Examine small x rotations that are still large enough
645 // to be well within the range represented by doubles.
646 for (int i = -2; i >= kSmallTinyCutoff; i--) {
647 double theta = pow(10.0, i);
648 J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) };
649 J3 quaternion[4];
650 J3 expected[4] = {
651 MakeJ3(cos(theta/2), -sin(theta/2)/2, 0, 0),
652 MakeJ3(sin(theta/2), cos(theta/2)/2, 0, 0),
653 MakeJ3(0, 0, sin(theta/2)/theta, 0),
654 MakeJ3(0, 0, 0, sin(theta/2)/theta),
655 };
656 AngleAxisToQuaternion(axis_angle, quaternion);
657 ExpectJetArraysClose<4, 3>(quaternion, expected);
658 }
659}
660
661
662// Test that conversion works for very small angles when jets are used.
663TEST(Rotation, TinyAngleAxisToQuaternionForJets) {
664 // Examine tiny x rotations that extend all the way to where
665 // underflow occurs.
666 for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) {
667 double theta = pow(10.0, i);
668 J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) };
669 J3 quaternion[4];
670 // To avoid loss of precision in the test itself,
671 // a finite expansion is used here, which will
672 // be exact up to machine precision for the test values used.
673 J3 expected[4] = {
674 MakeJ3(1.0, 0, 0, 0),
675 MakeJ3(0, 0.5, 0, 0),
676 MakeJ3(0, 0, 0.5, 0),
677 MakeJ3(0, 0, 0, 0.5),
678 };
679 AngleAxisToQuaternion(axis_angle, quaternion);
680 ExpectJetArraysClose<4, 3>(quaternion, expected);
681 }
682}
683
684// Test that derivatives are correct for zero rotation.
685TEST(Rotation, ZeroAngleAxisToQuaternionForJets) {
686 J3 axis_angle[3] = { J3(0, 0), J3(0, 1), J3(0, 2) };
687 J3 quaternion[4];
688 J3 expected[4] = {
689 MakeJ3(1.0, 0, 0, 0),
690 MakeJ3(0, 0.5, 0, 0),
691 MakeJ3(0, 0, 0.5, 0),
692 MakeJ3(0, 0, 0, 0.5),
693 };
694 AngleAxisToQuaternion(axis_angle, quaternion);
695 ExpectJetArraysClose<4, 3>(quaternion, expected);
696}
697
698// Test that exact conversion works for small angles.
699TEST(Rotation, SmallQuaternionToAngleAxisForJets) {
700 // Examine small x rotations that are still large enough
701 // to be well within the range represented by doubles.
702 for (int i = -2; i >= kSmallTinyCutoff; i--) {
703 double theta = pow(10.0, i);
704 double s = sin(theta);
705 double c = cos(theta);
706 J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) };
707 J4 axis_angle[3];
708 J4 expected[3] = {
709 MakeJ4(s, -2*theta, 2*theta*c, 0, 0),
710 MakeJ4(0, 0, 0, 2*theta/s, 0),
711 MakeJ4(0, 0, 0, 0, 2*theta/s),
712 };
713 QuaternionToAngleAxis(quaternion, axis_angle);
714 ExpectJetArraysClose<3, 4>(axis_angle, expected);
715 }
716}
717
718// Test that conversion works for very small angles.
719TEST(Rotation, TinyQuaternionToAngleAxisForJets) {
720 // Examine tiny x rotations that extend all the way to where
721 // underflow occurs.
722 for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) {
723 double theta = pow(10.0, i);
724 double s = sin(theta);
725 double c = cos(theta);
726 J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) };
727 J4 axis_angle[3];
728 // To avoid loss of precision in the test itself,
729 // a finite expansion is used here, which will
730 // be exact up to machine precision for the test values used.
731 J4 expected[3] = {
732 MakeJ4(theta, -2*theta, 2.0, 0, 0),
733 MakeJ4(0, 0, 0, 2.0, 0),
734 MakeJ4(0, 0, 0, 0, 2.0),
735 };
736 QuaternionToAngleAxis(quaternion, axis_angle);
737 ExpectJetArraysClose<3, 4>(axis_angle, expected);
738 }
739}
740
741// Test that conversion works for no rotation.
742TEST(Rotation, ZeroQuaternionToAngleAxisForJets) {
743 J4 quaternion[4] = { J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3) };
744 J4 axis_angle[3];
745 J4 expected[3] = {
746 MakeJ4(0, 0, 2.0, 0, 0),
747 MakeJ4(0, 0, 0, 2.0, 0),
748 MakeJ4(0, 0, 0, 0, 2.0),
749 };
750 QuaternionToAngleAxis(quaternion, axis_angle);
751 ExpectJetArraysClose<3, 4>(axis_angle, expected);
752}
753
754TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) {
755 // Canned data generated in octave.
756 double const q[4] = {
757 +0.1956830471754074,
758 -0.0150618562474847,
759 +0.7634572982788086,
760 -0.3019454777240753,
761 };
762 double const Q[3][3] = { // Scaled rotation matrix.
763 { -0.6355194033477252, 0.0951730541682254, 0.3078870197911186 },
764 { -0.1411693904792992, 0.5297609702153905, -0.4551502574482019 },
765 { -0.2896955822708862, -0.4669396571547050, -0.4536309793389248 },
766 };
767 double const R[3][3] = { // With unit rows and columns.
768 { -0.8918859164053080, 0.1335655625725649, 0.4320876677394745 },
769 { -0.1981166751680096, 0.7434648665444399, -0.6387564287225856 },
770 { -0.4065578619806013, -0.6553016349046693, -0.6366242786393164 },
771 };
772
773 // Compute R from q and compare to known answer.
774 double Rq[3][3];
775 QuaternionToScaledRotation<double>(q, Rq[0]);
776 ExpectArraysClose(9, Q[0], Rq[0], kTolerance);
777
778 // Now do the same but compute R with normalization.
779 QuaternionToRotation<double>(q, Rq[0]);
780 ExpectArraysClose(9, R[0], Rq[0], kTolerance);
781}
782
783
784TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) {
785 // Rotation defined by a unit quaternion.
786 double const q[4] = {
787 0.2318160216097109,
788 -0.0178430356832060,
789 0.9044300776717159,
790 -0.3576998641394597,
791 };
792 double const p[3] = {
793 +0.11,
794 -13.15,
795 1.17,
796 };
797
798 double R[3 * 3];
799 QuaternionToRotation(q, R);
800
801 double result1[3];
802 UnitQuaternionRotatePoint(q, p, result1);
803
804 double result2[3];
805 VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3)* ConstVectorRef(p, 3);
806 ExpectArraysClose(3, result1, result2, kTolerance);
807}
808
809
810// Verify that (a * b) * c == a * (b * c).
811TEST(Quaternion, MultiplicationIsAssociative) {
812 double a[4];
813 double b[4];
814 double c[4];
815 for (int i = 0; i < 4; ++i) {
816 a[i] = 2 * RandDouble() - 1;
817 b[i] = 2 * RandDouble() - 1;
818 c[i] = 2 * RandDouble() - 1;
819 }
820
821 double ab[4];
822 double ab_c[4];
823 QuaternionProduct(a, b, ab);
824 QuaternionProduct(ab, c, ab_c);
825
826 double bc[4];
827 double a_bc[4];
828 QuaternionProduct(b, c, bc);
829 QuaternionProduct(a, bc, a_bc);
830
831 ASSERT_NEAR(ab_c[0], a_bc[0], kTolerance);
832 ASSERT_NEAR(ab_c[1], a_bc[1], kTolerance);
833 ASSERT_NEAR(ab_c[2], a_bc[2], kTolerance);
834 ASSERT_NEAR(ab_c[3], a_bc[3], kTolerance);
835}
836
837
838TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) {
839 double angle_axis[3];
840 double R[9];
841 double p[3];
842 double angle_axis_rotated_p[3];
843 double rotation_matrix_rotated_p[3];
844
845 for (int i = 0; i < 10000; ++i) {
846 double theta = (2.0 * i * 0.0011 - 1.0) * M_PI;
847 for (int j = 0; j < 50; ++j) {
848 double norm2 = 0.0;
849 for (int k = 0; k < 3; ++k) {
850 angle_axis[k] = 2.0 * RandDouble() - 1.0;
851 p[k] = 2.0 * RandDouble() - 1.0;
852 norm2 = angle_axis[k] * angle_axis[k];
853 }
854
855 const double inv_norm = theta / sqrt(norm2);
856 for (int k = 0; k < 3; ++k) {
857 angle_axis[k] *= inv_norm;
858 }
859
860 AngleAxisToRotationMatrix(angle_axis, R);
861 rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2];
862 rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2];
863 rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2];
864
865 AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p);
866 for (int k = 0; k < 3; ++k) {
867 EXPECT_NEAR(rotation_matrix_rotated_p[k],
868 angle_axis_rotated_p[k],
869 kTolerance) << "p: " << p[0]
870 << " " << p[1]
871 << " " << p[2]
872 << " angle_axis: " << angle_axis[0]
873 << " " << angle_axis[1]
874 << " " << angle_axis[2];
875 }
876 }
877 }
878}
879
880TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) {
881 double angle_axis[3];
882 double R[9];
883 double p[3];
884 double angle_axis_rotated_p[3];
885 double rotation_matrix_rotated_p[3];
886
887 for (int i = 0; i < 10000; ++i) {
888 double norm2 = 0.0;
889 for (int k = 0; k < 3; ++k) {
890 angle_axis[k] = 2.0 * RandDouble() - 1.0;
891 p[k] = 2.0 * RandDouble() - 1.0;
892 norm2 = angle_axis[k] * angle_axis[k];
893 }
894
895 double theta = (2.0 * i * 0.0001 - 1.0) * 1e-16;
896 const double inv_norm = theta / sqrt(norm2);
897 for (int k = 0; k < 3; ++k) {
898 angle_axis[k] *= inv_norm;
899 }
900
901 AngleAxisToRotationMatrix(angle_axis, R);
902 rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2];
903 rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2];
904 rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2];
905
906 AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p);
907 for (int k = 0; k < 3; ++k) {
908 EXPECT_NEAR(rotation_matrix_rotated_p[k],
909 angle_axis_rotated_p[k],
910 kTolerance) << "p: " << p[0]
911 << " " << p[1]
912 << " " << p[2]
913 << " angle_axis: " << angle_axis[0]
914 << " " << angle_axis[1]
915 << " " << angle_axis[2];
916 }
917 }
918}
919
920
921} // namespace internal
922} // namespace ceres