diff options
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerAngles.h | 28 | ||||
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerSystem.h | 36 | ||||
-rw-r--r-- | unsupported/test/EulerAngles.cpp | 175 |
3 files changed, 163 insertions, 76 deletions
diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index da86cc13b..8a723d9ee 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -36,7 +36,7 @@ namespace Eigen * ### Rotation representation and conversions ### * * It has been proved(see Wikipedia link below) that every rotation can be represented - * by Euler angles, but there is no singular representation (e.g. unlike rotation matrices). + * by Euler angles, but there is no single representation (e.g. unlike rotation matrices). * Therefore, you can convert from Eigen rotation and to them * (including rotation matrices, which is not called "rotations" by Eigen design). * @@ -55,10 +55,12 @@ namespace Eigen * Additionally, some axes related computation is done in compile time. * * #### Euler angles ranges in conversions #### - * Rotations representation as EulerAngles are not singular (unlike matrices), and even have infinite EulerAngles representations.<BR> + * Rotations representation as EulerAngles are not single (unlike matrices), + * and even have infinite EulerAngles representations.<BR> * For example, add or subtract 2*PI from either angle of EulerAngles * and you'll get the same rotation. - * This is the reason for infinite representation, but it's not the only reason for non-singularity. + * This is the general reason for infinite representation, + * but it's not the only general reason for not having a single representation. * * When converting rotation to EulerAngles, this class convert it to specific ranges * When converting some rotation to EulerAngles, the rules for ranges are as follow: @@ -66,10 +68,10 @@ namespace Eigen * (even when it represented as RotationBase explicitly), angles ranges are __undefined__. * - otherwise, Alpha and Gamma angles will be in the range [-PI, PI].<BR> * As for Beta angle: - * - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI]. + * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2]. * - otherwise: - * - If the beta axis is positive, the beta angle will be in the range [0, 2*PI] - * - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0] + * - If the beta axis is positive, the beta angle will be in the range [0, PI] + * - If the beta axis is negative, the beta angle will be in the range [-PI, 0] * * \sa EulerAngles(const MatrixBase<Derived>&) * \sa EulerAngles(const RotationBase<Derived, 3>&) @@ -95,7 +97,7 @@ namespace Eigen * * More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles * - * \tparam _Scalar the scalar type, i.e., the type of the angles. + * \tparam _Scalar the scalar type, i.e. the type of the angles. * * \tparam _System the EulerSystem to use, which represents the axes of rotation. */ @@ -146,10 +148,10 @@ namespace Eigen * * \note Alpha and Gamma angles will be in the range [-PI, PI].<BR> * As for Beta angle: - * - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI]. + * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2]. * - otherwise: - * - If the beta axis is positive, the beta angle will be in the range [0, 2*PI] - * - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0] + * - If the beta axis is positive, the beta angle will be in the range [0, PI] + * - If the beta axis is negative, the beta angle will be in the range [-PI, 0] */ template<typename Derived> EulerAngles(const MatrixBase<Derived>& m) { System::CalcEulerAngles(*this, m); } @@ -160,10 +162,10 @@ namespace Eigen * angles ranges are __undefined__. * Otherwise, Alpha and Gamma angles will be in the range [-PI, PI].<BR> * As for Beta angle: - * - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI]. + * - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2]. * - otherwise: - * - If the beta axis is positive, the beta angle will be in the range [0, 2*PI] - * - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0] + * - If the beta axis is positive, the beta angle will be in the range [0, PI] + * - If the beta axis is negative, the beta angle will be in the range [-PI, 0] */ template<typename Derived> EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); } diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index aa96461f9..0790e612f 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -18,7 +18,7 @@ namespace Eigen namespace internal { - // TODO: Check if already exists on the rest API + // TODO: Add this trait to the Eigen internal API? template <int Num, bool IsPositive = (Num > 0)> struct Abs { @@ -186,25 +186,25 @@ namespace Eigen typedef typename Derived::Scalar Scalar; - Scalar plusMinus = IsEven? 1 : -1; - Scalar minusPlus = IsOdd? 1 : -1; + const Scalar plusMinus = IsEven? 1 : -1; + const Scalar minusPlus = IsOdd? 1 : -1; - Scalar Rsum = sqrt((mat(I,I) * mat(I,I) + mat(I,J) * mat(I,J) + mat(J,K) * mat(J,K) + mat(K,K) * mat(K,K))/2); + const Scalar Rsum = sqrt((mat(I,I) * mat(I,I) + mat(I,J) * mat(I,J) + mat(J,K) * mat(J,K) + mat(K,K) * mat(K,K))/2); res[1] = atan2(plusMinus * mat(I,K), Rsum); - // There is a singularity when cos(beta) = 0 - if(Rsum > 4 * NumTraits<Scalar>::epsilon()) { + // There is a singularity when cos(beta) == 0 + if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0 res[0] = atan2(minusPlus * mat(J, K), mat(K, K)); res[2] = atan2(minusPlus * mat(I, J), mat(I, I)); } - else if(plusMinus * mat(I, K) > 0) { - Scalar spos = mat(J, I) + plusMinus * mat(K, J); // 2*sin(alpha + plusMinus * gamma) - Scalar cpos = mat(J, J) + minusPlus * mat(K, I); // 2*cos(alpha + plusMinus * gamma); + else if(plusMinus * mat(I, K) > 0) {// cos(beta) == 0 and sin(beta) == 1 + Scalar spos = mat(J, I) + plusMinus * mat(K, J); // 2*sin(alpha + plusMinus * gamma + Scalar cpos = mat(J, J) + minusPlus * mat(K, I); // 2*cos(alpha + plusMinus * gamma) Scalar alphaPlusMinusGamma = atan2(spos, cpos); res[0] = alphaPlusMinusGamma; res[2] = 0; } - else { + else {// cos(beta) == 0 and sin(beta) == -1 Scalar sneg = plusMinus * (mat(K, J) + minusPlus * mat(J, I)); // 2*sin(alpha + minusPlus*gamma) Scalar cneg = mat(J, J) + plusMinus * mat(K, I); // 2*cos(alpha + minusPlus*gamma) Scalar alphaMinusPlusBeta = atan2(sneg, cneg); @@ -222,30 +222,30 @@ namespace Eigen typedef typename Derived::Scalar Scalar; - Scalar plusMinus = IsEven? 1 : -1; - Scalar minusPlus = IsOdd? 1 : -1; + const Scalar plusMinus = IsEven? 1 : -1; + const Scalar minusPlus = IsOdd? 1 : -1; - Scalar Rsum = sqrt((mat(I, J) * mat(I, J) + mat(I, K) * mat(I, K) + mat(J, I) * mat(J, I) + mat(K, I) * mat(K, I)) / 2); + const Scalar Rsum = sqrt((mat(I, J) * mat(I, J) + mat(I, K) * mat(I, K) + mat(J, I) * mat(J, I) + mat(K, I) * mat(K, I)) / 2); res[1] = atan2(Rsum, mat(I, I)); - if(Rsum > 4 * NumTraits<Scalar>::epsilon()) { + // There is a singularity when sin(beta) == 0 + if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0 res[0] = atan2(mat(J, I), minusPlus * mat(K, I)); res[2] = atan2(mat(I, J), plusMinus * mat(I, K)); } - else if( mat(I, I) > 0) { + else if(mat(I, I) > 0) {// sin(beta) == 0 and cos(beta) == 1 Scalar spos = plusMinus * mat(K, J) + minusPlus * mat(J, K); // 2*sin(alpha + gamma) Scalar cpos = mat(J, J) + mat(K, K); // 2*cos(alpha + gamma) res[0] = atan2(spos, cpos); res[2] = 0; } - else { + else {// sin(beta) == 0 and cos(beta) == -1 Scalar sneg = plusMinus * mat(K, J) + plusMinus * mat(J, K); // 2*sin(alpha - gamma) Scalar cneg = mat(J, J) - mat(K, K); // 2*cos(alpha - gamma) res[0] = atan2(sneg, cneg); - res[1] = 0; + res[2] = 0; } - } template<typename Scalar> diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp index 8b4706686..43291b31d 100644 --- a/unsupported/test/EulerAngles.cpp +++ b/unsupported/test/EulerAngles.cpp @@ -15,13 +15,17 @@ using namespace Eigen; // Verify that x is in the approxed range [a, b] #define VERIFY_APPROXED_RANGE(a, x, b) \ - do { \ - VERIFY_IS_APPROX_OR_LESS_THAN(a, x); \ - VERIFY_IS_APPROX_OR_LESS_THAN(x, b); \ - } while(0) + do { \ + VERIFY_IS_APPROX_OR_LESS_THAN(a, x); \ + VERIFY_IS_APPROX_OR_LESS_THAN(x, b); \ + } while(0) -template<typename EulerSystem, typename Scalar> -void verify_euler(const Matrix<Scalar,3,1>& ea) +const char X = EULER_X; +const char Y = EULER_Y; +const char Z = EULER_Z; + +template<typename Scalar, typename EulerSystem> +void verify_euler(const EulerAngles<Scalar, EulerSystem>& e) { typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType; typedef Matrix<Scalar,3,3> Matrix3; @@ -41,17 +45,24 @@ void verify_euler(const Matrix<Scalar,3,1>& ea) } else { - betaRangeStart = -PI; - betaRangeEnd = PI; + if (!EulerSystem::IsBetaOpposite) + { + betaRangeStart = 0; + betaRangeEnd = PI; + } + else + { + betaRangeStart = -PI; + betaRangeEnd = 0; + } } const Vector3 I = EulerAnglesType::AlphaAxisVector(); const Vector3 J = EulerAnglesType::BetaAxisVector(); const Vector3 K = EulerAnglesType::GammaAxisVector(); - - EulerAnglesType e(ea[0], ea[1], ea[2]); - Matrix3 m(e); + const Matrix3 m(e); + VERIFY_IS_APPROX(Scalar(m.determinant()), ONE); Vector3 eabis = static_cast<EulerAnglesType>(m).angles(); @@ -60,8 +71,16 @@ void verify_euler(const Matrix<Scalar,3,1>& ea) VERIFY_APPROXED_RANGE(betaRangeStart, eabis[1], betaRangeEnd); VERIFY_APPROXED_RANGE(-PI, eabis[2], PI); - Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); - VERIFY_IS_APPROX(m, mbis); + const Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); + VERIFY_IS_APPROX(Scalar(mbis.determinant()), ONE); + /*std::cout << "===================\n" << + "e: " << e << std::endl << + "eabis: " << eabis.transpose() << std::endl << + "m: " << m << std::endl << + "mbis: " << mbis << std::endl << + "X: " << (m * Vector3::UnitX()).transpose() << std::endl << + "X: " << (mbis * Vector3::UnitX()).transpose() << std::endl;*/ + VERIFY_IS_APPROX(m, mbis); // Test if ea and eabis are the same // Need to check both singular and non-singular cases @@ -69,47 +88,107 @@ void verify_euler(const Matrix<Scalar,3,1>& ea) // 1. When I==K and sin(ea(1)) == 0 // 2. When I!=K and cos(ea(1)) == 0 - // Tests that are only relevant for no positive range - /*if (!(positiveRangeAlpha || positiveRangeGamma)) - { - // 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 || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) ) - VERIFY((ea-eabis).norm() <= test_precision<Scalar>()); - - // approx_or_less_than does not work for 0 - VERIFY(0 < eabis[0] || VERIFY_IS_MUCH_SMALLER_THAN(eabis[0], Scalar(1))); - }*/ + // TODO: Make this test work well, and use range saturation function. + /*// 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 || ea[1]!=0) && (i==k || !internal::isApprox(abs(ea[1]),Scalar(EIGEN_PI/2),test_precision<Scalar>())) ) + VERIFY_IS_APPROX(ea, eabis);*/ // Quaternions - QuaternionType q(e); + const QuaternionType q(e); eabis = static_cast<EulerAnglesType>(q).angles(); - QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); + const QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K)); VERIFY_IS_APPROX(std::abs(q.dot(qbis)), ONE); //VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same } +template<signed char A, signed char B, signed char C, typename Scalar> +void verify_euler_vec(const Matrix<Scalar,3,1>& ea) +{ + verify_euler(EulerAngles<Scalar, EulerSystem<A, B, C> >(ea[0], ea[1], ea[2])); +} + +template<signed char A, signed char B, signed char C, typename Scalar> +void verify_euler_all_neg(const Matrix<Scalar,3,1>& ea) +{ + verify_euler_vec<+A,+B,+C>(ea); + verify_euler_vec<+A,+B,-C>(ea); + verify_euler_vec<+A,-B,+C>(ea); + verify_euler_vec<+A,-B,-C>(ea); + + verify_euler_vec<-A,+B,+C>(ea); + verify_euler_vec<-A,+B,-C>(ea); + verify_euler_vec<-A,-B,+C>(ea); + verify_euler_vec<-A,-B,-C>(ea); +} + template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) { - verify_euler<EulerSystemXYZ>(ea); - verify_euler<EulerSystemXYX>(ea); - verify_euler<EulerSystemXZY>(ea); - verify_euler<EulerSystemXZX>(ea); - - verify_euler<EulerSystemYZX>(ea); - verify_euler<EulerSystemYZY>(ea); - verify_euler<EulerSystemYXZ>(ea); - verify_euler<EulerSystemYXY>(ea); - - verify_euler<EulerSystemZXY>(ea); - verify_euler<EulerSystemZXZ>(ea); - verify_euler<EulerSystemZYX>(ea); - verify_euler<EulerSystemZYZ>(ea); - - // TODO: Test negative axes as well! (only test if the angles get negative when needed) + verify_euler_all_neg<X,Y,Z>(ea); + verify_euler_all_neg<X,Y,X>(ea); + verify_euler_all_neg<X,Z,Y>(ea); + verify_euler_all_neg<X,Z,X>(ea); + + verify_euler_all_neg<Y,Z,X>(ea); + verify_euler_all_neg<Y,Z,Y>(ea); + verify_euler_all_neg<Y,X,Z>(ea); + verify_euler_all_neg<Y,X,Y>(ea); + + verify_euler_all_neg<Z,X,Y>(ea); + verify_euler_all_neg<Z,X,Z>(ea); + verify_euler_all_neg<Z,Y,X>(ea); + verify_euler_all_neg<Z,Y,Z>(ea); +} + +template<typename Scalar> void check_singular_cases(const Scalar& singularBeta) +{ + typedef Matrix<Scalar,3,1> Vector3; + const Scalar epsilon = std::numeric_limits<Scalar>::epsilon(); + const Scalar PI = Scalar(EIGEN_PI); + + check_all_var(Vector3(PI/4, singularBeta, PI/3)); + check_all_var(Vector3(PI/4, singularBeta - epsilon, PI/3)); + check_all_var(Vector3(PI/4, singularBeta - Scalar(1.5)*epsilon, PI/3)); + check_all_var(Vector3(PI/4, singularBeta - 2*epsilon, PI/3)); + check_all_var(Vector3(PI*Scalar(0.8), singularBeta - epsilon, Scalar(0.9)*PI)); + check_all_var(Vector3(PI*Scalar(-0.9), singularBeta + epsilon, PI*Scalar(0.3))); + check_all_var(Vector3(PI*Scalar(-0.6), singularBeta + Scalar(1.5)*epsilon, PI*Scalar(0.3))); + check_all_var(Vector3(PI*Scalar(-0.5), singularBeta + 2*epsilon, PI*Scalar(0.4))); + check_all_var(Vector3(PI*Scalar(0.9), singularBeta + epsilon, Scalar(0.8)*PI)); +} + +template<typename Scalar> void eulerangles_manual() +{ + typedef Matrix<Scalar,3,1> Vector3; + const Vector3 Zero = Vector3::Zero(); + const Scalar PI = Scalar(EIGEN_PI); + + check_all_var(Zero); + + // singular cases + check_singular_cases(PI/2); + check_singular_cases(-PI/2); + + check_singular_cases(Scalar(0)); + check_singular_cases(Scalar(-0)); + + check_singular_cases(PI); + check_singular_cases(-PI); + + // non-singular cases + VectorXd alpha = VectorXd::LinSpaced(Eigen::Sequential, 20, Scalar(-0.99) * PI, PI); + VectorXd beta = VectorXd::LinSpaced(Eigen::Sequential, 20, Scalar(-0.49) * PI, Scalar(0.49) * PI); + VectorXd gamma = VectorXd::LinSpaced(Eigen::Sequential, 20, Scalar(-0.99) * PI, PI); + for (int i = 0; i < alpha.size(); ++i) { + for (int j = 0; j < beta.size(); ++j) { + for (int k = 0; k < gamma.size(); ++k) { + check_all_var(Vector3d(alpha(i), beta(j), gamma(k))); + } + } + } } -template<typename Scalar> void eulerangles() +template<typename Scalar> void eulerangles_rand() { typedef Matrix<Scalar,3,3> Matrix3; typedef Matrix<Scalar,3,1> Vector3; @@ -158,8 +237,14 @@ template<typename Scalar> void eulerangles() void test_EulerAngles() { + CALL_SUBTEST_1( eulerangles_manual<float>() ); + CALL_SUBTEST_2( eulerangles_manual<double>() ); + for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( eulerangles<float>() ); - CALL_SUBTEST_2( eulerangles<double>() ); + CALL_SUBTEST_3( eulerangles_rand<float>() ); + CALL_SUBTEST_4( eulerangles_rand<double>() ); } + + // TODO: Add tests for auto diff + // TODO: Add tests for complex numbers } |