From 7f06bcae2c4aae657fded7c7b999d69ee68962d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Wed, 19 Apr 2023 19:12:24 +0000 Subject: [PATCH] Geometry/EulerAngles: make sure that returned solution has canonical ranges --- Eigen/src/Core/MatrixBase.h | 2 +- Eigen/src/Geometry/EulerAngles.h | 28 +++++++++++++-- test/geo_eulerangles.cpp | 61 ++++++++++++++++++++++---------- 3 files changed, 70 insertions(+), 21 deletions(-) diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index 605a05e84..adb970e06 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h @@ -400,7 +400,7 @@ template class MatrixBase inline PlainObject unitOrthogonal(void) const; EIGEN_DEVICE_FUNC - inline Matrix eulerAngles(Index a0, Index a1, Index a2) const; + inline Matrix eulerAngles(Index a0, Index a1, Index a2, bool canonical = true) const; // put this as separate enum value to work around possible GCC 4.3 bug (?) enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical) diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h index 2b9996018..7caedd91c 100644 --- a/Eigen/src/Geometry/EulerAngles.h +++ b/Eigen/src/Geometry/EulerAngles.h @@ -30,13 +30,19 @@ namespace Eigen { * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode * This corresponds to the right-multiply conventions (with right hand side frames). * - * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. + * When canonical == true (the default): + * For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]. + * For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi]. + * + * When canonical == false: + * The returned angles follow a non-standard range convention used by legacy versions of Eigen, [0:pi]x[-pi:pi]x[-pi:pi]. + * Set canonical to false to retain legacy behaviour. * * \sa class AngleAxis */ template EIGEN_DEVICE_FUNC inline Matrix::Scalar,3,1> -MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const +MatrixBase::eulerAngles(Index a0, Index a1, Index a2, bool canonical) const { EIGEN_USING_STD(atan2) EIGEN_USING_STD(sin) @@ -107,6 +113,24 @@ MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const } if (!odd) res = -res; + + if (canonical) + { + // If Tait-Bryan angles, make sure that the result is in the canonical range (middle axis angle in [-pi/2, pi/2]). + if (a0 != a2 && res.cwiseAbs()[1] > Scalar(EIGEN_PI / 2)) + { + res -= Scalar(EIGEN_PI) * res.cwiseSign(); + res[1] = -res[1]; + } + + // If proper Euler angles, make sure that the result is in the canonical range (middle axis angle in [0, pi]). + if (a0 == a2 && res[1] < Scalar(0)) + { + res[0] -= Scalar(EIGEN_PI) * res.cwiseSign()[0]; + res[2] -= Scalar(EIGEN_PI) * res.cwiseSign()[2]; + res[1] = -res[1]; + } + } return res; } diff --git a/test/geo_eulerangles.cpp b/test/geo_eulerangles.cpp index bea241965..ba23e1f5f 100644 --- a/test/geo_eulerangles.cpp +++ b/test/geo_eulerangles.cpp @@ -20,22 +20,47 @@ void verify_euler(const Matrix& ea, int i, int j, int k) typedef Matrix Vector3; typedef AngleAxis AngleAxisx; using std::abs; - Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); - Vector3 eabis = m.eulerAngles(i, j, k); - Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); - VERIFY_IS_APPROX(m, mbis); - /* If I==K, and ea[1]==0, then there no unique solution. */ - /* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ - if((i!=k || !numext::is_exactly_zero(ea[1])) && (i == k || !internal::isApprox(abs(ea[1]), Scalar(EIGEN_PI / 2), test_precision())) ) - VERIFY((ea-eabis).norm() <= test_precision()); - - // approx_or_less_than does not work for 0 - VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); - VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); - VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); - VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); + const Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); + + // Test the new default canonical ranges behaviour of eulerAngles (canonical = true) + { + Vector3 eabis = m.eulerAngles(i, j, k); + Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); + VERIFY_IS_APPROX(m, mbis); + + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[0]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); + if (i != k) + { + // Tait-Bryan sequence + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI / 2), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI / 2)); + } + else + { + // Proper Euler sequence + // approx_or_less_than does not work for 0 + VERIFY(0 < eabis[1] || test_isMuchSmallerThan(eabis[1], Scalar(1))); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); + } + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); + } + + // Test legacy behaviour of eulerAngles (canonical = false) + { + Vector3 eabis = m.eulerAngles(i, j, k, false); + Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k))); + VERIFY_IS_APPROX(m, mbis); + + // approx_or_less_than does not work for 0 + VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); + VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); + VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); + } } template void check_all_var(const Matrix& ea) @@ -83,8 +108,8 @@ template void eulerangles() ea = m.eulerAngles(0,1,0); check_all_var(ea); - // Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. - ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1); + // Check with random angles in range [-pi:pi]x[-pi:pi]x[-pi:pi]. + ea = Array3::Random() * Scalar(EIGEN_PI)*Array3(1,1,1); check_all_var(ea); ea[2] = ea[0] = internal::random(0,Scalar(EIGEN_PI));