diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h index ccde28eb6..3362d9c3e 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h +++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h @@ -57,10 +57,32 @@ namespace Eigen EulerAngles() {} inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {} - inline EulerAngles(const QuaternionType& q) { *this = q; } - inline EulerAngles(const AngleAxisType& aa) { *this = aa; } + template inline EulerAngles(const MatrixBase& m) { *this = m; } + + template + inline EulerAngles( + const MatrixBase& m, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { + + fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + } + + template + inline EulerAngles(const RotationBase& rot) { *this = rot; } + + template + inline EulerAngles( + const RotationBase& rot, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { + + fromRotation(rot, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + } // TODO: Support assignment from euler to euler @@ -104,65 +126,108 @@ namespace Eigen /** Constructs and \returns an equivalent 3x3 rotation matrix. */ template - // TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) ) - EulerAngles& fromRotationMatrix(const MatrixBase& m) + EulerAngles& fromRotation(const MatrixBase& m) { System::eulerAngles(*this, m); return *this; } - /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1). - */ - template - EulerAngles& operator=(const MatrixBase& mat){ - return fromRotationMatrix(mat); + template< + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll, + typename Derived> + EulerAngles& fromRotation(const MatrixBase& m) + { + System::eulerAngles(*this, m); + return *this; } - - // TODO: Assign and construct from another EulerAngle (with different system) - /** Set \c *this from a quaternion. - * The axis is normalized. - */ - EulerAngles& operator=(const QuaternionType& q){ - // TODO: Implement it in a better way + template + EulerAngles& fromRotation( + const MatrixBase& m, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) + { + System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + return *this; + } + + template + EulerAngles& fromRotation(const RotationBase& rot) + { + return fromRotation(rot.toRotationMatrix()); + } + + template< + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll, + typename Derived> + EulerAngles& fromRotation(const RotationBase& rot) + { + return fromRotation(rot.toRotationMatrix()); + } + + template + EulerAngles& fromRotation( + const RotationBase& rot, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) + { + return fromRotation(rot.toRotationMatrix(), positiveRangeHeading, positiveRangePitch, positiveRangeRoll); + } + + /*EulerAngles& fromQuaternion(const QuaternionType& q) + { + // TODO: Implement it in a faster way for quaternions // According to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ // we can compute only the needed matrix cells and then convert to euler angles. (see ZYX example below) // Currently we compute all matrix cells from quaternion. - fromRotationMatrix(q.toRotationMatrix()); - // Special case only for ZYX - /*Scalar y2 = q.y() * q.y(); - m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); - m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); - m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); + //Scalar y2 = q.y() * q.y(); + //m_angles[0] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); + //m_angles[1] = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); + //m_angles[2] = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); + }*/ + + /** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1). */ - - return *this; + template + EulerAngles& operator=(const MatrixBase& mat) { + return fromRotation(mat); + } + + // TODO: Assign and construct from another EulerAngle (with different system) + + /** Set \c *this from a rotation. + */ + template + EulerAngles& operator=(const RotationBase& rot) { + return fromRotation(rot.toRotationMatrix()); } // TODO: Support isApprox function - - /** Set \c *this from AngleAxis \a ea. - */ - EulerAngles& operator=(const AngleAxisType& ea) - { - // TODO: Implement it in a better way - return *this = ea.toRotationMatrix(); - } - // TODO: Fix this function, and make it generic - Matrix3 toRotationMatrix(void) const + Matrix3 toRotationMatrix() const { return static_cast(*this).toRotationMatrix(); } + + QuaternionType toQuaternion() const + { + return + AngleAxisType(h(), HeadingAxisVector()) * + AngleAxisType(p(), PitchAxisVector()) * + AngleAxisType(r(), RollAxisVector()); + } operator QuaternionType() const { - return - AngleAxisType((System::IsHeadingOpposite ? -1 : 1) * h(), Vector3::Unit(System::HeadingAxisAbs - 1)) * - AngleAxisType((System::IsPitchOpposite ? -1 : 1) * p(), Vector3::Unit(System::PitchAxisAbs - 1)) * - AngleAxisType((System::IsRollOpposite ? -1 : 1) * r(), Vector3::Unit(System::RollAxisAbs - 1)); + return toQuaternion(); } }; diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h index ba33d5400..9699dd10d 100644 --- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h +++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h @@ -19,7 +19,7 @@ namespace Eigen namespace internal { // TODO: Check if already exists on the rest API - template 0)> + template 0)> struct Abs { enum { value = Num }; @@ -73,6 +73,26 @@ namespace Eigen template struct NegateIfXor : NegateIf {}; + + template + struct AddConstIf + { + template + static void run(T& t) + { + t += T(value); + } + }; + + template + struct AddConstIf + { + template + static void run(T&) + { + // no op + } + }; template struct IsValidAxis @@ -196,17 +216,50 @@ namespace Eigen public: template - static void eulerAngles(EulerAngles& res, const typename EulerAngles::Matrix3& mat) + static void eulerAngles( + EulerAngles& res, + const typename EulerAngles::Matrix3& mat) + { + eulerAngles(res, mat, false, false, false); + } + + template< + typename Scalar, + bool PositiveRangeHeading, + bool PositiveRangePitch, + bool PositiveRangeRoll> + static void eulerAngles( + EulerAngles& res, + const typename EulerAngles::Matrix3& mat) + { + eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll); + } + + template + static void eulerAngles( + EulerAngles& res, + const typename EulerAngles::Matrix3& mat, + bool positiveRangeHeading, + bool positiveRangePitch, + bool positiveRangeRoll) { eulerAngles_imp( res.coeffs(), mat, typename internal::conditional::type()); internal::NegateIfXor::run(res.h()); - internal::NegateIfXor::run(res.p()); - internal::NegateIfXor::run(res.r()); + + // Saturate results to the requested range + if (positiveRangeHeading && (res.h() < 0)) + res.h() += Scalar(2 * EIGEN_PI); + + if (positiveRangePitch && (res.p() < 0)) + res.p() += Scalar(2 * EIGEN_PI); + + if (positiveRangeRoll && (res.r() < 0)) + res.r() += Scalar(2 * EIGEN_PI); } }; diff --git a/unsupported/test/EulerAngles.cpp b/unsupported/test/EulerAngles.cpp index 57a34776a..e65399ffa 100644 --- a/unsupported/test/EulerAngles.cpp +++ b/unsupported/test/EulerAngles.cpp @@ -14,14 +14,53 @@ using namespace Eigen; template -void verify_euler(const Matrix& ea) +void verify_euler_ranged(const Matrix& ea, + bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll) { typedef EulerAngles EulerAnglesType; typedef Matrix Matrix3; typedef Matrix Vector3; - typedef AngleAxis AngleAxisx; + typedef Quaternion QuaternionType; + typedef AngleAxis AngleAxisType; using std::abs; + Scalar headingRangeStart, headingRangeEnd; + Scalar pitchRangeStart, pitchRangeEnd; + Scalar rollRangeStart, rollRangeEnd; + + if (positiveRangeHeading) + { + headingRangeStart = Scalar(0); + headingRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + headingRangeStart = -Scalar(EIGEN_PI); + headingRangeEnd = Scalar(EIGEN_PI); + } + + if (positiveRangePitch) + { + pitchRangeStart = Scalar(0); + pitchRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + pitchRangeStart = -Scalar(EIGEN_PI); + pitchRangeEnd = Scalar(EIGEN_PI); + } + + if (positiveRangeRoll) + { + rollRangeStart = Scalar(0); + rollRangeEnd = Scalar(2 * EIGEN_PI); + } + else + { + rollRangeStart = -Scalar(EIGEN_PI); + rollRangeEnd = Scalar(EIGEN_PI); + } + const int i = EulerSystem::HeadingAxisAbs - 1; const int j = EulerSystem::PitchAxisAbs - 1; const int k = EulerSystem::RollAxisAbs - 1; @@ -37,46 +76,80 @@ void verify_euler(const Matrix& ea) EulerAnglesType e(ea[0], ea[1], ea[2]); Matrix3 m(e); - Vector3 eabis = EulerAnglesType(m).coeffs(); + Vector3 eabis = EulerAnglesType(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs(); + + // Check that eabis in range + VERIFY(headingRangeStart <= eabis[0] && eabis[0] <= headingRangeEnd); + VERIFY(pitchRangeStart <= eabis[1] && eabis[1] <= pitchRangeEnd); + VERIFY(rollRangeStart <= eabis[2] && eabis[2] <= rollRangeEnd); + Vector3 eabis2 = m.eulerAngles(i, j, k); + + // Invert the relevant axes eabis2[0] *= iFactor; eabis2[1] *= jFactor; eabis2[2] *= kFactor; + // Saturate the angles to the correct range + if (positiveRangeHeading && (eabis2[0] < 0)) + eabis2[0] += Scalar(2 * EIGEN_PI); + if (positiveRangePitch && (eabis2[1] < 0)) + eabis2[1] += Scalar(2 * EIGEN_PI); + if (positiveRangeRoll && (eabis2[2] < 0)) + eabis2[2] += Scalar(2 * EIGEN_PI); + VERIFY_IS_APPROX(eabis, eabis2);// Verify that our estimation is the same as m.eulerAngles() is - Matrix3 mbis(AngleAxisx(eabis[0], I) * AngleAxisx(eabis[1], J) * AngleAxisx(eabis[2], K)); + Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], 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 || 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] || 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)); + // Tests that are only relevant for no possitive range + if (!(positiveRangeHeading || positiveRangePitch || positiveRangeRoll)) + { + /* 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] || test_isMuchSmallerThan(eabis[0], Scalar(1))); + } + + // Quaternions + QuaternionType q(e); + eabis = EulerAnglesType(q, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs(); + VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same +} + +template +void verify_euler(const Matrix& ea) +{ + verify_euler_ranged(ea, false, false, false); + verify_euler_ranged(ea, false, false, true); + verify_euler_ranged(ea, false, true, false); + verify_euler_ranged(ea, false, true, true); + verify_euler_ranged(ea, true, false, false); + verify_euler_ranged(ea, true, false, true); + verify_euler_ranged(ea, true, true, false); + verify_euler_ranged(ea, true, true, true); } 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); + 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); } template void eulerangles() @@ -85,11 +158,11 @@ template void eulerangles() typedef Matrix Vector3; typedef Array Array3; typedef Quaternion Quaternionx; - typedef AngleAxis AngleAxisx; + typedef AngleAxis AngleAxisType; Scalar a = internal::random(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); Quaternionx q1; - q1 = AngleAxisx(a, Vector3::Random().normalized()); + q1 = AngleAxisType(a, Vector3::Random().normalized()); Matrix3 m; m = q1;