Implement tests for Euler conversion with jets

https://github.com/ceres-solver/ceres-solver/issues/965

Change-Id: I6bdabf5ee09c000e49ea1757fde724d4bc4ccb92
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index b7061c5..7d2d79e 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -610,7 +610,7 @@
   //   [-pi, pi) x [0, pi / 2) x [-pi, pi)
   // which is enforced here
   if constexpr (EulerSystem::kIsProperEuler) {
-    const T kPi(constants::pi_v<T>);
+    const T kPi(constants::pi_v<double>);
     const T kTwoPi(2.0 * kPi);
     if (euler[1] < T(0.0) || ea[1] > kPi) {
       euler[0] += kPi;
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index f677759..87f9de7 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -913,6 +913,25 @@
 
 namespace {
 
+// Converts an array of N real numbers (doubles) to an array of jets
+template <int N>
+void ArrayToArrayOfJets(double const* const src, Jet<double, N>* dst) {
+  for (int i = 0; i < N; ++i) {
+    dst[i] = Jet<double, N>(src[i], i);
+  }
+}
+
+// Generically initializes a Jet with type T and a N-dimensional dual part
+// N is explicitly given (instead of inferred from sizeof...(Ts)) so that the
+// dual part can be initialized from Eigen expressions
+template <int N, typename T, typename... Ts>
+Jet<T, N> MakeJet(T a, const T& v0, Ts&&... v) {
+  Jet<T, N> j;
+  j.a = a;                                  // Real part
+  ((j.v << v0), ..., std::forward<Ts>(v));  // Fill dual part with N components
+  return j;
+}
+
 J3 MakeJ3(double a, double v0, double v1, double v2) {
   J3 j;
   j.a = a;
@@ -932,45 +951,50 @@
   return j;
 }
 
-bool IsClose(double x, double y) {
-  EXPECT_FALSE(isnan(x));
-  EXPECT_FALSE(isnan(y));
-  return internal::IsClose(x, y, kTolerance, nullptr, nullptr);
-}
-
 }  // namespace
 
-template <int N>
-bool IsClose(const Jet<double, N>& x, const Jet<double, N>& y) {
-  if (!IsClose(x.a, y.a)) {
+// Use EXPECT_THAT(x, testing::PointWise(JetClose(prec), y); to achieve Jet
+// array comparison
+MATCHER_P(JetClose, relative_precision, "") {
+  using internal::IsClose;
+  using LHSJetType = std::remove_reference_t<std::tuple_element_t<0, arg_type>>;
+  using RHSJetType = std::remove_reference_t<std::tuple_element_t<1, arg_type>>;
+
+  constexpr int kDualPartDimension = LHSJetType::DIMENSION;
+  static_assert(
+      kDualPartDimension == RHSJetType::DIMENSION,
+      "Can only compare Jets with dual parts having equal dimensions");
+  auto&& [x, y] = arg;
+  double relative_error;
+  double absolute_error;
+  if (!IsClose(
+          x.a, y.a, relative_precision, &relative_error, &absolute_error)) {
+    *result_listener << "Real part mismatch: x.a = " << x.a
+                     << " and y.a = " << y.a
+                     << " where the relative error between them is "
+                     << relative_error
+                     << " and the absolute error between them is "
+                     << absolute_error;
     return false;
   }
-  for (int i = 0; i < N; i++) {
-    if (!IsClose(x.v[i], y.v[i])) {
+  for (int i = 0; i < kDualPartDimension; i++) {
+    if (!IsClose(x.v[i],
+                 y.v[i],
+                 relative_precision,
+                 &relative_error,
+                 &absolute_error)) {
+      *result_listener << "Dual part mismatch: x.v[" << i << "] = " << x.v[i]
+                       << " and y.v[" << i << "] = " << y.v[i]
+                       << " where the relative error between them is "
+                       << relative_error
+                       << " and the absolute error between them is "
+                       << absolute_error;
       return false;
     }
   }
   return true;
 }
 
-template <int M, int N>
-void ExpectJetArraysClose(const Jet<double, N>* x, const Jet<double, N>* y) {
-  for (int i = 0; i < M; i++) {
-    if (!IsClose(x[i], y[i])) {
-      LOG(ERROR) << "Jet " << i << "/" << M << " not equal";
-      LOG(ERROR) << "x[" << i << "]: " << x[i];
-      LOG(ERROR) << "y[" << i << "]: " << y[i];
-      Jet<double, N> d, zero;
-      d.a = y[i].a - x[i].a;
-      for (int j = 0; j < N; j++) {
-        d.v[j] = y[i].v[j] - x[i].v[j];
-      }
-      LOG(ERROR) << "diff: " << d;
-      EXPECT_TRUE(IsClose(x[i], y[i]));
-    }
-  }
-}
-
 // Log-10 of a value well below machine precision.
 static const int kSmallTinyCutoff = static_cast<int>(
     2 * log(std::numeric_limits<double>::epsilon()) / log(10.0));
@@ -994,7 +1018,7 @@
         MakeJ3(0, 0, 0, sin(theta / 2) / theta),
     };
     AngleAxisToQuaternion(axis_angle, quaternion);
-    ExpectJetArraysClose<4, 3>(quaternion, expected);
+    EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected));
   }
 }
 
@@ -1016,7 +1040,7 @@
         MakeJ3(0, 0, 0, 0.5),
     };
     AngleAxisToQuaternion(axis_angle, quaternion);
-    ExpectJetArraysClose<4, 3>(quaternion, expected);
+    EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected));
   }
 }
 
@@ -1031,7 +1055,7 @@
       MakeJ3(0, 0, 0, 0.5),
   };
   AngleAxisToQuaternion(axis_angle, quaternion);
-  ExpectJetArraysClose<4, 3>(quaternion, expected);
+  EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected));
 }
 
 // Test that exact conversion works for small angles.
@@ -1052,7 +1076,7 @@
     };
     // clang-format on
     QuaternionToAngleAxis(quaternion, axis_angle);
-    ExpectJetArraysClose<3, 4>(axis_angle, expected);
+    EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected));
   }
 }
 
@@ -1077,7 +1101,7 @@
     };
     // clang-format on
     QuaternionToAngleAxis(quaternion, axis_angle);
-    ExpectJetArraysClose<3, 4>(axis_angle, expected);
+    EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected));
   }
 }
 
@@ -1091,7 +1115,508 @@
       MakeJ4(0, 0, 0, 0, 2.0),
   };
   QuaternionToAngleAxis(quaternion, axis_angle);
-  ExpectJetArraysClose<3, 4>(axis_angle, expected);
+  EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected));
+}
+
+// The following 4 test cases cover the conversion of Euler Angles to rotation
+// matrices for Jets
+//
+// The dual parts (with dimension 3) of the resultant matrix of Jets contain the
+// derivative of each matrix element w.r.t. the input Euler Angles. In other
+// words, for each element in R = EulerAnglesToRotationMatrix(angles), we have
+// R_ij.v = jacobian(R_ij, angles)
+//
+// The test data (dual parts of the Jets) is generated by analytically
+// differentiating the formulas for Euler Angle to Rotation Matrix conversion
+
+// Test ZXY/312 Intrinsic Euler Angles to rotation matrix conversion using Jets
+// The two ZXY test cases specifically cover handling of Tait-Bryan angles
+// i.e. last axis of rotation is different from the first
+TEST(EulerAngles, Intrinsic312EulerSequenceToRotationMatrixForJets) {
+  J3 euler_angles[3];
+  J3 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_euler[0], euler_angles);
+  EulerAnglesToRotation<IntrinsicZXY>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.306186083320, -0.883883627842, -0.176776571821, -0.918558748402),  // NOLINT
+      MakeJ3(-0.249999816229, -0.433012359189,  0.433012832394,  0.000000000000),  // NOLINT
+      MakeJ3( 0.918558748402,  0.176776777947,  0.176776558880,  0.306186083320),  // NOLINT
+      MakeJ3( 0.883883627842,  0.306186083320,  0.306185986727,  0.176776777947),  // NOLINT
+      MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771,  0.000000000000),  // NOLINT
+      MakeJ3(-0.176776777947,  0.918558748402, -0.306185964313,  0.883883627842),  // NOLINT
+      MakeJ3(-0.353553128699,  0.000000000000,  0.612372616786, -0.353553102817),  // NOLINT
+      MakeJ3( 0.866025628186,  0.000000000000,  0.499999611325,  0.000000000000),  // NOLINT
+      MakeJ3( 0.353553102817,  0.000000000000, -0.612372571957, -0.353553128699)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[1], euler_angles);
+  EulerAnglesToRotation<IntrinsicZXY>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.533493553520, -0.808012821828, -0.124999913397, -0.808012821828),  // NOLINT
+      MakeJ3(-0.249999816229, -0.433012359189,  0.433012832394,  0.000000000000),  // NOLINT
+      MakeJ3( 0.808012821828,  0.399519181706,  0.216506188745,  0.533493553520),  // NOLINT
+      MakeJ3( 0.808012821828,  0.533493553520,  0.216506188745,  0.399519181706),  // NOLINT
+      MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771,  0.000000000000),  // NOLINT
+      MakeJ3(-0.399519181706,  0.808012821828, -0.374999697927,  0.808012821828),  // NOLINT
+      MakeJ3(-0.249999816229,  0.000000000000,  0.433012832394, -0.433012359189),  // NOLINT
+      MakeJ3( 0.866025628186,  0.000000000000,  0.499999611325,  0.000000000000),  // NOLINT
+      MakeJ3( 0.433012359189,  0.000000000000, -0.750000183771, -0.249999816229)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[2], euler_angles);
+  EulerAnglesToRotation<IntrinsicZXY>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.047366781483, -0.659739427619, -0.530330235247, -0.789149143778),  // NOLINT
+      MakeJ3(-0.612372449483, -0.612372404654,  0.353553418477,  0.000000000000),  // NOLINT
+      MakeJ3( 0.789149143778, -0.435596057906,  0.306185986727,  0.047366781483),  // NOLINT
+      MakeJ3( 0.659739427619,  0.047366781483,  0.530330196424, -0.435596057906),  // NOLINT
+      MakeJ3( 0.612372404654, -0.612372449483, -0.353553392595,  0.000000000000),  // NOLINT
+      MakeJ3( 0.435596057906,  0.789149143778, -0.306185964313,  0.659739427619),  // NOLINT
+      MakeJ3(-0.750000183771,  0.000000000000,  0.433012832394, -0.433012359189),  // NOLINT
+      MakeJ3( 0.500000021132,  0.000000000000,  0.866025391584,  0.000000000000),  // NOLINT
+      MakeJ3( 0.433012359189,  0.000000000000, -0.249999816229, -0.750000183771)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+// Test ZXY/312 Extrinsic Euler Angles to rotation matrix conversion using Jets
+TEST(EulerAngles, Extrinsic312EulerSequenceToRotationMatrixForJets) {
+  J3 euler_angles[3];
+  J3 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_euler[0], euler_angles);
+  EulerAnglesToRotation<ExtrinsicZXY>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.918558725988,  0.176776842652,  0.176776571821, -0.306186150563),  // NOLINT
+      MakeJ3( 0.176776842652, -0.918558725988,  0.306185986727,  0.883883614902),  // NOLINT
+      MakeJ3( 0.353553128699,  0.000000000000, -0.612372616786,  0.353553102817),  // NOLINT
+      MakeJ3( 0.249999816229,  0.433012359189, -0.433012832394,  0.000000000000),  // NOLINT
+      MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771,  0.000000000000),  // NOLINT
+      MakeJ3(-0.866025628186,  0.000000000000, -0.499999611325,  0.000000000000),  // NOLINT
+      MakeJ3(-0.306186150563,  0.883883614902,  0.176776558880, -0.918558725988),  // NOLINT
+      MakeJ3( 0.883883614902,  0.306186150563,  0.306185964313, -0.176776842652),  // NOLINT
+      MakeJ3( 0.353553102817,  0.000000000000, -0.612372571957, -0.353553128699)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[1], euler_angles);
+  EulerAnglesToRotation<ExtrinsicZXY>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.966506404215, -0.058012606358,  0.124999913397, -0.058012606358),  // NOLINT
+      MakeJ3(-0.058012606358, -0.966506404215,  0.216506188745,  0.899519223971),  // NOLINT
+      MakeJ3( 0.249999816229,  0.000000000000, -0.433012832394,  0.433012359189),  // NOLINT
+      MakeJ3( 0.249999816229,  0.433012359189, -0.433012832394,  0.000000000000),  // NOLINT
+      MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771,  0.000000000000),  // NOLINT
+      MakeJ3(-0.866025628186,  0.000000000000, -0.499999611325,  0.000000000000),  // NOLINT
+      MakeJ3(-0.058012606358,  0.899519223971,  0.216506188745, -0.966506404215),  // NOLINT
+      MakeJ3( 0.899519223971,  0.058012606358,  0.374999697927,  0.058012606358),  // NOLINT
+      MakeJ3( 0.433012359189,  0.000000000000, -0.750000183771, -0.249999816229)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[2], euler_angles);
+  EulerAnglesToRotation<ExtrinsicZXY>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.659739424151, -0.047366829780,  0.530330235247, -0.435596000136),  // NOLINT
+      MakeJ3(-0.047366829780, -0.659739424151,  0.530330196424,  0.789149175666),  // NOLINT
+      MakeJ3( 0.750000183771,  0.000000000000, -0.433012832394,  0.433012359189),  // NOLINT
+      MakeJ3( 0.612372449483,  0.612372404654, -0.353553418477,  0.000000000000),  // NOLINT
+      MakeJ3( 0.612372404654, -0.612372449483, -0.353553392595,  0.000000000000),  // NOLINT
+      MakeJ3(-0.500000021132,  0.000000000000, -0.866025391584,  0.000000000000),  // NOLINT
+      MakeJ3(-0.435596000136,  0.789149175666,  0.306185986727, -0.659739424151),  // NOLINT
+      MakeJ3( 0.789149175666,  0.435596000136,  0.306185964313,  0.047366829780),  // NOLINT
+      MakeJ3( 0.433012359189,  0.000000000000, -0.249999816229, -0.750000183771)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+// Test ZXZ/313 Intrinsic Euler Angles to rotation matrix conversion using Jets
+// The two ZXZ test cases specifically cover handling of proper Euler Sequences
+// i.e. last axis of rotation is same as the first
+TEST(EulerAngles, Intrinsic313EulerSequenceToRotationMatrixForJets) {
+  J3 euler_angles[3];
+  J3 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_euler[0], euler_angles);
+  EulerAnglesToRotation<IntrinsicZXZ>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.435595832833, -0.659739379323,  0.306186321334, -0.789149008363),  // NOLINT
+      MakeJ3(-0.789149008363,  0.047367454164,  0.306186298920, -0.435595832833),  // NOLINT
+      MakeJ3( 0.433012832394,  0.750000183771,  0.249999816229,  0.000000000000),  // NOLINT
+      MakeJ3( 0.659739379323,  0.435595832833, -0.530330235247, -0.047367454164),  // NOLINT
+      MakeJ3(-0.047367454164, -0.789149008363, -0.530330196424, -0.659739379323),  // NOLINT
+      MakeJ3(-0.750000183771,  0.433012832394, -0.433012359189,  0.000000000000),  // NOLINT
+      MakeJ3( 0.612372616786,  0.000000000000,  0.353553128699,  0.612372571957),  // NOLINT
+      MakeJ3( 0.612372571957,  0.000000000000,  0.353553102817, -0.612372616786),  // NOLINT
+      MakeJ3( 0.499999611325,  0.000000000000, -0.866025628186,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[1], euler_angles);
+  EulerAnglesToRotation<IntrinsicZXZ>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.625000065470, -0.649518902838,  0.216506425348, -0.649518902838),  // NOLINT
+      MakeJ3(-0.649518902838, -0.124999676795,  0.375000107735, -0.625000065470),  // NOLINT
+      MakeJ3( 0.433012832394,  0.750000183771,  0.249999816229,  0.000000000000),  // NOLINT
+      MakeJ3( 0.649518902838,  0.625000065470, -0.375000107735,  0.124999676795),  // NOLINT
+      MakeJ3( 0.124999676795, -0.649518902838, -0.649519202838, -0.649518902838),  // NOLINT
+      MakeJ3(-0.750000183771,  0.433012832394, -0.433012359189,  0.000000000000),  // NOLINT
+      MakeJ3( 0.433012832394,  0.000000000000,  0.249999816229,  0.750000183771),  // NOLINT
+      MakeJ3( 0.750000183771,  0.000000000000,  0.433012359189, -0.433012832394),  // NOLINT
+      MakeJ3( 0.499999611325,  0.000000000000, -0.866025628186,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[2], euler_angles);
+  EulerAnglesToRotation<IntrinsicZXZ>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3(-0.176777132430, -0.883883325124,  0.306186321334, -0.918558558685),  // NOLINT
+      MakeJ3(-0.918558558685,  0.306186652473,  0.176776571821,  0.176777132430),  // NOLINT
+      MakeJ3( 0.353553418477,  0.353553392595,  0.612372449483,  0.000000000000),  // NOLINT
+      MakeJ3( 0.883883325124, -0.176777132430, -0.306186298920, -0.306186652473),  // NOLINT
+      MakeJ3(-0.306186652473, -0.918558558685, -0.176776558880, -0.883883325124),  // NOLINT
+      MakeJ3(-0.353553392595,  0.353553418477, -0.612372404654,  0.000000000000),  // NOLINT
+      MakeJ3( 0.433012832394,  0.000000000000,  0.750000183771,  0.249999816229),  // NOLINT
+      MakeJ3( 0.249999816229,  0.000000000000,  0.433012359189, -0.433012832394),  // NOLINT
+      MakeJ3( 0.866025391584,  0.000000000000, -0.500000021132,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+// Test ZXZ/313 Extrinsic Euler Angles to rotation matrix conversion using Jets
+TEST(EulerAngles, Extrinsic313EulerSequenceToRotationMatrixForJets) {
+  J3 euler_angles[3];
+  J3 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_euler[0], euler_angles);
+  EulerAnglesToRotation<ExtrinsicZXZ>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.435595832833, -0.659739379323,  0.306186321334, -0.789149008363),  // NOLINT
+      MakeJ3(-0.659739379323, -0.435595832833,  0.530330235247,  0.047367454164),  // NOLINT
+      MakeJ3( 0.612372616786,  0.000000000000,  0.353553128699,  0.612372571957),  // NOLINT
+      MakeJ3( 0.789149008363, -0.047367454164, -0.306186298920,  0.435595832833),  // NOLINT
+      MakeJ3(-0.047367454164, -0.789149008363, -0.530330196424, -0.659739379323),  // NOLINT
+      MakeJ3(-0.612372571957,  0.000000000000, -0.353553102817,  0.612372616786),  // NOLINT
+      MakeJ3( 0.433012832394,  0.750000183771,  0.249999816229,  0.000000000000),  // NOLINT
+      MakeJ3( 0.750000183771, -0.433012832394,  0.433012359189,  0.000000000000),  // NOLINT
+      MakeJ3( 0.499999611325,  0.000000000000, -0.866025628186,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[1], euler_angles);
+  EulerAnglesToRotation<ExtrinsicZXZ>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3( 0.625000065470, -0.649518902838,  0.216506425348, -0.649518902838),  // NOLINT
+      MakeJ3(-0.649518902838, -0.625000065470,  0.375000107735, -0.124999676795),  // NOLINT
+      MakeJ3( 0.433012832394,  0.000000000000,  0.249999816229,  0.750000183771),  // NOLINT
+      MakeJ3( 0.649518902838,  0.124999676795, -0.375000107735,  0.625000065470),  // NOLINT
+      MakeJ3( 0.124999676795, -0.649518902838, -0.649519202838, -0.649518902838),  // NOLINT
+      MakeJ3(-0.750000183771,  0.000000000000, -0.433012359189,  0.433012832394),  // NOLINT
+      MakeJ3( 0.433012832394,  0.750000183771,  0.249999816229,  0.000000000000),  // NOLINT
+      MakeJ3( 0.750000183771, -0.433012832394,  0.433012359189,  0.000000000000),  // NOLINT
+      MakeJ3( 0.499999611325,  0.000000000000, -0.866025628186,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_euler[2], euler_angles);
+  EulerAnglesToRotation<ExtrinsicZXZ>(euler_angles, rotation_matrix);
+  {
+    // clang-format off
+    const J3 expected[] = {
+      MakeJ3(-0.176777132430, -0.883883325124,  0.306186321334, -0.918558558685),  // NOLINT
+      MakeJ3(-0.883883325124,  0.176777132430,  0.306186298920,  0.306186652473),  // NOLINT
+      MakeJ3( 0.433012832394,  0.000000000000,  0.750000183771,  0.249999816229),  // NOLINT
+      MakeJ3( 0.918558558685, -0.306186652473, -0.176776571821, -0.176777132430),  // NOLINT
+      MakeJ3(-0.306186652473, -0.918558558685, -0.176776558880, -0.883883325124),  // NOLINT
+      MakeJ3(-0.249999816229,  0.000000000000, -0.433012359189,  0.433012832394),  // NOLINT
+      MakeJ3( 0.353553418477,  0.353553392595,  0.612372449483,  0.000000000000),  // NOLINT
+      MakeJ3( 0.353553392595, -0.353553418477,  0.612372404654,  0.000000000000),  // NOLINT
+      MakeJ3( 0.866025391584,  0.000000000000, -0.500000021132,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(rotation_matrix,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+using J9 = Jet<double, 9>;
+
+// The following 4 tests Tests the conversion of rotation matrices to Euler
+// Angles for Jets.
+//
+// The dual parts (with dimension 9) of the resultant array of Jets contain the
+// derivative of each Euler angle w.r.t. each of the 9 elements of the rotation
+// matrix, or a 9-by-1 array formed from flattening the rotation matrix. In
+// other words, for each element in angles = RotationMatrixToEulerAngles(R), we
+// have angles.v = jacobian(angles, [R11 R12 R13 R21 ... R32 R33]);
+//
+// Note: the order of elements in v depend on row/column-wise flattening of
+// the rotation matrix
+//
+// The test data (dual parts of the Jets) is generated by analytically
+// differentiating the formulas for Rotation Matrix to Euler Angle conversion
+
+// clang-format off
+static double sample_matrices[][9] = {
+  { 0.433012359189,  0.176776842652,  0.883883614902,  0.249999816229,  0.918558725988, -0.306186150563, -0.866025628186,  0.353553128699,  0.353553102817},  // NOLINT
+  { 0.433012359189, -0.058012606358,  0.899519223971,  0.249999816229,  0.966506404215, -0.058012606358, -0.866025628186,  0.249999816229,  0.433012359189},  // NOLINT
+  { 0.612372404654, -0.047366829780,  0.789149175666,  0.612372449483,  0.659739424151, -0.435596000136, -0.500000021132,  0.750000183771,  0.433012359189}   // NOLINT
+};
+// clang-format on
+
+// Test rotation matrix to ZXY/312 Intrinsic Euler Angles conversion using Jets
+// The two ZXY test cases specifically cover handling of Tait-Bryan angles
+// i.e. last axis of rotation is different from the first
+TEST(EulerAngles, RotationMatrixToIntrinsic312EulerSequenceForJets) {
+  J9 euler_angles[3];
+  J9 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_matrices[0], rotation_matrix);
+  RotationMatrixToEulerAngles<IntrinsicZXY>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>(-0.190125743401,  0.000000000000, -1.049781178951,  0.000000000000,  0.000000000000,  0.202030634558,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.361366843930,  0.000000000000, -0.066815309609,  0.000000000000,  0.000000000000, -0.347182270882,  0.000000000000,  0.000000000000,  0.935414445680,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.183200015636,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.404060603418,  0.000000000000, -0.989743365598)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[1], rotation_matrix);
+  RotationMatrixToEulerAngles<IntrinsicZXY>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 0.059951064811,  0.000000000000, -1.030940063452,  0.000000000000,  0.000000000000, -0.061880107384,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.252680065344,  0.000000000000,  0.014978778808,  0.000000000000,  0.000000000000, -0.249550684831,  0.000000000000,  0.000000000000,  0.968245884001,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.107149138016,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.461879804532,  0.000000000000, -0.923760579526)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[2], rotation_matrix);
+  RotationMatrixToEulerAngles<IntrinsicZXY>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 0.071673287221,  0.000000000000, -1.507976776767,  0.000000000000,  0.000000000000, -0.108267107713,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.848062356818,  0.000000000000,  0.053708966648,  0.000000000000,  0.000000000000, -0.748074610289,  0.000000000000,  0.000000000000,  0.661437619389,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.857072360427,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.989743158900,  0.000000000000, -1.142857911244)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+// Test rotation matrix to ZXY/312 Extrinsic Euler Angles conversion using Jets
+TEST(EulerAngles, RotationMatrixToExtrinsic312EulerSequenceForJets) {
+  J9 euler_angles[3];
+  J9 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_matrices[0], rotation_matrix);
+  RotationMatrixToEulerAngles<ExtrinsicZXY>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 0.265728912717,  0.000000000000,  0.000000000000,  0.000000000000,  1.013581996386, -0.275861853641,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.311184173598,  0.000000000000,  0.000000000000, -0.284286741927,  0.000000000000,  0.000000000000, -0.951971659874,  0.000000000000,  0.000000000000, -0.113714586405),  // NOLINT
+      MakeJet<9>( 1.190290284357,  0.000000000000,  0.000000000000,  0.390127543992,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.975319806582)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[1], rotation_matrix);
+  RotationMatrixToEulerAngles<ExtrinsicZXY>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 0.253115668605,  0.000000000000,  0.000000000000,  0.000000000000,  0.969770129215, -0.250844022378,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.058045195612,  0.000000000000,  0.000000000000, -0.052271487648,  0.000000000000,  0.000000000000, -0.998315850572,  0.000000000000,  0.000000000000, -0.025162553041),  // NOLINT
+      MakeJet<9>( 1.122153748896,  0.000000000000,  0.000000000000,  0.434474567050,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.902556744846)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[2], rotation_matrix);
+  RotationMatrixToEulerAngles<ExtrinsicZXY>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 0.748180444286,  0.000000000000,  0.000000000000,  0.000000000000,  0.814235652244, -0.755776390750,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 0.450700288478,  0.000000000000,  0.000000000000, -0.381884322045,  0.000000000000,  0.000000000000, -0.900142280234,  0.000000000000,  0.000000000000, -0.209542930950),  // NOLINT
+      MakeJet<9>( 1.068945699497,  0.000000000000,  0.000000000000,  0.534414175972,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.973950275281)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+// Test rotation matrix to ZXZ/313 Intrinsic Euler Angles conversion using Jets
+//// The two ZXZ test cases specifically cover handling of proper Euler
+/// Sequences
+// i.e. last axis of rotation is same as the first
+TEST(EulerAngles, RotationMatrixToIntrinsic313EulerSequenceForJets) {
+  J9 euler_angles[3];
+  J9 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_matrices[0], rotation_matrix);
+  RotationMatrixToEulerAngles<IntrinsicZXZ>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 1.237323270947,  0.000000000000,  0.000000000000,  0.349926947837,  0.000000000000,  0.000000000000,  1.010152467826,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.209429510533,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.327326615680,  0.133630397662, -0.935414455462),  // NOLINT
+      MakeJet<9>(-1.183199990019,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.404060624546,  0.989743344897,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[1], rotation_matrix);
+  RotationMatrixToEulerAngles<IntrinsicZXZ>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 1.506392616830,  0.000000000000,  0.000000000000,  0.071400104821,  0.000000000000,  0.000000000000,  1.107100178948,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.122964310061,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.416024849727,  0.120095910090, -0.901387983495),  // NOLINT
+      MakeJet<9>(-1.289761690216,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.307691969119,  1.065877306886,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[2], rotation_matrix);
+  RotationMatrixToEulerAngles<IntrinsicZXZ>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>( 1.066432836578,  0.000000000000,  0.000000000000,  0.536117958181,  0.000000000000,  0.000000000000,  0.971260169116,  0.000000000000,  0.000000000000,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.122964310061,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.240192006893,  0.360288083393, -0.901387983495),  // NOLINT
+      MakeJet<9>(-0.588002509965,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.923076812076,  0.615384416607,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+}
+
+// Test rotation matrix to ZXZ/313 Extrinsic Euler Angles conversion using Jets
+TEST(EulerAngles, RotationMatrixToExtrinsic313EulerSequenceForJets) {
+  J9 euler_angles[3];
+  J9 rotation_matrix[9];
+
+  ArrayToArrayOfJets(sample_matrices[0], rotation_matrix);
+  RotationMatrixToEulerAngles<ExtrinsicZXZ>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>(-1.183199990019,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.404060624546,  0.989743344897,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.209429510533,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.327326615680,  0.133630397662, -0.935414455462),  // NOLINT
+      MakeJet<9>( 1.237323270947,  0.000000000000,  0.000000000000,  0.349926947837,  0.000000000000,  0.000000000000,  1.010152467826,  0.000000000000,  0.000000000000,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[1], rotation_matrix);
+  RotationMatrixToEulerAngles<ExtrinsicZXZ>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>(-1.289761690216,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.307691969119,  1.065877306886,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.122964310061,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.416024849727,  0.120095910090, -0.901387983495),  // NOLINT
+      MakeJet<9>( 1.506392616830,  0.000000000000,  0.000000000000,  0.071400104821,  0.000000000000,  0.000000000000,  1.107100178948,  0.000000000000,  0.000000000000,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
+
+  ArrayToArrayOfJets(sample_matrices[2], rotation_matrix);
+  RotationMatrixToEulerAngles<ExtrinsicZXZ>(rotation_matrix, euler_angles);
+  {
+    // clang-format off
+    const J9 expected[] = {
+      MakeJet<9>(-0.588002509965,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.923076812076,  0.615384416607,  0.000000000000),  // NOLINT
+      MakeJet<9>( 1.122964310061,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000,  0.000000000000, -0.240192006893,  0.360288083393, -0.901387983495),  // NOLINT
+      MakeJet<9>( 1.066432836578,  0.000000000000,  0.000000000000,  0.536117958181,  0.000000000000,  0.000000000000,  0.971260169116,  0.000000000000,  0.000000000000,  0.000000000000)   // NOLINT
+    };
+    // clang-format on
+    EXPECT_THAT(euler_angles,
+                testing::Pointwise(JetClose(kLooseTolerance), expected));
+  }
 }
 
 TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) {