From 58f5d7d058e21bec85d902504efe988d17aa28cf Mon Sep 17 00:00:00 2001 From: Tal Hadad Date: Sun, 16 Oct 2016 14:39:26 +0300 Subject: Fix calc bug, docs and better testing. Test code changes: * better coded * rand and manual numbers * singularity checking --- unsupported/test/EulerAngles.cpp | 175 +++++++++++++++++++++++++++++---------- 1 file changed, 130 insertions(+), 45 deletions(-) (limited to 'unsupported/test/EulerAngles.cpp') 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 -void verify_euler(const Matrix& ea) +const char X = EULER_X; +const char Y = EULER_Y; +const char Z = EULER_Z; + +template +void verify_euler(const EulerAngles& e) { typedef EulerAngles EulerAnglesType; typedef Matrix Matrix3; @@ -41,17 +45,24 @@ void verify_euler(const Matrix& 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(m).angles(); @@ -60,8 +71,16 @@ void verify_euler(const Matrix& 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& 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())) ) - VERIFY((ea-eabis).norm() <= test_precision()); - - // 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())) ) + VERIFY_IS_APPROX(ea, eabis);*/ // Quaternions - QuaternionType q(e); + const QuaternionType q(e); eabis = static_cast(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 +void verify_euler_vec(const Matrix& ea) +{ + verify_euler(EulerAngles >(ea[0], ea[1], ea[2])); +} + +template +void verify_euler_all_neg(const Matrix& 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 void check_all_var(const Matrix& ea) { - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - verify_euler(ea); - - // TODO: Test negative axes as well! (only test if the angles get negative when needed) + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); + verify_euler_all_neg(ea); +} + +template void check_singular_cases(const Scalar& singularBeta) +{ + typedef Matrix Vector3; + const Scalar epsilon = std::numeric_limits::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 void eulerangles_manual() +{ + typedef Matrix 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 void eulerangles() +template void eulerangles_rand() { typedef Matrix Matrix3; typedef Matrix Vector3; @@ -158,8 +237,14 @@ template void eulerangles() void test_EulerAngles() { + CALL_SUBTEST_1( eulerangles_manual() ); + CALL_SUBTEST_2( eulerangles_manual() ); + for(int i = 0; i < g_repeat; i++) { - CALL_SUBTEST_1( eulerangles() ); - CALL_SUBTEST_2( eulerangles() ); + CALL_SUBTEST_3( eulerangles_rand() ); + CALL_SUBTEST_4( eulerangles_rand() ); } + + // TODO: Add tests for auto diff + // TODO: Add tests for complex numbers } -- cgit v1.2.3