1// 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#include "ceres/internal/eigen.h"
35#include "ceres/internal/port.h"
36#include "ceres/jet.h"
37#include "ceres/rotation.h"
38#include "ceres/stringprintf.h"
39#include "ceres/test_util.h"
40#include "glog/logging.h"
41#include "gmock/gmock.h"
42#include "gtest/gtest.h"
43
44namespace ceres {
45namespace internal {
46
47const double kPi = 3.14159265358979323846;
48const double kHalfSqrt2 = 0.707106781186547524401;
49
50double RandDouble() {
51  double r = rand();
52  return r / RAND_MAX;
53}
54
55// A tolerance value for floating-point comparisons.
56static double const kTolerance = numeric_limits<double>::epsilon() * 10;
57
58// Looser tolerance used for numerically unstable conversions.
59static double const kLooseTolerance = 1e-9;
60
61// Use as:
62// double quaternion[4];
63// EXPECT_THAT(quaternion, IsNormalizedQuaternion());
64MATCHER(IsNormalizedQuaternion, "") {
65  if (arg == NULL) {
66    *result_listener << "Null quaternion";
67    return false;
68  }
69
70  double norm2 = arg[0] * arg[0] + arg[1] * arg[1] +
71      arg[2] * arg[2] + arg[3] * arg[3];
72  if (fabs(norm2 - 1.0) > kTolerance) {
73    *result_listener << "squared norm is " << norm2;
74    return false;
75  }
76
77  return true;
78}
79
80// Use as:
81// double expected_quaternion[4];
82// double actual_quaternion[4];
83// EXPECT_THAT(actual_quaternion, IsNearQuaternion(expected_quaternion));
84MATCHER_P(IsNearQuaternion, expected, "") {
85  if (arg == NULL) {
86    *result_listener << "Null quaternion";
87    return false;
88  }
89
90  // Quaternions are equivalent upto a sign change. So we will compare
91  // both signs before declaring failure.
92  bool near = true;
93  for (int i = 0; i < 4; i++) {
94    if (fabs(arg[i] - expected[i]) > kTolerance) {
95      near = false;
96      break;
97    }
98  }
99
100  if (near) {
101    return true;
102  }
103
104  near = true;
105  for (int i = 0; i < 4; i++) {
106    if (fabs(arg[i] + expected[i]) > kTolerance) {
107      near = false;
108      break;
109    }
110  }
111
112  if (near) {
113    return true;
114  }
115
116  *result_listener << "expected : "
117                   << expected[0] << " "
118                   << expected[1] << " "
119                   << expected[2] << " "
120                   << expected[3] << " "
121                   << "actual : "
122                   << arg[0] << " "
123                   << arg[1] << " "
124                   << arg[2] << " "
125                   << arg[3];
126  return false;
127}
128
129// Use as:
130// double expected_axis_angle[4];
131// double actual_axis_angle[4];
132// EXPECT_THAT(actual_axis_angle, IsNearAngleAxis(expected_axis_angle));
133MATCHER_P(IsNearAngleAxis, expected, "") {
134  if (arg == NULL) {
135    *result_listener << "Null axis/angle";
136    return false;
137  }
138
139  for (int i = 0; i < 3; i++) {
140    if (fabs(arg[i] - expected[i]) > kTolerance) {
141      *result_listener << "component " << i << " should be " << expected[i];
142      return false;
143    }
144  }
145
146  return true;
147}
148
149// Use as:
150// double matrix[9];
151// EXPECT_THAT(matrix, IsOrthonormal());
152MATCHER(IsOrthonormal, "") {
153  if (arg == NULL) {
154    *result_listener << "Null matrix";
155    return false;
156  }
157
158  for (int c1 = 0; c1 < 3; c1++) {
159    for (int c2 = 0; c2 < 3; c2++) {
160      double v = 0;
161      for (int i = 0; i < 3; i++) {
162        v += arg[i + 3 * c1] * arg[i + 3 * c2];
163      }
164      double expected = (c1 == c2) ? 1 : 0;
165      if (fabs(expected - v) > kTolerance) {
166        *result_listener << "Columns " << c1 << " and " << c2
167                         << " should have dot product " << expected
168                         << " but have " << v;
169        return false;
170      }
171    }
172  }
173
174  return true;
175}
176
177// Use as:
178// double matrix1[9];
179// double matrix2[9];
180// EXPECT_THAT(matrix1, IsNear3x3Matrix(matrix2));
181MATCHER_P(IsNear3x3Matrix, expected, "") {
182  if (arg == NULL) {
183    *result_listener << "Null matrix";
184    return false;
185  }
186
187  for (int i = 0; i < 9; i++) {
188    if (fabs(arg[i] - expected[i]) > kTolerance) {
189      *result_listener << "component " << i << " should be " << expected[i];
190      return false;
191    }
192  }
193
194  return true;
195}
196
197// Transforms a zero axis/angle to a quaternion.
198TEST(Rotation, ZeroAngleAxisToQuaternion) {
199  double axis_angle[3] = { 0, 0, 0 };
200  double quaternion[4];
201  double expected[4] = { 1, 0, 0, 0 };
202  AngleAxisToQuaternion(axis_angle, quaternion);
203  EXPECT_THAT(quaternion, IsNormalizedQuaternion());
204  EXPECT_THAT(quaternion, IsNearQuaternion(expected));
205}
206
207// Test that exact conversion works for small angles.
208TEST(Rotation, SmallAngleAxisToQuaternion) {
209  // Small, finite value to test.
210  double theta = 1.0e-2;
211  double axis_angle[3] = { theta, 0, 0 };
212  double quaternion[4];
213  double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
214  AngleAxisToQuaternion(axis_angle, quaternion);
215  EXPECT_THAT(quaternion, IsNormalizedQuaternion());
216  EXPECT_THAT(quaternion, IsNearQuaternion(expected));
217}
218
219// Test that approximate conversion works for very small angles.
220TEST(Rotation, TinyAngleAxisToQuaternion) {
221  // Very small value that could potentially cause underflow.
222  double theta = pow(numeric_limits<double>::min(), 0.75);
223  double axis_angle[3] = { theta, 0, 0 };
224  double quaternion[4];
225  double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
226  AngleAxisToQuaternion(axis_angle, quaternion);
227  EXPECT_THAT(quaternion, IsNormalizedQuaternion());
228  EXPECT_THAT(quaternion, IsNearQuaternion(expected));
229}
230
231// Transforms a rotation by pi/2 around X to a quaternion.
232TEST(Rotation, XRotationToQuaternion) {
233  double axis_angle[3] = { kPi / 2, 0, 0 };
234  double quaternion[4];
235  double expected[4] = { kHalfSqrt2, kHalfSqrt2, 0, 0 };
236  AngleAxisToQuaternion(axis_angle, quaternion);
237  EXPECT_THAT(quaternion, IsNormalizedQuaternion());
238  EXPECT_THAT(quaternion, IsNearQuaternion(expected));
239}
240
241// Transforms a unit quaternion to an axis angle.
242TEST(Rotation, UnitQuaternionToAngleAxis) {
243  double quaternion[4] = { 1, 0, 0, 0 };
244  double axis_angle[3];
245  double expected[3] = { 0, 0, 0 };
246  QuaternionToAngleAxis(quaternion, axis_angle);
247  EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
248}
249
250// Transforms a quaternion that rotates by pi about the Y axis to an axis angle.
251TEST(Rotation, YRotationQuaternionToAngleAxis) {
252  double quaternion[4] = { 0, 0, 1, 0 };
253  double axis_angle[3];
254  double expected[3] = { 0, kPi, 0 };
255  QuaternionToAngleAxis(quaternion, axis_angle);
256  EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
257}
258
259// Transforms a quaternion that rotates by pi/3 about the Z axis to an axis
260// angle.
261TEST(Rotation, ZRotationQuaternionToAngleAxis) {
262  double quaternion[4] = { sqrt(3) / 2, 0, 0, 0.5 };
263  double axis_angle[3];
264  double expected[3] = { 0, 0, kPi / 3 };
265  QuaternionToAngleAxis(quaternion, axis_angle);
266  EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
267}
268
269// Test that exact conversion works for small angles.
270TEST(Rotation, SmallQuaternionToAngleAxis) {
271  // Small, finite value to test.
272  double theta = 1.0e-2;
273  double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
274  double axis_angle[3];
275  double expected[3] = { theta, 0, 0 };
276  QuaternionToAngleAxis(quaternion, axis_angle);
277  EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
278}
279
280// Test that approximate conversion works for very small angles.
281TEST(Rotation, TinyQuaternionToAngleAxis) {
282  // Very small value that could potentially cause underflow.
283  double theta = pow(numeric_limits<double>::min(), 0.75);
284  double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 };
285  double axis_angle[3];
286  double expected[3] = { theta, 0, 0 };
287  QuaternionToAngleAxis(quaternion, axis_angle);
288  EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
289}
290
291TEST(Rotation, QuaternionToAngleAxisAngleIsLessThanPi) {
292  double quaternion[4];
293  double angle_axis[3];
294
295  const double half_theta = 0.75 * kPi;
296
297  quaternion[0] = cos(half_theta);
298  quaternion[1] = 1.0 * sin(half_theta);
299  quaternion[2] = 0.0;
300  quaternion[3] = 0.0;
301  QuaternionToAngleAxis(quaternion, angle_axis);
302  const double angle = sqrt(angle_axis[0] * angle_axis[0] +
303                            angle_axis[1] * angle_axis[1] +
304                            angle_axis[2] * angle_axis[2]);
305  EXPECT_LE(angle, kPi);
306}
307
308static const int kNumTrials = 10000;
309
310// Takes a bunch of random axis/angle values, converts them to quaternions,
311// and back again.
312TEST(Rotation, AngleAxisToQuaterionAndBack) {
313  srand(5);
314  for (int i = 0; i < kNumTrials; i++) {
315    double axis_angle[3];
316    // Make an axis by choosing three random numbers in [-1, 1) and
317    // normalizing.
318    double norm = 0;
319    for (int i = 0; i < 3; i++) {
320      axis_angle[i] = RandDouble() * 2 - 1;
321      norm += axis_angle[i] * axis_angle[i];
322    }
323    norm = sqrt(norm);
324
325    // Angle in [-pi, pi).
326    double theta = kPi * 2 * RandDouble() - kPi;
327    for (int i = 0; i < 3; i++) {
328      axis_angle[i] = axis_angle[i] * theta / norm;
329    }
330
331    double quaternion[4];
332    double round_trip[3];
333    // We use ASSERTs here because if there's one failure, there are
334    // probably many and spewing a million failures doesn't make anyone's
335    // day.
336    AngleAxisToQuaternion(axis_angle, quaternion);
337    ASSERT_THAT(quaternion, IsNormalizedQuaternion());
338    QuaternionToAngleAxis(quaternion, round_trip);
339    ASSERT_THAT(round_trip, IsNearAngleAxis(axis_angle));
340  }
341}
342
343// Takes a bunch of random quaternions, converts them to axis/angle,
344// and back again.
345TEST(Rotation, QuaterionToAngleAxisAndBack) {
346  srand(5);
347  for (int i = 0; i < kNumTrials; i++) {
348    double quaternion[4];
349    // Choose four random numbers in [-1, 1) and normalize.
350    double norm = 0;
351    for (int i = 0; i < 4; i++) {
352      quaternion[i] = RandDouble() * 2 - 1;
353      norm += quaternion[i] * quaternion[i];
354    }
355    norm = sqrt(norm);
356
357    for (int i = 0; i < 4; i++) {
358      quaternion[i] = quaternion[i] / norm;
359    }
360
361    double axis_angle[3];
362    double round_trip[4];
363    QuaternionToAngleAxis(quaternion, axis_angle);
364    AngleAxisToQuaternion(axis_angle, round_trip);
365    ASSERT_THAT(round_trip, IsNormalizedQuaternion());
366    ASSERT_THAT(round_trip, IsNearQuaternion(quaternion));
367  }
368}
369
370// Transforms a zero axis/angle to a rotation matrix.
371TEST(Rotation, ZeroAngleAxisToRotationMatrix) {
372  double axis_angle[3] = { 0, 0, 0 };
373  double matrix[9];
374  double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
375  AngleAxisToRotationMatrix(axis_angle, matrix);
376  EXPECT_THAT(matrix, IsOrthonormal());
377  EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
378}
379
380TEST(Rotation, NearZeroAngleAxisToRotationMatrix) {
381  double axis_angle[3] = { 1e-24, 2e-24, 3e-24 };
382  double matrix[9];
383  double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
384  AngleAxisToRotationMatrix(axis_angle, matrix);
385  EXPECT_THAT(matrix, IsOrthonormal());
386  EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
387}
388
389// Transforms a rotation by pi/2 around X to a rotation matrix and back.
390TEST(Rotation, XRotationToRotationMatrix) {
391  double axis_angle[3] = { kPi / 2, 0, 0 };
392  double matrix[9];
393  // The rotation matrices are stored column-major.
394  double expected[9] = { 1, 0, 0, 0, 0, 1, 0, -1, 0 };
395  AngleAxisToRotationMatrix(axis_angle, matrix);
396  EXPECT_THAT(matrix, IsOrthonormal());
397  EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
398  double round_trip[3];
399  RotationMatrixToAngleAxis(matrix, round_trip);
400  EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle));
401}
402
403// Transforms an axis angle that rotates by pi about the Y axis to a
404// rotation matrix and back.
405TEST(Rotation, YRotationToRotationMatrix) {
406  double axis_angle[3] = { 0, kPi, 0 };
407  double matrix[9];
408  double expected[9] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 };
409  AngleAxisToRotationMatrix(axis_angle, matrix);
410  EXPECT_THAT(matrix, IsOrthonormal());
411  EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
412
413  double round_trip[3];
414  RotationMatrixToAngleAxis(matrix, round_trip);
415  EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle));
416}
417
418TEST(Rotation, NearPiAngleAxisRoundTrip) {
419  double in_axis_angle[3];
420  double matrix[9];
421  double out_axis_angle[3];
422
423  srand(5);
424  for (int i = 0; i < kNumTrials; i++) {
425    // Make an axis by choosing three random numbers in [-1, 1) and
426    // normalizing.
427    double norm = 0;
428    for (int i = 0; i < 3; i++) {
429      in_axis_angle[i] = RandDouble() * 2 - 1;
430      norm += in_axis_angle[i] * in_axis_angle[i];
431    }
432    norm = sqrt(norm);
433
434    // Angle in [pi - kMaxSmallAngle, pi).
435    const double kMaxSmallAngle = 1e-2;
436    double theta = kPi - kMaxSmallAngle * RandDouble();
437
438    for (int i = 0; i < 3; i++) {
439      in_axis_angle[i] *= (theta / norm);
440    }
441    AngleAxisToRotationMatrix(in_axis_angle, matrix);
442    RotationMatrixToAngleAxis(matrix, out_axis_angle);
443
444    for (int i = 0; i < 3; ++i) {
445      EXPECT_NEAR(out_axis_angle[i], in_axis_angle[i], kLooseTolerance);
446    }
447  }
448}
449
450TEST(Rotation, AtPiAngleAxisRoundTrip) {
451  // A rotation of kPi about the X axis;
452  static const double kMatrix[3][3] = {
453    {1.0,  0.0,  0.0},
454    {0.0,  -1.0,  0.0},
455    {0.0,  0.0,  -1.0}
456  };
457
458  double in_matrix[9];
459  // Fill it from kMatrix in col-major order.
460  for (int j = 0, k = 0; j < 3; ++j) {
461     for (int i = 0; i < 3; ++i, ++k) {
462       in_matrix[k] = kMatrix[i][j];
463     }
464  }
465
466  const double expected_axis_angle[3] = { kPi, 0, 0 };
467
468  double out_matrix[9];
469  double axis_angle[3];
470  RotationMatrixToAngleAxis(in_matrix, axis_angle);
471  AngleAxisToRotationMatrix(axis_angle, out_matrix);
472
473  LOG(INFO) << "AngleAxis = " << axis_angle[0] << " " << axis_angle[1]
474            << " " << axis_angle[2];
475  LOG(INFO) << "Expected AngleAxis = " << kPi << " 0 0";
476  double out_rowmajor[3][3];
477  for (int j = 0, k = 0; j < 3; ++j) {
478    for (int i = 0; i < 3; ++i, ++k) {
479      out_rowmajor[i][j] = out_matrix[k];
480    }
481  }
482  LOG(INFO) << "Rotation:";
483  LOG(INFO) << "EXPECTED      |        ACTUAL";
484  for (int i = 0; i < 3; ++i) {
485    string line;
486    for (int j = 0; j < 3; ++j) {
487      StringAppendF(&line, "%g ", kMatrix[i][j]);
488    }
489    line += "         |        ";
490    for (int j = 0; j < 3; ++j) {
491      StringAppendF(&line, "%g ", out_rowmajor[i][j]);
492    }
493    LOG(INFO) << line;
494  }
495
496  EXPECT_THAT(axis_angle, IsNearAngleAxis(expected_axis_angle));
497  EXPECT_THAT(out_matrix, IsNear3x3Matrix(in_matrix));
498}
499
500// Transforms an axis angle that rotates by pi/3 about the Z axis to a
501// rotation matrix.
502TEST(Rotation, ZRotationToRotationMatrix) {
503  double axis_angle[3] =  { 0, 0, kPi / 3 };
504  double matrix[9];
505  // This is laid-out row-major on the screen but is actually stored
506  // column-major.
507  double expected[9] = { 0.5, sqrt(3) / 2, 0,   // Column 1
508                         -sqrt(3) / 2, 0.5, 0,  // Column 2
509                         0, 0, 1 };             // Column 3
510  AngleAxisToRotationMatrix(axis_angle, matrix);
511  EXPECT_THAT(matrix, IsOrthonormal());
512  EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
513  double round_trip[3];
514  RotationMatrixToAngleAxis(matrix, round_trip);
515  EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle));
516}
517
518// Takes a bunch of random axis/angle values, converts them to rotation
519// matrices, and back again.
520TEST(Rotation, AngleAxisToRotationMatrixAndBack) {
521  srand(5);
522  for (int i = 0; i < kNumTrials; i++) {
523    double axis_angle[3];
524    // Make an axis by choosing three random numbers in [-1, 1) and
525    // normalizing.
526    double norm = 0;
527    for (int i = 0; i < 3; i++) {
528      axis_angle[i] = RandDouble() * 2 - 1;
529      norm += axis_angle[i] * axis_angle[i];
530    }
531    norm = sqrt(norm);
532
533    // Angle in [-pi, pi).
534    double theta = kPi * 2 * RandDouble() - kPi;
535    for (int i = 0; i < 3; i++) {
536      axis_angle[i] = axis_angle[i] * theta / norm;
537    }
538
539    double matrix[9];
540    double round_trip[3];
541    AngleAxisToRotationMatrix(axis_angle, matrix);
542    ASSERT_THAT(matrix, IsOrthonormal());
543    RotationMatrixToAngleAxis(matrix, round_trip);
544
545    for (int i = 0; i < 3; ++i) {
546      EXPECT_NEAR(round_trip[i], axis_angle[i], kLooseTolerance);
547    }
548  }
549}
550
551// Takes a bunch of random axis/angle values near zero, converts them
552// to rotation matrices, and back again.
553TEST(Rotation, AngleAxisToRotationMatrixAndBackNearZero) {
554  srand(5);
555  for (int i = 0; i < kNumTrials; i++) {
556    double axis_angle[3];
557    // Make an axis by choosing three random numbers in [-1, 1) and
558    // normalizing.
559    double norm = 0;
560    for (int i = 0; i < 3; i++) {
561      axis_angle[i] = RandDouble() * 2 - 1;
562      norm += axis_angle[i] * axis_angle[i];
563    }
564    norm = sqrt(norm);
565
566    // Tiny theta.
567    double theta = 1e-16 * (kPi * 2 * RandDouble() - kPi);
568    for (int i = 0; i < 3; i++) {
569      axis_angle[i] = axis_angle[i] * theta / norm;
570    }
571
572    double matrix[9];
573    double round_trip[3];
574    AngleAxisToRotationMatrix(axis_angle, matrix);
575    ASSERT_THAT(matrix, IsOrthonormal());
576    RotationMatrixToAngleAxis(matrix, round_trip);
577
578    for (int i = 0; i < 3; ++i) {
579      EXPECT_NEAR(round_trip[i], axis_angle[i],
580                  std::numeric_limits<double>::epsilon());
581    }
582  }
583}
584
585
586// Transposes a 3x3 matrix.
587static void Transpose3x3(double m[9]) {
588  std::swap(m[1], m[3]);
589  std::swap(m[2], m[6]);
590  std::swap(m[5], m[7]);
591}
592
593// Convert Euler angles from radians to degrees.
594static void ToDegrees(double ea[3]) {
595  for (int i = 0; i < 3; ++i)
596    ea[i] *= 180.0 / kPi;
597}
598
599// Compare the 3x3 rotation matrices produced by the axis-angle
600// rotation 'aa' and the Euler angle rotation 'ea' (in radians).
601static void CompareEulerToAngleAxis(double aa[3], double ea[3]) {
602  double aa_matrix[9];
603  AngleAxisToRotationMatrix(aa, aa_matrix);
604  Transpose3x3(aa_matrix);  // Column to row major order.
605
606  double ea_matrix[9];
607  ToDegrees(ea);  // Radians to degrees.
608  const int kRowStride = 3;
609  EulerAnglesToRotationMatrix(ea, kRowStride, ea_matrix);
610
611  EXPECT_THAT(aa_matrix, IsOrthonormal());
612  EXPECT_THAT(ea_matrix, IsOrthonormal());
613  EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix));
614}
615
616// Test with rotation axis along the x/y/z axes.
617// Also test zero rotation.
618TEST(EulerAnglesToRotationMatrix, OnAxis) {
619  int n_tests = 0;
620  for (double x = -1.0; x <= 1.0; x += 1.0) {
621    for (double y = -1.0; y <= 1.0; y += 1.0) {
622      for (double z = -1.0; z <= 1.0; z += 1.0) {
623        if ((x != 0) + (y != 0) + (z != 0) > 1)
624          continue;
625        double axis_angle[3] = {x, y, z};
626        double euler_angles[3] = {x, y, z};
627        CompareEulerToAngleAxis(axis_angle, euler_angles);
628        ++n_tests;
629      }
630    }
631  }
632  CHECK_EQ(7, n_tests);
633}
634
635// Test that a random rotation produces an orthonormal rotation
636// matrix.
637TEST(EulerAnglesToRotationMatrix, IsOrthonormal) {
638  srand(5);
639  for (int trial = 0; trial < kNumTrials; ++trial) {
640    double ea[3];
641    for (int i = 0; i < 3; ++i)
642      ea[i] = 360.0 * (RandDouble() * 2.0 - 1.0);
643    double ea_matrix[9];
644    ToDegrees(ea);  // Radians to degrees.
645    EulerAnglesToRotationMatrix(ea, 3, ea_matrix);
646    EXPECT_THAT(ea_matrix, IsOrthonormal());
647  }
648}
649
650// Tests using Jets for specific behavior involving auto differentiation
651// near singularity points.
652
653typedef Jet<double, 3> J3;
654typedef Jet<double, 4> J4;
655
656J3 MakeJ3(double a, double v0, double v1, double v2) {
657  J3 j;
658  j.a = a;
659  j.v[0] = v0;
660  j.v[1] = v1;
661  j.v[2] = v2;
662  return j;
663}
664
665J4 MakeJ4(double a, double v0, double v1, double v2, double v3) {
666  J4 j;
667  j.a = a;
668  j.v[0] = v0;
669  j.v[1] = v1;
670  j.v[2] = v2;
671  j.v[3] = v3;
672  return j;
673}
674
675
676bool IsClose(double x, double y) {
677  EXPECT_FALSE(IsNaN(x));
678  EXPECT_FALSE(IsNaN(y));
679  double absdiff = fabs(x - y);
680  if (x == 0 || y == 0) {
681    return absdiff <= kTolerance;
682  }
683  double reldiff = absdiff / max(fabs(x), fabs(y));
684  return reldiff <= kTolerance;
685}
686
687template <int N>
688bool IsClose(const Jet<double, N> &x, const Jet<double, N> &y) {
689  if (IsClose(x.a, y.a)) {
690    for (int i = 0; i < N; i++) {
691      if (!IsClose(x.v[i], y.v[i])) {
692        return false;
693      }
694    }
695  }
696  return true;
697}
698
699template <int M, int N>
700void ExpectJetArraysClose(const Jet<double, N> *x, const Jet<double, N> *y) {
701  for (int i = 0; i < M; i++) {
702    if (!IsClose(x[i], y[i])) {
703      LOG(ERROR) << "Jet " << i << "/" << M << " not equal";
704      LOG(ERROR) << "x[" << i << "]: " << x[i];
705      LOG(ERROR) << "y[" << i << "]: " << y[i];
706      Jet<double, N> d, zero;
707      d.a = y[i].a - x[i].a;
708      for (int j = 0; j < N; j++) {
709        d.v[j] = y[i].v[j] - x[i].v[j];
710      }
711      LOG(ERROR) << "diff: " << d;
712      EXPECT_TRUE(IsClose(x[i], y[i]));
713    }
714  }
715}
716
717// Log-10 of a value well below machine precision.
718static const int kSmallTinyCutoff =
719    static_cast<int>(2 * log(numeric_limits<double>::epsilon())/log(10.0));
720
721// Log-10 of a value just below values representable by double.
722static const int kTinyZeroLimit   =
723    static_cast<int>(1 + log(numeric_limits<double>::min())/log(10.0));
724
725// Test that exact conversion works for small angles when jets are used.
726TEST(Rotation, SmallAngleAxisToQuaternionForJets) {
727  // Examine small x rotations that are still large enough
728  // to be well within the range represented by doubles.
729  for (int i = -2; i >= kSmallTinyCutoff; i--) {
730    double theta = pow(10.0, i);
731    J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) };
732    J3 quaternion[4];
733    J3 expected[4] = {
734        MakeJ3(cos(theta/2), -sin(theta/2)/2, 0, 0),
735        MakeJ3(sin(theta/2), cos(theta/2)/2, 0, 0),
736        MakeJ3(0, 0, sin(theta/2)/theta, 0),
737        MakeJ3(0, 0, 0, sin(theta/2)/theta),
738    };
739    AngleAxisToQuaternion(axis_angle, quaternion);
740    ExpectJetArraysClose<4, 3>(quaternion, expected);
741  }
742}
743
744
745// Test that conversion works for very small angles when jets are used.
746TEST(Rotation, TinyAngleAxisToQuaternionForJets) {
747  // Examine tiny x rotations that extend all the way to where
748  // underflow occurs.
749  for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) {
750    double theta = pow(10.0, i);
751    J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) };
752    J3 quaternion[4];
753    // To avoid loss of precision in the test itself,
754    // a finite expansion is used here, which will
755    // be exact up to machine precision for the test values used.
756    J3 expected[4] = {
757        MakeJ3(1.0, 0, 0, 0),
758        MakeJ3(0, 0.5, 0, 0),
759        MakeJ3(0, 0, 0.5, 0),
760        MakeJ3(0, 0, 0, 0.5),
761    };
762    AngleAxisToQuaternion(axis_angle, quaternion);
763    ExpectJetArraysClose<4, 3>(quaternion, expected);
764  }
765}
766
767// Test that derivatives are correct for zero rotation.
768TEST(Rotation, ZeroAngleAxisToQuaternionForJets) {
769  J3 axis_angle[3] = { J3(0, 0), J3(0, 1), J3(0, 2) };
770  J3 quaternion[4];
771  J3 expected[4] = {
772      MakeJ3(1.0, 0, 0, 0),
773      MakeJ3(0, 0.5, 0, 0),
774      MakeJ3(0, 0, 0.5, 0),
775      MakeJ3(0, 0, 0, 0.5),
776  };
777  AngleAxisToQuaternion(axis_angle, quaternion);
778  ExpectJetArraysClose<4, 3>(quaternion, expected);
779}
780
781// Test that exact conversion works for small angles.
782TEST(Rotation, SmallQuaternionToAngleAxisForJets) {
783  // Examine small x rotations that are still large enough
784  // to be well within the range represented by doubles.
785  for (int i = -2; i >= kSmallTinyCutoff; i--) {
786    double theta = pow(10.0, i);
787    double s = sin(theta);
788    double c = cos(theta);
789    J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) };
790    J4 axis_angle[3];
791    J4 expected[3] = {
792        MakeJ4(s, -2*theta, 2*theta*c, 0, 0),
793        MakeJ4(0, 0, 0, 2*theta/s, 0),
794        MakeJ4(0, 0, 0, 0, 2*theta/s),
795    };
796    QuaternionToAngleAxis(quaternion, axis_angle);
797    ExpectJetArraysClose<3, 4>(axis_angle, expected);
798  }
799}
800
801// Test that conversion works for very small angles.
802TEST(Rotation, TinyQuaternionToAngleAxisForJets) {
803  // Examine tiny x rotations that extend all the way to where
804  // underflow occurs.
805  for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) {
806    double theta = pow(10.0, i);
807    double s = sin(theta);
808    double c = cos(theta);
809    J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) };
810    J4 axis_angle[3];
811    // To avoid loss of precision in the test itself,
812    // a finite expansion is used here, which will
813    // be exact up to machine precision for the test values used.
814    J4 expected[3] = {
815        MakeJ4(theta, -2*theta, 2.0, 0, 0),
816        MakeJ4(0, 0, 0, 2.0, 0),
817        MakeJ4(0, 0, 0, 0, 2.0),
818    };
819    QuaternionToAngleAxis(quaternion, axis_angle);
820    ExpectJetArraysClose<3, 4>(axis_angle, expected);
821  }
822}
823
824// Test that conversion works for no rotation.
825TEST(Rotation, ZeroQuaternionToAngleAxisForJets) {
826  J4 quaternion[4] = { J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3) };
827  J4 axis_angle[3];
828  J4 expected[3] = {
829      MakeJ4(0, 0, 2.0, 0, 0),
830      MakeJ4(0, 0, 0, 2.0, 0),
831      MakeJ4(0, 0, 0, 0, 2.0),
832  };
833  QuaternionToAngleAxis(quaternion, axis_angle);
834  ExpectJetArraysClose<3, 4>(axis_angle, expected);
835}
836
837TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) {
838  // Canned data generated in octave.
839  double const q[4] = {
840    +0.1956830471754074,
841    -0.0150618562474847,
842    +0.7634572982788086,
843    -0.3019454777240753,
844  };
845  double const Q[3][3] = {  // Scaled rotation matrix.
846    { -0.6355194033477252,  0.0951730541682254,  0.3078870197911186 },
847    { -0.1411693904792992,  0.5297609702153905, -0.4551502574482019 },
848    { -0.2896955822708862, -0.4669396571547050, -0.4536309793389248 },
849  };
850  double const R[3][3] = {  // With unit rows and columns.
851    { -0.8918859164053080,  0.1335655625725649,  0.4320876677394745 },
852    { -0.1981166751680096,  0.7434648665444399, -0.6387564287225856 },
853    { -0.4065578619806013, -0.6553016349046693, -0.6366242786393164 },
854  };
855
856  // Compute R from q and compare to known answer.
857  double Rq[3][3];
858  QuaternionToScaledRotation<double>(q, Rq[0]);
859  ExpectArraysClose(9, Q[0], Rq[0], kTolerance);
860
861  // Now do the same but compute R with normalization.
862  QuaternionToRotation<double>(q, Rq[0]);
863  ExpectArraysClose(9, R[0], Rq[0], kTolerance);
864}
865
866
867TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) {
868  // Rotation defined by a unit quaternion.
869  double const q[4] = {
870    0.2318160216097109,
871    -0.0178430356832060,
872    0.9044300776717159,
873    -0.3576998641394597,
874  };
875  double const p[3] = {
876    +0.11,
877    -13.15,
878    1.17,
879  };
880
881  double R[3 * 3];
882  QuaternionToRotation(q, R);
883
884  double result1[3];
885  UnitQuaternionRotatePoint(q, p, result1);
886
887  double result2[3];
888  VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3)* ConstVectorRef(p, 3);
889  ExpectArraysClose(3, result1, result2, kTolerance);
890}
891
892
893// Verify that (a * b) * c == a * (b * c).
894TEST(Quaternion, MultiplicationIsAssociative) {
895  double a[4];
896  double b[4];
897  double c[4];
898  for (int i = 0; i < 4; ++i) {
899    a[i] = 2 * RandDouble() - 1;
900    b[i] = 2 * RandDouble() - 1;
901    c[i] = 2 * RandDouble() - 1;
902  }
903
904  double ab[4];
905  double ab_c[4];
906  QuaternionProduct(a, b, ab);
907  QuaternionProduct(ab, c, ab_c);
908
909  double bc[4];
910  double a_bc[4];
911  QuaternionProduct(b, c, bc);
912  QuaternionProduct(a, bc, a_bc);
913
914  ASSERT_NEAR(ab_c[0], a_bc[0], kTolerance);
915  ASSERT_NEAR(ab_c[1], a_bc[1], kTolerance);
916  ASSERT_NEAR(ab_c[2], a_bc[2], kTolerance);
917  ASSERT_NEAR(ab_c[3], a_bc[3], kTolerance);
918}
919
920
921TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) {
922  double angle_axis[3];
923  double R[9];
924  double p[3];
925  double angle_axis_rotated_p[3];
926  double rotation_matrix_rotated_p[3];
927
928  for (int i = 0; i < 10000; ++i) {
929    double theta = (2.0 * i * 0.0011 - 1.0) * kPi;
930    for (int j = 0; j < 50; ++j) {
931      double norm2 = 0.0;
932      for (int k = 0; k < 3; ++k) {
933        angle_axis[k] = 2.0 * RandDouble() - 1.0;
934        p[k] = 2.0 * RandDouble() - 1.0;
935        norm2 = angle_axis[k] * angle_axis[k];
936      }
937
938      const double inv_norm = theta / sqrt(norm2);
939      for (int k = 0; k < 3; ++k) {
940        angle_axis[k] *= inv_norm;
941      }
942
943      AngleAxisToRotationMatrix(angle_axis, R);
944      rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2];
945      rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2];
946      rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2];
947
948      AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p);
949      for (int k = 0; k < 3; ++k) {
950        EXPECT_NEAR(rotation_matrix_rotated_p[k],
951                    angle_axis_rotated_p[k],
952                    kTolerance) << "p: " << p[0]
953                                << " " << p[1]
954                                << " " << p[2]
955                                << " angle_axis: " << angle_axis[0]
956                                << " " << angle_axis[1]
957                                << " " << angle_axis[2];
958      }
959    }
960  }
961}
962
963TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) {
964  double angle_axis[3];
965  double R[9];
966  double p[3];
967  double angle_axis_rotated_p[3];
968  double rotation_matrix_rotated_p[3];
969
970  for (int i = 0; i < 10000; ++i) {
971    double norm2 = 0.0;
972    for (int k = 0; k < 3; ++k) {
973      angle_axis[k] = 2.0 * RandDouble() - 1.0;
974      p[k] = 2.0 * RandDouble() - 1.0;
975      norm2 = angle_axis[k] * angle_axis[k];
976    }
977
978    double theta = (2.0 * i * 0.0001  - 1.0) * 1e-16;
979    const double inv_norm = theta / sqrt(norm2);
980    for (int k = 0; k < 3; ++k) {
981      angle_axis[k] *= inv_norm;
982    }
983
984    AngleAxisToRotationMatrix(angle_axis, R);
985    rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2];
986    rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2];
987    rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2];
988
989    AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p);
990    for (int k = 0; k < 3; ++k) {
991      EXPECT_NEAR(rotation_matrix_rotated_p[k],
992                  angle_axis_rotated_p[k],
993                  kTolerance) << "p: " << p[0]
994                              << " " << p[1]
995                              << " " << p[2]
996                              << " angle_axis: " << angle_axis[0]
997                              << " " << angle_axis[1]
998                              << " " << angle_axis[2];
999    }
1000  }
1001}
1002
1003TEST(MatrixAdapter, RowMajor3x3ReturnTypeAndAccessIsCorrect) {
1004  double array[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 };
1005  const float const_array[9] =
1006      { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f };
1007  MatrixAdapter<double, 3, 1> A = RowMajorAdapter3x3(array);
1008  MatrixAdapter<const float, 3, 1> B = RowMajorAdapter3x3(const_array);
1009
1010  for (int i = 0; i < 3; ++i) {
1011    for (int j = 0; j < 3; ++j) {
1012      // The values are integers from 1 to 9, so equality tests are appropriate
1013      // even for float and double values.
1014      EXPECT_EQ(A(i, j), array[3*i+j]);
1015      EXPECT_EQ(B(i, j), const_array[3*i+j]);
1016    }
1017  }
1018}
1019
1020TEST(MatrixAdapter, ColumnMajor3x3ReturnTypeAndAccessIsCorrect) {
1021  double array[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 };
1022  const float const_array[9] =
1023      { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f };
1024  MatrixAdapter<double, 1, 3> A = ColumnMajorAdapter3x3(array);
1025  MatrixAdapter<const float, 1, 3> B = ColumnMajorAdapter3x3(const_array);
1026
1027  for (int i = 0; i < 3; ++i) {
1028    for (int j = 0; j < 3; ++j) {
1029      // The values are integers from 1 to 9, so equality tests are
1030      // appropriate even for float and double values.
1031      EXPECT_EQ(A(i, j), array[3*j+i]);
1032      EXPECT_EQ(B(i, j), const_array[3*j+i]);
1033    }
1034  }
1035}
1036
1037TEST(MatrixAdapter, RowMajor2x4IsCorrect) {
1038  const int expected[8] = { 1, 2, 3, 4, 5, 6, 7, 8 };
1039  int array[8];
1040  MatrixAdapter<int, 4, 1> M(array);
1041  M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4;
1042  M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8;
1043  for (int k = 0; k < 8; ++k) {
1044    EXPECT_EQ(array[k], expected[k]);
1045  }
1046}
1047
1048TEST(MatrixAdapter, ColumnMajor2x4IsCorrect) {
1049  const int expected[8] = { 1, 5, 2, 6, 3, 7, 4, 8 };
1050  int array[8];
1051  MatrixAdapter<int, 1, 2> M(array);
1052  M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4;
1053  M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8;
1054  for (int k = 0; k < 8; ++k) {
1055    EXPECT_EQ(array[k], expected[k]);
1056  }
1057}
1058
1059}  // namespace internal
1060}  // namespace ceres
1061