diff options
author | Hongkai Dai <daihongkai@gmail.com> | 2016-10-13 14:45:51 -0700 |
---|---|---|
committer | Hongkai Dai <daihongkai@gmail.com> | 2016-10-13 14:45:51 -0700 |
commit | 014d9f1d9b60206deaeb7ac5349816cb556fb35b (patch) | |
tree | f53b6806c5adb6e4b530a7536b9077d1d47d64ae /unsupported/Eigen/src/EulerAngles | |
parent | 26f99075425cd9bf1db31d6d76a5b08570162bd2 (diff) |
implement euler angles with the right ranges
Diffstat (limited to 'unsupported/Eigen/src/EulerAngles')
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerAngles.h | 26 | ||||
-rw-r--r-- | unsupported/Eigen/src/EulerAngles/EulerSystem.h | 137 |
2 files changed, 78 insertions, 85 deletions
diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index 13a0da1ab..a737a221a 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -79,8 +79,8 @@ namespace Eigen * * ##### run-time time ranges ##### * Run-time ranges are also supported. - * \sa EulerAngles(const MatrixBase<Derived>&, bool, bool, bool) - * \sa EulerAngles(const RotationBase<Derived, 3>&, bool, bool, bool) + * \sa EulerAngles(const MatrixBase<Derived>&, bool, bool) + * \sa EulerAngles(const RotationBase<Derived, 3>&, bool, bool) * * ### Convenient user typedefs ### * @@ -160,22 +160,24 @@ namespace Eigen /** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m, * with options to choose for each angle the requested range. * - * If positive range is true, then the specified angle will be in the range [0, +2*PI]. + * For angle alpha and gamma, if positive range is true, then the + * specified angle will be in the range [0, +2*PI]. * Otherwise, the specified angle will be in the range [-PI, +PI]. + * For angle beta, depending on whether AlphaAxis is the same as GammaAxis + * if AlphaAxis is the same as Gamma ais, then the range of beta is [0, PI]; + * otherwise the range of beta is [-PI/2, PI/2] * * \param m The 3x3 rotation matrix to convert * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. */ template<typename Derived> EulerAngles( const MatrixBase<Derived>& m, bool positiveRangeAlpha, - bool positiveRangeBeta, bool positiveRangeGamma) { - System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); + System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeGamma); } /** Constructs and initialize Euler angles from a rotation \p rot. @@ -195,17 +197,15 @@ namespace Eigen * * \param rot The 3x3 rotation matrix to convert * \param positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \param positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. * \param positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. */ template<typename Derived> EulerAngles( const RotationBase<Derived, 3>& rot, bool positiveRangeAlpha, - bool positiveRangeBeta, bool positiveRangeGamma) { - System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma); + System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeGamma); } /** \returns The angle values stored in a vector (alpha, beta, gamma). */ @@ -254,12 +254,10 @@ namespace Eigen * * \param m The 3x3 rotation matrix to convert * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. */ template< bool PositiveRangeAlpha, - bool PositiveRangeBeta, bool PositiveRangeGamma, typename Derived> static EulerAngles FromRotation(const MatrixBase<Derived>& m) @@ -268,7 +266,7 @@ namespace Eigen EulerAngles e; System::template CalcEulerAngles< - PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m); + PositiveRangeAlpha, PositiveRangeGamma, _Scalar>(e, m); return e; } @@ -280,17 +278,15 @@ namespace Eigen * * \param rot The 3x3 rotation matrix to convert * \tparam positiveRangeAlpha If true, alpha will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. - * \tparam positiveRangeBeta If true, beta will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. * \tparam positiveRangeGamma If true, gamma will be in [0, 2*PI]. Otherwise, in [-PI, +PI]. */ template< bool PositiveRangeAlpha, - bool PositiveRangeBeta, bool PositiveRangeGamma, typename Derived> static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot) { - return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix()); + return FromRotation<PositiveRangeAlpha, PositiveRangeGamma>(rot.toRotationMatrix()); } /*EulerAngles& fromQuaternion(const QuaternionType& q) diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index 98f9f647d..76d0b7c57 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -112,9 +112,9 @@ namespace Eigen * * \tparam _AlphaAxis the first fixed EulerAxis * - * \tparam _AlphaAxis the second fixed EulerAxis + * \tparam _BetaAxis the second fixed EulerAxis * - * \tparam _AlphaAxis the third fixed EulerAxis + * \tparam _GammaAxis the third fixed EulerAxis */ template <int _AlphaAxis, int _BetaAxis, int _GammaAxis> class EulerSystem @@ -138,14 +138,16 @@ namespace Eigen BetaAxisAbs = internal::Abs<BetaAxis>::value, /*!< the second rotation axis unsigned */ GammaAxisAbs = internal::Abs<GammaAxis>::value, /*!< the third rotation axis unsigned */ - IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< weather alpha axis is negative */ - IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< weather beta axis is negative */ - IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< weather gamma axis is negative */ - - IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */ - IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */ + IsAlphaOpposite = (AlphaAxis < 0) ? 1 : 0, /*!< whether alpha axis is negative */ + IsBetaOpposite = (BetaAxis < 0) ? 1 : 0, /*!< whether beta axis is negative */ + IsGammaOpposite = (GammaAxis < 0) ? 1 : 0, /*!< whether gamma axis is negative */ + + // Parity is even if alpha axis X is followed by beta axis Y, or Y is followed + // by Z, or Z is followed by X; otherwise it is odd. + IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< whether the Euler system is odd */ + IsEven = IsOdd ? 0 : 1, /*!< whether the Euler system is even */ - IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */ + IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< whether the Euler system is tait bryan */ }; private: @@ -180,71 +182,70 @@ namespace Eigen static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>& res, const MatrixBase<Derived>& mat, internal::true_type /*isTaitBryan*/) { using std::atan2; - using std::sin; - using std::cos; + using std::sqrt; typedef typename Derived::Scalar Scalar; - typedef Matrix<Scalar,2,1> Vector2; - - res[0] = atan2(mat(J,K), mat(K,K)); - Scalar c2 = Vector2(mat(I,I), mat(I,J)).norm(); - if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) { - if(res[0] > Scalar(0)) { - res[0] -= Scalar(EIGEN_PI); - } - else { - res[0] += Scalar(EIGEN_PI); - } - res[1] = atan2(-mat(I,K), -c2); + + Scalar plusMinus = IsEven? 1 : -1; + 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); + res[1] = atan2(plusMinus * mat(I,K), Rsum); + + // There is a singularity when cos(beta) = 0 + if(Rsum > 4 * NumTraits<Scalar>::epsilon()) { + 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); + Scalar alphaPlusMinusGamma = atan2(spos, cpos); + res[0] = alphaPlusMinusGamma; + res[2] = 0; + } + else { + 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); + res[0] = alphaMinusPlusBeta; + res[2] = 0; } - else - res[1] = atan2(-mat(I,K), c2); - Scalar s1 = sin(res[0]); - Scalar c1 = cos(res[0]); - res[2] = atan2(s1*mat(K,I)-c1*mat(J,I), c1*mat(J,J) - s1 * mat(K,J)); } template <typename Derived> - static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) + static void CalcEulerAngles_imp(Matrix<typename MatrixBase<Derived>::Scalar,3,1>& res, + const MatrixBase<Derived>& mat, internal::false_type /*isTaitBryan*/) { using std::atan2; - using std::sin; - using std::cos; + using std::sqrt; typedef typename Derived::Scalar Scalar; - typedef Matrix<Scalar,2,1> Vector2; - - res[0] = atan2(mat(J,I), mat(K,I)); - if((IsOdd && res[0]<Scalar(0)) || ((!IsOdd) && res[0]>Scalar(0))) - { - if(res[0] > Scalar(0)) { - res[0] -= Scalar(EIGEN_PI); - } - else { - res[0] += Scalar(EIGEN_PI); - } - Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); - res[1] = -atan2(s2, mat(I,I)); + + Scalar plusMinus = IsEven? 1 : -1; + 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); + + res[1] = atan2(Rsum, mat(I, I)); + + if(Rsum > 4 * NumTraits<Scalar>::epsilon()) { + res[0] = atan2(mat(J, I), minusPlus * mat(K, I)); + res[2] = atan2(mat(I, J), plusMinus * mat(I, K)); } - else - { - Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm(); - res[1] = atan2(s2, mat(I,I)); + else if( mat(I, I) > 0) { + 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 { + 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; } - // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, - // we can compute their respective rotation, and apply its inverse to M. Since the result must - // be a rotation around x, we have: - // - // c2 s1.s2 c1.s2 1 0 0 - // 0 c1 -s1 * M = 0 c3 s3 - // -s2 s1.c2 c1.c2 0 -s3 c3 - // - // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 - - Scalar s1 = sin(res[0]); - Scalar c1 = cos(res[0]); - res[2] = atan2(c1*mat(J,K)-s1*mat(K,K), c1*mat(J,J) - s1 * mat(K,J)); } template<typename Scalar> @@ -257,14 +258,13 @@ namespace Eigen template< bool PositiveRangeAlpha, - bool PositiveRangeBeta, bool PositiveRangeGamma, typename Scalar> static void CalcEulerAngles( EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat) { - CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma); + CalcEulerAngles(res, mat, PositiveRangeAlpha, PositiveRangeGamma); } template<typename Scalar> @@ -272,28 +272,25 @@ namespace Eigen EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat, bool PositiveRangeAlpha, - bool PositiveRangeBeta, bool PositiveRangeGamma) { CalcEulerAngles_imp( res.angles(), mat, typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type()); - if (IsAlphaOpposite == IsOdd) + if (IsAlphaOpposite) res.alpha() = -res.alpha(); - if (IsBetaOpposite == IsOdd) + if (IsBetaOpposite) res.beta() = -res.beta(); - if (IsGammaOpposite == IsOdd) + if (IsGammaOpposite) res.gamma() = -res.gamma(); // Saturate results to the requested range if (PositiveRangeAlpha && (res.alpha() < 0)) res.alpha() += Scalar(2 * EIGEN_PI); - - if (PositiveRangeBeta && (res.beta() < 0)) - res.beta() += Scalar(2 * EIGEN_PI); + if (PositiveRangeGamma && (res.gamma() < 0)) res.gamma() += Scalar(2 * EIGEN_PI); |