From 0b51f763cbbd0ed08168f88972724329f0375498 Mon Sep 17 00:00:00 2001 From: Rasmus Munk Larsen Date: Thu, 27 Apr 2023 00:06:23 +0000 Subject: [PATCH] Revert "Geometry/EulerAngles: make sure that returned solution has canonical ranges" This reverts commit 7f06bcae2c4aae657fded7c7b999d69ee68962d9 --- Eigen/src/Core/MatrixBase.h | 2 +- Eigen/src/Geometry/EulerAngles.h | 28 ++------------- test/geo_eulerangles.cpp | 61 ++++++++++---------------------- 3 files changed, 21 insertions(+), 70 deletions(-) diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index adb970e06..605a05e84 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, bool canonical = true) const; + inline Matrix eulerAngles(Index a0, Index a1, Index a2) 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 7caedd91c..2b9996018 100644 --- a/Eigen/src/Geometry/EulerAngles.h +++ b/Eigen/src/Geometry/EulerAngles.h @@ -30,19 +30,13 @@ namespace Eigen { * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode * This corresponds to the right-multiply conventions (with right hand side frames). * - * 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. + * The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. * * \sa class AngleAxis */ template EIGEN_DEVICE_FUNC inline Matrix::Scalar,3,1> -MatrixBase::eulerAngles(Index a0, Index a1, Index a2, bool canonical) const +MatrixBase::eulerAngles(Index a0, Index a1, Index a2) const { EIGEN_USING_STD(atan2) EIGEN_USING_STD(sin) @@ -113,24 +107,6 @@ MatrixBase::eulerAngles(Index a0, Index a1, Index a2, bool canonical) c } 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 ba23e1f5f..bea241965 100644 --- a/test/geo_eulerangles.cpp +++ b/test/geo_eulerangles.cpp @@ -20,47 +20,22 @@ void verify_euler(const Matrix& ea, int i, int j, int k) typedef Matrix Vector3; typedef AngleAxis AngleAxisx; using std::abs; - 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)); - } + 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)); } template void check_all_var(const Matrix& ea) @@ -108,8 +83,8 @@ template void eulerangles() ea = m.eulerAngles(0,1,0); check_all_var(ea); - // 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 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_all_var(ea); ea[2] = ea[0] = internal::random(0,Scalar(EIGEN_PI));