Add generalized Euler Angle conversions
Conversions function include Euler Angles to / from Rotation Matrices
and Quaternions. They are generalized for any Euler convention that can
be specified in the arguments. Algorithm is from "Euler angle
conversion", Ken Shoemake, Graphics Gems IV
Change-Id: I7f9ddc0b8d686efca16299d2ba374295744376ce
diff --git a/include/ceres/constants.h b/include/ceres/constants.h
new file mode 100644
index 0000000..a0d84c6
--- /dev/null
+++ b/include/ceres/constants.h
@@ -0,0 +1,41 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: hellston20a@gmail.com (H S Helson Go)
+
+#ifndef CERES_PUBLIC_CONSTANTS_H_
+#define CERES_PUBLIC_CONSTANTS_H_
+
+// TODO(HSHelson): This header should no longer be necessary once C++20's
+// <numbers> (e.g. std::numbers::pi_v) becomes usable
+namespace ceres::constants {
+template <typename T>
+inline constexpr T pi_v(3.141592653589793238462643383279502884);
+} // namespace ceres::constants
+
+#endif // CERES_PUBLIC_CONSTANTS_H_
diff --git a/include/ceres/internal/euler_angles.h b/include/ceres/internal/euler_angles.h
new file mode 100644
index 0000000..19668b9
--- /dev/null
+++ b/include/ceres/internal/euler_angles.h
@@ -0,0 +1,199 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
+#define CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
+
+#include <type_traits>
+
+namespace ceres {
+namespace internal {
+
+// The EulerSystem struct represents an Euler Angle Convention in compile time.
+// It acts like a trait structure and is also used as a tag for dispatching
+// Euler angle conversion function templates
+//
+// Internally, it implements the convention laid out in "Euler angle
+// conversion", Ken Shoemake, Graphics Gems IV, where a choice of axis for the
+// first rotation (out of 3) and 3 binary choices compactly specify all 24
+// rotation conventions
+//
+// - InnerAxis: Axis for the first rotation. This is specified by struct tags
+// axis::X, axis::Y, and axis::Z
+//
+// - Parity: Defines the parity of the axis permutation. The axis sequence has
+// Even parity if the second axis of rotation is 'greater-than' the first axis
+// of rotation according to the order X<Y<Z<X, otherwise it has Odd parity.
+// This is specified by struct tags Even and Odd
+//
+// - AngleConvention: Defines whether Proper Euler Angles (originally defined
+// by Euler, which has the last axis repeated, i.e. ZYZ, ZXZ, etc), or
+// Tait-Bryan Angles (introduced by the nautical and aerospace fields, i.e.
+// using ZYX for roll-pitch-yaw) are used. This is specified by struct Tags
+// ProperEuler and TaitBryan.
+//
+// - FrameConvention: Defines whether the three rotations are be in a global
+// frame of reference (extrinsic) or in a body centred frame of reference
+// (intrinsic). This is specified by struct tags Extrinsic and Intrinsic
+
+namespace axis {
+struct X : std::integral_constant<int, 0> {};
+struct Y : std::integral_constant<int, 1> {};
+struct Z : std::integral_constant<int, 2> {};
+} // namespace axis
+
+struct Even;
+struct Odd;
+
+struct ProperEuler;
+struct TaitBryan;
+
+struct Extrinsic;
+struct Intrinsic;
+
+template <typename InnerAxisType,
+ typename ParityType,
+ typename AngleConventionType,
+ typename FrameConventionType>
+struct EulerSystem {
+ static constexpr bool kIsParityOdd = std::is_same_v<ParityType, Odd>;
+ static constexpr bool kIsProperEuler =
+ std::is_same_v<AngleConventionType, ProperEuler>;
+ static constexpr bool kIsIntrinsic =
+ std::is_same_v<FrameConventionType, Intrinsic>;
+
+ static constexpr int kAxes[3] = {
+ InnerAxisType::value,
+ (InnerAxisType::value + 1 + static_cast<int>(kIsParityOdd)) % 3,
+ (InnerAxisType::value + 2 - static_cast<int>(kIsParityOdd)) % 3};
+};
+
+} // namespace internal
+
+// Define human readable aliases to the type of the tags
+using ExtrinsicXYZ = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicXYX = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicXZY = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicXZX = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicYZX = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicYZY = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicYXZ = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicYXY = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicZXY = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicZXZ = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+using ExtrinsicZYX = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Extrinsic>;
+using ExtrinsicZYZ = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Extrinsic>;
+/* Rotating axes */
+using IntrinsicZYX = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicXYX = internal::EulerSystem<internal::axis::X,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicYZX = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicXZX = internal::EulerSystem<internal::axis::X,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicXZY = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicYZY = internal::EulerSystem<internal::axis::Y,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicZXY = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicYXY = internal::EulerSystem<internal::axis::Y,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicYXZ = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicZXZ = internal::EulerSystem<internal::axis::Z,
+ internal::Even,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+using IntrinsicXYZ = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::TaitBryan,
+ internal::Intrinsic>;
+using IntrinsicZYZ = internal::EulerSystem<internal::axis::Z,
+ internal::Odd,
+ internal::ProperEuler,
+ internal::Intrinsic>;
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_EULER_ANGLES_H_
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index 8cdd3fc..a92d84a 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -49,6 +49,8 @@
#include <cmath>
#include <limits>
+#include "ceres/constants.h"
+#include "ceres/internal/euler_angles.h"
#include "glog/logging.h"
namespace ceres {
@@ -136,6 +138,71 @@
void EulerAnglesToRotationMatrix(
const T* euler, const MatrixAdapter<T, row_stride, col_stride>& R);
+// Convert a generic Euler Angle sequence (in radians) to a 3x3 rotation matrix.
+//
+// Euler Angles define a sequence of 3 rotations about a sequence of axes,
+// typically taken to be the X, Y, or Z axes. The last axis may be the same as
+// the first axis (e.g. ZYZ) per Euler's original definition of his angles
+// (proper Euler angles) or not (e.g. ZYX / yaw-pitch-roll), per common usage in
+// the nautical and aerospace fields (Tait-Bryan angles). The three rotations
+// may be in a global frame of reference (Extrinsic) or in a body fixed frame of
+// reference (Intrinsic) that moves with the rotating object.
+//
+// Internally, Euler Axis sequences are classified by Ken Shoemake's scheme from
+// "Euler angle conversion", Graphics Gems IV, where a choice of axis for the
+// first rotation and 3 binary choices:
+// 1. Parity of the axis permutation. The axis sequence has Even parity if the
+// second axis of rotation is 'greater-than' the first axis of rotation
+// according to the order X<Y<Z<X, otherwise it has Odd parity.
+// 2. Proper Euler Angles v.s. Tait-Bryan Angles
+// 3. Extrinsic Rotations v.s. Intrinsic Rotations
+// compactly represent all 24 possible Euler Angle Conventions
+//
+// One template parameter: EulerSystem must be explicitly given. This parameter
+// is a tag named by 'Extrinsic' or 'Intrinsic' followed by three characters in
+// the set '[XYZ]', specifying the axis sequence, e.g. ceres::ExtrinsicYZY
+// (robotic arms), ceres::IntrinsicZYX (for aerospace), etc.
+//
+// The order of elements in the input array 'euler' follows the axis sequence
+template <typename EulerSystem, typename T>
+inline void EulerAnglesToRotation(const T* euler, T* R);
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void EulerAnglesToRotation(const T* euler,
+ const MatrixAdapter<T, row_stride, col_stride>& R);
+
+// Convert a 3x3 rotation matrix to a generic Euler Angle sequence (in radians)
+//
+// Euler Angles define a sequence of 3 rotations about a sequence of axes,
+// typically taken to be the X, Y, or Z axes. The last axis may be the same as
+// the first axis (e.g. ZYZ) per Euler's original definition of his angles
+// (proper Euler angles) or not (e.g. ZYX / yaw-pitch-roll), per common usage in
+// the nautical and aerospace fields (Tait-Bryan angles). The three rotations
+// may be in a global frame of reference (Extrinsic) or in a body fixed frame of
+// reference (Intrinsic) that moves with the rotating object.
+//
+// Internally, Euler Axis sequences are classified by Ken Shoemake's scheme from
+// "Euler angle conversion", Graphics Gems IV, where a choice of axis for the
+// first rotation and 3 binary choices:
+// 1. Oddness of the axis permutation, that defines whether the second axis is
+// 'greater-than' the first axis according to the order X>Y>Z>X)
+// 2. Proper Euler Angles v.s. Tait-Bryan Angles
+// 3. Extrinsic Rotations v.s. Intrinsic Rotations
+// compactly represent all 24 possible Euler Angle Conventions
+//
+// One template parameter: EulerSystem must be explicitly given. This parameter
+// is a tag named by 'Extrinsic' or 'Intrinsic' followed by three characters in
+// the set '[XYZ]', specifying the axis sequence, e.g. ceres::ExtrinsicYZY
+// (robotic arms), ceres::IntrinsicZYX (for aerospace), etc.
+//
+// The order of elements in the output array 'euler' follows the axis sequence
+template <typename EulerSystem, typename T>
+inline void RotationMatrixToEulerAngles(const T* R, T* euler);
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void RotationMatrixToEulerAngles(
+ const MatrixAdapter<const T, row_stride, col_stride>& R, T* euler);
+
// Convert a 4-vector to a 3x3 scaled rotation matrix.
//
// The choice of rotation is such that the quaternion [1 0 0 0] goes to an
@@ -424,6 +491,141 @@
}
}
+template <typename EulerSystem, typename T>
+inline void EulerAnglesToRotation(const T* euler, T* R) {
+ EulerAnglesToRotation<EulerSystem>(euler, RowMajorAdapter3x3(R));
+}
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void EulerAnglesToRotation(const T* euler,
+ const MatrixAdapter<T, row_stride, col_stride>& R) {
+ using std::cos;
+ using std::sin;
+
+ const auto [i, j, k] = EulerSystem::kAxes;
+
+ T ea[3];
+ ea[1] = euler[1];
+ if constexpr (EulerSystem::kIsIntrinsic) {
+ ea[0] = euler[2];
+ ea[2] = euler[0];
+ } else {
+ ea[0] = euler[0];
+ ea[2] = euler[2];
+ }
+ if constexpr (EulerSystem::kIsParityOdd) {
+ ea[0] = -ea[0];
+ ea[1] = -ea[1];
+ ea[2] = -ea[2];
+ }
+
+ const T ci = cos(ea[0]);
+ const T cj = cos(ea[1]);
+ const T ch = cos(ea[2]);
+ const T si = sin(ea[0]);
+ const T sj = sin(ea[1]);
+ const T sh = sin(ea[2]);
+ const T cc = ci * ch;
+ const T cs = ci * sh;
+ const T sc = si * ch;
+ const T ss = si * sh;
+ if constexpr (EulerSystem::kIsProperEuler) {
+ R(i, i) = cj;
+ R(i, j) = sj * si;
+ R(i, k) = sj * ci;
+ R(j, i) = sj * sh;
+ R(j, j) = -cj * ss + cc;
+ R(j, k) = -cj * cs - sc;
+ R(k, i) = -sj * ch;
+ R(k, j) = cj * sc + cs;
+ R(k, k) = cj * cc - ss;
+ } else {
+ R(i, i) = cj * ch;
+ R(i, j) = sj * sc - cs;
+ R(i, k) = sj * cc + ss;
+ R(j, i) = cj * sh;
+ R(j, j) = sj * ss + cc;
+ R(j, k) = sj * cs - sc;
+ R(k, i) = -sj;
+ R(k, j) = cj * si;
+ R(k, k) = cj * ci;
+ }
+}
+
+template <typename EulerSystem, typename T>
+inline void RotationMatrixToEulerAngles(const T* R, T* euler) {
+ RotationMatrixToEulerAngles<EulerSystem>(RowMajorAdapter3x3(R), euler);
+}
+
+template <typename EulerSystem, typename T, int row_stride, int col_stride>
+void RotationMatrixToEulerAngles(
+ const MatrixAdapter<const T, row_stride, col_stride>& R, T* euler) {
+ using std::atan2;
+ using std::fpclassify;
+ using std::hypot;
+
+ const auto [i, j, k] = EulerSystem::kAxes;
+
+ T ea[3];
+ if constexpr (EulerSystem::kIsProperEuler) {
+ const T sy = hypot(R(i, j), R(i, k));
+ if (fpclassify(sy) != FP_ZERO) {
+ ea[0] = atan2(R(i, j), R(i, k));
+ ea[1] = atan2(sy, R(i, i));
+ ea[2] = atan2(R(j, i), -R(k, i));
+ } else {
+ ea[0] = atan2(-R(j, k), R(j, j));
+ ea[1] = atan2(sy, R(i, i));
+ ea[2] = 0;
+ }
+ } else {
+ const T cy = hypot(R(i, i), R(j, i));
+ if (fpclassify(cy) != FP_ZERO) {
+ ea[0] = atan2(R(k, j), R(k, k));
+ ea[1] = atan2(-R(k, i), cy);
+ ea[2] = atan2(R(j, i), R(i, i));
+ } else {
+ ea[0] = atan2(-R(j, k), R(j, j));
+ ea[1] = atan2(-R(k, i), cy);
+ ea[2] = 0;
+ }
+ }
+ if constexpr (EulerSystem::kIsParityOdd) {
+ ea[0] = -ea[0];
+ ea[1] = -ea[1];
+ ea[2] = -ea[2];
+ }
+ euler[1] = ea[1];
+ if constexpr (EulerSystem::kIsIntrinsic) {
+ euler[0] = ea[2];
+ euler[2] = ea[0];
+ } else {
+ euler[0] = ea[0];
+ euler[2] = ea[2];
+ }
+
+ // Proper euler angles are defined for angles in
+ // [-pi, pi) x [0, pi / 2) x [-pi, pi)
+ // which is enforced here
+ if constexpr (EulerSystem::kIsProperEuler) {
+ constexpr T kPi = constants::pi_v<T>;
+ constexpr T kTwoPi(2.0 * kPi);
+ if (euler[1] < T(0.0) || ea[1] > kPi) {
+ euler[0] += kPi;
+ euler[1] = -euler[1];
+ euler[2] -= kPi;
+ }
+
+ for (int i = 0; i < 3; ++i) {
+ if (euler[i] < -kPi) {
+ euler[i] += kTwoPi;
+ } else if (euler[i] > kPi) {
+ euler[i] -= kTwoPi;
+ }
+ }
+ }
+}
+
template <typename T>
inline void EulerAnglesToRotationMatrix(const T* euler,
const int row_stride_parameter,
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index 97ea514..688edc3 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -35,7 +35,9 @@
#include <random>
#include <string>
+#include "ceres/constants.h"
#include "ceres/internal/eigen.h"
+#include "ceres/internal/euler_angles.h"
#include "ceres/internal/export.h"
#include "ceres/is_close.h"
#include "ceres/jet.h"
@@ -54,7 +56,7 @@
using std::string;
using std::swap;
-const double kPi = 3.14159265358979323846;
+inline constexpr double kPi = constants::pi_v<double>;
const double kHalfSqrt2 = 0.707106781186547524401;
// A tolerance value for floating-point comparisons.
@@ -670,6 +672,244 @@
}
}
+static double sample_euler[][3] = {{0.5235988, 1.047198, 0.7853982},
+ {0.5235988, 1.047198, 0.5235988},
+ {0.7853982, 0.5235988, 1.047198}};
+
+// ZXY Intrinsic Euler Angle to rotation matrix conversion test from
+// scipy/spatial/transform/test/test_rotation.py
+TEST(EulerAngles, IntrinsicEulerSequence312ToRotationMatrixCanned) {
+ // clang-format off
+ double const expected[][9] =
+ {{0.306186083320088, -0.249999816228639, 0.918558748402491,
+ 0.883883627842492, 0.433012359189203, -0.176776777947208,
+ -0.353553128699351, 0.866025628186053, 0.353553102817459},
+ { 0.533493553519713, -0.249999816228639, 0.808012821828067,
+ 0.808012821828067, 0.433012359189203, -0.399519181705765,
+ -0.249999816228639, 0.866025628186053, 0.433012359189203},
+ { 0.047366781483451, -0.612372449482883, 0.789149143778432,
+ 0.659739427618959, 0.612372404654096, 0.435596057905909,
+ -0.750000183771249, 0.500000021132493, 0.433012359189203}};
+ // clang-format on
+
+ for (int i = 0; i < 3; ++i) {
+ double results[9];
+ EulerAnglesToRotation<IntrinsicZXY>(sample_euler[i], results);
+ ASSERT_THAT(results, IsNear3x3Matrix(expected[i]));
+ }
+}
+
+// ZXY Extrinsic Euler Angle to rotation matrix conversion test from
+// scipy/spatial/transform/test/test_rotation.py
+TEST(EulerAngles, ExtrinsicEulerSequence312ToRotationMatrix) {
+ // clang-format off
+ double const expected[][9] =
+ {{0.918558725988105, 0.176776842651999, 0.353553128699352,
+ 0.249999816228639, 0.433012359189203, -0.866025628186053,
+ -0.306186150563275, 0.883883614901527, 0.353553102817459},
+ { 0.966506404215301, -0.058012606358071, 0.249999816228639,
+ 0.249999816228639, 0.433012359189203, -0.866025628186053,
+ -0.058012606358071, 0.899519223970752, 0.433012359189203},
+ { 0.659739424151467, -0.047366829779744, 0.750000183771249,
+ 0.612372449482883, 0.612372404654096, -0.500000021132493,
+ -0.435596000136163, 0.789149175666285, 0.433012359189203}};
+ // clang-format on
+
+ for (int i = 0; i < 3; ++i) {
+ double results[9];
+ EulerAnglesToRotation<ExtrinsicZXY>(sample_euler[i], results);
+ ASSERT_THAT(results, IsNear3x3Matrix(expected[i]));
+ }
+}
+
+// ZXZ Intrinsic Euler Angle to rotation matrix conversion test from
+// scipy/spatial/transform/test/test_rotation.py
+TEST(EulerAngles, IntrinsicEulerSequence313ToRotationMatrix) {
+ // clang-format off
+ double expected[][9] =
+ {{0.435595832832961, -0.789149008363071, 0.433012832394307,
+ 0.659739379322704, -0.047367454164077, -0.750000183771249,
+ 0.612372616786097, 0.612372571957297, 0.499999611324802},
+ { 0.625000065470068, -0.649518902838302, 0.433012832394307,
+ 0.649518902838302, 0.124999676794869, -0.750000183771249,
+ 0.433012832394307, 0.750000183771249, 0.499999611324802},
+ {-0.176777132429787, -0.918558558684756, 0.353553418477159,
+ 0.883883325123719, -0.306186652473014, -0.353553392595246,
+ 0.433012832394307, 0.249999816228639, 0.866025391583588}};
+ // clang-format on
+ for (int i = 0; i < 3; ++i) {
+ double results[9];
+ EulerAnglesToRotation<IntrinsicZXZ>(sample_euler[i], results);
+ ASSERT_THAT(results, IsNear3x3Matrix(expected[i]));
+ }
+}
+
+// ZXZ Extrinsic Euler Angle to rotation matrix conversion test from
+// scipy/spatial/transform/test/test_rotation.py
+TEST(EulerAngles, ExtrinsicEulerSequence313ToRotationMatrix) {
+ // clang-format off
+ double expected[][9] =
+ {{0.435595832832961, -0.659739379322704, 0.612372616786097,
+ 0.789149008363071, -0.047367454164077, -0.612372571957297,
+ 0.433012832394307, 0.750000183771249, 0.499999611324802},
+ { 0.625000065470068, -0.649518902838302, 0.433012832394307,
+ 0.649518902838302, 0.124999676794869, -0.750000183771249,
+ 0.433012832394307, 0.750000183771249, 0.499999611324802},
+ {-0.176777132429787, -0.883883325123719, 0.433012832394307,
+ 0.918558558684756, -0.306186652473014, -0.249999816228639,
+ 0.353553418477159, 0.353553392595246, 0.866025391583588}};
+ // clang-format on
+ for (int i = 0; i < 3; ++i) {
+ double results[9];
+ EulerAnglesToRotation<ExtrinsicZXZ>(sample_euler[i], results);
+ ASSERT_THAT(results, IsNear3x3Matrix(expected[i]));
+ }
+}
+
+template <typename T>
+struct GeneralEulerAngles : public ::testing::Test {
+ public:
+ static constexpr bool kIsParityOdd = T::kIsParityOdd;
+ static constexpr bool kIsProperEuler = T::kIsProperEuler;
+ static constexpr bool kIsIntrinsic = T::kIsIntrinsic;
+
+ template <typename URBG>
+ static void RandomEulerAngles(double* euler, URBG& prng) {
+ using ParamType = std::uniform_real_distribution<double>::param_type;
+ std::uniform_real_distribution<double> uniform_distribution{-kPi, kPi};
+ // Euler angles should be in
+ // [-pi,pi) x [0,pi) x [-pi,pi])
+ // if the outer axes are repeated and
+ // [-pi,pi) x [-pi/2,pi/2) x [-pi,pi])
+ // otherwise
+ euler[0] = uniform_distribution(prng);
+ euler[2] = uniform_distribution(prng);
+ if constexpr (kIsProperEuler) {
+ euler[1] = uniform_distribution(prng, ParamType{0, kPi});
+ } else {
+ euler[1] = uniform_distribution(prng, ParamType{-kPi / 2, kPi / 2});
+ }
+ }
+
+ static void CheckPrincipalRotationMatrixProduct(double angles[3]) {
+ // Convert Shoemake's Euler angle convention into 'apparent' rotation axes
+ // sequences, i.e. the alphabetic code (ZYX, ZYZ, etc.) indicates in what
+ // sequence rotations about different axes are applied
+ constexpr int i = T::kAxes[0];
+ constexpr int j = (3 + (kIsParityOdd ? (i - 1) % 3 : (i + 1) % 3)) % 3;
+ constexpr int k = kIsProperEuler ? i : 3 ^ i ^ j;
+ constexpr auto kSeq =
+ kIsIntrinsic ? std::array{k, j, i} : std::array{i, j, k};
+
+ double aa_matrix[9];
+ Eigen::Map<Eigen::Matrix3d, 0, Eigen::Stride<1, 3>> aa(aa_matrix);
+ aa.setIdentity();
+ for (int i = 0; i < 3; ++i) {
+ Eigen::Vector3d angle_axis;
+ if constexpr (kIsIntrinsic) {
+ angle_axis = -angles[i] * Eigen::Vector3d::Unit(kSeq[i]);
+ } else {
+ angle_axis = angles[i] * Eigen::Vector3d::Unit(kSeq[i]);
+ }
+ Eigen::Matrix3d m;
+ AngleAxisToRotationMatrix(angle_axis.data(), m.data());
+ aa = m * aa;
+ }
+ if constexpr (kIsIntrinsic) {
+ aa.transposeInPlace();
+ }
+
+ double ea_matrix[9];
+ EulerAnglesToRotation<T>(angles, ea_matrix);
+
+ EXPECT_THAT(aa_matrix, IsOrthonormal());
+ EXPECT_THAT(ea_matrix, IsOrthonormal());
+ EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix));
+ }
+};
+
+using EulerSystemList = ::testing::Types<ExtrinsicXYZ,
+ ExtrinsicXYX,
+ ExtrinsicXZY,
+ ExtrinsicXZX,
+ ExtrinsicYZX,
+ ExtrinsicYZY,
+ ExtrinsicYXZ,
+ ExtrinsicYXY,
+ ExtrinsicZXY,
+ ExtrinsicZXZ,
+ ExtrinsicZYX,
+ ExtrinsicZYZ,
+ IntrinsicZYX,
+ IntrinsicXYX,
+ IntrinsicYZX,
+ IntrinsicXZX,
+ IntrinsicXZY,
+ IntrinsicYZY,
+ IntrinsicZXY,
+ IntrinsicYXY,
+ IntrinsicYXZ,
+ IntrinsicZXZ,
+ IntrinsicXYZ,
+ IntrinsicZYZ>;
+TYPED_TEST_SUITE(GeneralEulerAngles, EulerSystemList);
+
+TYPED_TEST(GeneralEulerAngles, EulerAnglesToRotationMatrixAndBack) {
+ std::mt19937 prng;
+ std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
+ for (int i = 0; i < kNumTrials; ++i) {
+ double euler[3];
+ TestFixture::RandomEulerAngles(euler, prng);
+
+ double matrix[9];
+ double round_trip[3];
+ EulerAnglesToRotation<TypeParam>(euler, matrix);
+ ASSERT_THAT(matrix, IsOrthonormal());
+ RotationMatrixToEulerAngles<TypeParam>(matrix, round_trip);
+ for (int j = 0; j < 3; ++j)
+ ASSERT_NEAR(euler[j], round_trip[j], 128.0 * kLooseTolerance);
+ }
+}
+
+// Check that the rotation matrix converted from euler angles is equivalent to
+// product of three principal axis rotation matrices
+// R_euler = R_a2(euler_2) * R_a1(euler_1) * R_a0(euler_0)
+TYPED_TEST(GeneralEulerAngles, PrincipalRotationMatrixProduct) {
+ std::mt19937 prng;
+ double euler[3];
+ for (int i = 0; i < kNumTrials; ++i) {
+ TestFixture::RandomEulerAngles(euler, prng);
+ TestFixture::CheckPrincipalRotationMatrixProduct(euler);
+ }
+}
+
+// Gimbal lock (euler[1] == +/-pi) handling test. If a rotation matrix
+// represents a gimbal-locked configuration, then converting this rotation
+// matrix to euler angles and back must produce the same rotation matrix.
+//
+// From scipy/spatial/transform/test/test_rotation.py, but additionally covers
+// gimbal lock handling for proper euler angles, which scipy appears to fail to
+// do properly.
+TYPED_TEST(GeneralEulerAngles, GimbalLocked) {
+ constexpr auto kBoundaryAngles = TestFixture::kIsProperEuler
+ ? std::array{0.0, kPi}
+ : std::array{-kPi / 2, kPi / 2};
+ constexpr double gimbal_locked_configurations[4][3] = {
+ {0.78539816, kBoundaryAngles[1], 0.61086524},
+ {0.61086524, kBoundaryAngles[0], 0.34906585},
+ {0.61086524, kBoundaryAngles[1], 0.43633231},
+ {0.43633231, kBoundaryAngles[0], 0.26179939}};
+ double angle_estimates[3];
+ double mat_expected[9];
+ double mat_estimated[9];
+ for (const auto& euler_angles : gimbal_locked_configurations) {
+ EulerAnglesToRotation<TypeParam>(euler_angles, mat_expected);
+ RotationMatrixToEulerAngles<TypeParam>(mat_expected, angle_estimates);
+ EulerAnglesToRotation<TypeParam>(angle_estimates, mat_estimated);
+ ASSERT_THAT(mat_expected, IsNear3x3Matrix(mat_estimated));
+ }
+}
+
// Tests using Jets for specific behavior involving auto differentiation
// near singularity points.
