devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

implement euler angles with the right ranges

Browse Source
This commit is contained in:
Hongkai Dai
2016-10-13 14:45:51 -07:00
parent 26f9907542
commit 014d9f1d9b
3 changed files with 113 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
unsupported/test/EulerAngles.cpp
View File

@@ -15,7 +15,7 @@ using namespace Eigen;
template<typename EulerSystem, typename Scalar>
void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
bool positiveRangeAlpha, bool positiveRangeGamma)
{
typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
typedef Matrix<Scalar,3,3> Matrix3;
@@ -39,10 +39,10 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
alphaRangeEnd = Scalar(EIGEN_PI);
}
if (positiveRangeBeta)
if (EulerSystem::IsTaitBryan)
{
betaRangeStart = Scalar(0);
betaRangeEnd = Scalar(2 * EIGEN_PI);
betaRangeStart = -Scalar(EIGEN_PI / 2);
betaRangeEnd = Scalar(EIGEN_PI / 2);
}
else
{
@@ -61,77 +61,70 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
gammaRangeEnd = Scalar(EIGEN_PI);
}
const int i = EulerSystem::AlphaAxisAbs - 1;
/*const int i = EulerSystem::AlphaAxisAbs - 1;
const int j = EulerSystem::BetaAxisAbs - 1;
const int k = EulerSystem::GammaAxisAbs - 1;
const int iFactor = EulerSystem::IsAlphaOpposite ? -1 : 1;
const int jFactor = EulerSystem::IsBetaOpposite ? -1 : 1;
const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;
const int kFactor = EulerSystem::IsGammaOpposite ? -1 : 1;*/
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);
Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeGamma).angles();
// Check that eabis in range
VERIFY(alphaRangeStart <= eabis[0] && eabis[0] <= alphaRangeEnd);
VERIFY(betaRangeStart <= eabis[1] && eabis[1] <= betaRangeEnd);
VERIFY(gammaRangeStart <= eabis[2] && eabis[2] <= gammaRangeEnd);
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 (positiveRangeAlpha && (eabis2[0] < 0))
eabis2[0] += Scalar(2 * EIGEN_PI);
if (positiveRangeBeta && (eabis2[1] < 0))
eabis2[1] += Scalar(2 * EIGEN_PI);
if (positiveRangeGamma && (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(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
VERIFY_IS_APPROX(m, mbis);
// Tests that are only relevant for no possitive range
if (!(positiveRangeAlpha || positiveRangeBeta || positiveRangeGamma))
// Test if ea and eabis are the same
// Need to check both singular and non-singular cases
// There are two singular cases.
// 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, 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] || test_isMuchSmallerThan(eabis[0], Scalar(1)));
}
}*/
// Quaternions
QuaternionType q(e);
eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeGamma).angles();
QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
VERIFY_IS_APPROX(std::abs(q.dot(qbis)), static_cast<Scalar>(1));
//VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
}
template<typename EulerSystem, typename Scalar>
void verify_euler(const Matrix<Scalar,3,1>& ea)
{
verify_euler_ranged<EulerSystem>(ea, false, false, false);
verify_euler_ranged<EulerSystem>(ea, false, false, true);
verify_euler_ranged<EulerSystem>(ea, false, true, false);
verify_euler_ranged<EulerSystem>(ea, false, true, true);
verify_euler_ranged<EulerSystem>(ea, true, false, false);
verify_euler_ranged<EulerSystem>(ea, true, false, true);
verify_euler_ranged<EulerSystem>(ea, true, true, false);
verify_euler_ranged<EulerSystem>(ea, true, true, true);
verify_euler_ranged<EulerSystem>(ea, false, false);
verify_euler_ranged<EulerSystem>(ea, false, true);
verify_euler_ranged<EulerSystem>(ea, false, false);
verify_euler_ranged<EulerSystem>(ea, false, true);
verify_euler_ranged<EulerSystem>(ea, true, false);
verify_euler_ranged<EulerSystem>(ea, true, true);
verify_euler_ranged<EulerSystem>(ea, true, false);
verify_euler_ranged<EulerSystem>(ea, true, true);
}
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)