mirror of
https://gitlab.com/libeigen/eigen.git
synced 2026-04-10 11:34:33 +08:00
Merged in tal500/eigen-eulerangles (pull request PR-237)
Euler angles
This commit is contained in:
Gael Guennebaud
@@ -13,146 +13,219 @@
|
||||
|
||||
using namespace Eigen;
|
||||
|
||||
template<typename EulerSystem, typename Scalar>
|
||||
void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
|
||||
// Unfortunately, we need to specialize it in order to work. (We could add it in main.h test framework)
|
||||
template <typename Scalar, class System>
|
||||
bool verifyIsApprox(const Eigen::EulerAngles<Scalar, System>& a, const Eigen::EulerAngles<Scalar, System>& b)
|
||||
{
|
||||
return verifyIsApprox(a.angles(), b.angles());
|
||||
}
|
||||
|
||||
// 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)
|
||||
|
||||
const char X = EULER_X;
|
||||
const char Y = EULER_Y;
|
||||
const char Z = EULER_Z;
|
||||
|
||||
template<typename Scalar, class EulerSystem>
|
||||
void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
|
||||
{
|
||||
typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
typedef Quaternion<Scalar> QuaternionType;
|
||||
typedef AngleAxis<Scalar> AngleAxisType;
|
||||
using std::abs;
|
||||
|
||||
Scalar alphaRangeStart, alphaRangeEnd;
|
||||
const Scalar ONE = Scalar(1);
|
||||
const Scalar HALF_PI = Scalar(EIGEN_PI / 2);
|
||||
const Scalar PI = Scalar(EIGEN_PI);
|
||||
|
||||
// It's very important calc the acceptable precision depending on the distance from the pole.
|
||||
const Scalar longitudeRadius = std::abs(
|
||||
EulerSystem::IsTaitBryan ?
|
||||
std::cos(e.beta()) :
|
||||
std::sin(e.beta())
|
||||
);
|
||||
Scalar precision = test_precision<Scalar>() / longitudeRadius;
|
||||
|
||||
Scalar betaRangeStart, betaRangeEnd;
|
||||
Scalar gammaRangeStart, gammaRangeEnd;
|
||||
|
||||
if (positiveRangeAlpha)
|
||||
if (EulerSystem::IsTaitBryan)
|
||||
{
|
||||
alphaRangeStart = Scalar(0);
|
||||
alphaRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
betaRangeStart = -HALF_PI;
|
||||
betaRangeEnd = HALF_PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
alphaRangeStart = -Scalar(EIGEN_PI);
|
||||
alphaRangeEnd = Scalar(EIGEN_PI);
|
||||
if (!EulerSystem::IsBetaOpposite)
|
||||
{
|
||||
betaRangeStart = 0;
|
||||
betaRangeEnd = PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
betaRangeStart = -PI;
|
||||
betaRangeEnd = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (positiveRangeBeta)
|
||||
{
|
||||
betaRangeStart = Scalar(0);
|
||||
betaRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
}
|
||||
else
|
||||
{
|
||||
betaRangeStart = -Scalar(EIGEN_PI);
|
||||
betaRangeEnd = Scalar(EIGEN_PI);
|
||||
}
|
||||
|
||||
if (positiveRangeGamma)
|
||||
{
|
||||
gammaRangeStart = Scalar(0);
|
||||
gammaRangeEnd = Scalar(2 * EIGEN_PI);
|
||||
}
|
||||
else
|
||||
{
|
||||
gammaRangeStart = -Scalar(EIGEN_PI);
|
||||
gammaRangeEnd = Scalar(EIGEN_PI);
|
||||
}
|
||||
|
||||
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 Vector3 I = EulerAnglesType::AlphaAxisVector();
|
||||
const Vector3 J = EulerAnglesType::BetaAxisVector();
|
||||
const Vector3 K = EulerAnglesType::GammaAxisVector();
|
||||
|
||||
EulerAnglesType e(ea[0], ea[1], ea[2]);
|
||||
// Is approx checks
|
||||
VERIFY(e.isApprox(e));
|
||||
VERIFY_IS_APPROX(e, e);
|
||||
VERIFY_IS_NOT_APPROX(e, EulerAnglesType(e.alpha() + ONE, e.beta() + ONE, e.gamma() + ONE));
|
||||
|
||||
const Matrix3 m(e);
|
||||
VERIFY_IS_APPROX(Scalar(m.determinant()), ONE);
|
||||
|
||||
EulerAnglesType ebis(m);
|
||||
|
||||
Matrix3 m(e);
|
||||
Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
|
||||
// When no roll(acting like polar representation), we have the best precision.
|
||||
// One of those cases is when the Euler angles are on the pole, and because it's singular case,
|
||||
// the computation returns no roll.
|
||||
if (ebis.beta() == 0)
|
||||
precision = test_precision<Scalar>();
|
||||
|
||||
// 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))
|
||||
{
|
||||
/* 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)));
|
||||
}
|
||||
VERIFY_APPROXED_RANGE(-PI, ebis.alpha(), PI);
|
||||
VERIFY_APPROXED_RANGE(betaRangeStart, ebis.beta(), betaRangeEnd);
|
||||
VERIFY_APPROXED_RANGE(-PI, ebis.gamma(), PI);
|
||||
|
||||
const Matrix3 mbis(AngleAxisType(ebis.alpha(), I) * AngleAxisType(ebis.beta(), J) * AngleAxisType(ebis.gamma(), K));
|
||||
VERIFY_IS_APPROX(Scalar(mbis.determinant()), ONE);
|
||||
VERIFY_IS_APPROX(mbis, ebis.toRotationMatrix());
|
||||
/*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(m.isApprox(mbis, precision));
|
||||
|
||||
// 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
|
||||
|
||||
// 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);
|
||||
eabis = EulerAnglesType(q, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
|
||||
VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
|
||||
const QuaternionType q(e);
|
||||
ebis = q;
|
||||
const QuaternionType qbis(ebis);
|
||||
VERIFY(internal::isApprox<Scalar>(std::abs(q.dot(qbis)), ONE, precision));
|
||||
//VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
|
||||
|
||||
// A suggestion for simple product test when will be supported.
|
||||
/*EulerAnglesType e2(PI/2, PI/2, PI/2);
|
||||
Matrix3 m2(e2);
|
||||
VERIFY_IS_APPROX(e*e2, m*m2);*/
|
||||
}
|
||||
|
||||
template<typename EulerSystem, typename Scalar>
|
||||
void verify_euler(const Matrix<Scalar,3,1>& ea)
|
||||
template<signed char A, signed char B, signed char C, typename Scalar>
|
||||
void verify_euler_vec(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(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_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<EulerSystemYZX>(ea);
|
||||
verify_euler<EulerSystemYZY>(ea);
|
||||
verify_euler<EulerSystemYXZ>(ea);
|
||||
verify_euler<EulerSystemYXY>(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<EulerSystemZXY>(ea);
|
||||
verify_euler<EulerSystemZXZ>(ea);
|
||||
verify_euler<EulerSystemZYX>(ea);
|
||||
verify_euler<EulerSystemZYZ>(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 eulerangles()
|
||||
template<typename Scalar> void check_singular_cases(const Scalar& singularBeta)
|
||||
{
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
const Scalar PI = Scalar(EIGEN_PI);
|
||||
|
||||
for (Scalar epsilon = NumTraits<Scalar>::epsilon(); epsilon < 1; epsilon *= Scalar(1.2))
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
||||
// This one for sanity, it had a problem with near pole cases in float scalar.
|
||||
check_all_var(Vector3(PI*Scalar(0.8), singularBeta - Scalar(1E-6), Scalar(0.9)*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_rand()
|
||||
{
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
@@ -201,8 +274,19 @@ template<typename Scalar> void eulerangles()
|
||||
|
||||
void test_EulerAngles()
|
||||
{
|
||||
// Simple cast test
|
||||
EulerAnglesXYZd onesEd(1, 1, 1);
|
||||
EulerAnglesXYZf onesEf = onesEd.cast<float>();
|
||||
VERIFY_IS_APPROX(onesEd, onesEf.cast<double>());
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user