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) {