mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-08-12 03:39:01 +08:00
Merged in tal500/eigen-eulerangles (pull request PR-237)
Euler angles
This commit is contained in:
Gael Guennebaud
commit
7f6333c32b
@ -12,11 +12,6 @@
|
||||
|
||||
namespace Eigen
|
||||
{
|
||||
/*template<typename Other,
|
||||
int OtherRows=Other::RowsAtCompileTime,
|
||||
int OtherCols=Other::ColsAtCompileTime>
|
||||
struct ei_eulerangles_assign_impl;*/
|
||||
|
||||
/** \class EulerAngles
|
||||
*
|
||||
* \ingroup EulerAngles_Module
|
||||
@ -36,7 +31,7 @@ namespace Eigen
|
||||
* ### Rotation representation and conversions ###
|
||||
*
|
||||
* It has been proved(see Wikipedia link below) that every rotation can be represented
|
||||
* by Euler angles, but there is no singular representation (e.g. unlike rotation matrices).
|
||||
* by Euler angles, but there is no single representation (e.g. unlike rotation matrices).
|
||||
* Therefore, you can convert from Eigen rotation and to them
|
||||
* (including rotation matrices, which is not called "rotations" by Eigen design).
|
||||
*
|
||||
@ -55,33 +50,27 @@ namespace Eigen
|
||||
* Additionally, some axes related computation is done in compile time.
|
||||
*
|
||||
* #### Euler angles ranges in conversions ####
|
||||
* Rotations representation as EulerAngles are not single (unlike matrices),
|
||||
* and even have infinite EulerAngles representations.<BR>
|
||||
* For example, add or subtract 2*PI from either angle of EulerAngles
|
||||
* and you'll get the same rotation.
|
||||
* This is the general reason for infinite representation,
|
||||
* but it's not the only general reason for not having a single representation.
|
||||
*
|
||||
* When converting some rotation to Euler angles, there are some ways you can guarantee
|
||||
* the Euler angles ranges.
|
||||
* When converting rotation to EulerAngles, this class convert it to specific ranges
|
||||
* When converting some rotation to EulerAngles, the rules for ranges are as follow:
|
||||
* - If the rotation we converting from is an EulerAngles
|
||||
* (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
|
||||
* - otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
|
||||
* As for Beta angle:
|
||||
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
|
||||
* - otherwise:
|
||||
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
|
||||
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
|
||||
*
|
||||
* #### implicit ranges ####
|
||||
* When using implicit ranges, all angles are guarantee to be in the range [-PI, +PI],
|
||||
* unless you convert from some other Euler angles.
|
||||
* In this case, the range is __undefined__ (might be even less than -PI or greater than +2*PI).
|
||||
* \sa EulerAngles(const MatrixBase<Derived>&)
|
||||
* \sa EulerAngles(const RotationBase<Derived, 3>&)
|
||||
*
|
||||
* #### explicit ranges ####
|
||||
* When using explicit ranges, all angles are guarantee to be in the range you choose.
|
||||
* In the range Boolean parameter, you're been ask whether you prefer the positive range or not:
|
||||
* - _true_ - force the range between [0, +2*PI]
|
||||
* - _false_ - force the range between [-PI, +PI]
|
||||
*
|
||||
* ##### compile time ranges #####
|
||||
* This is when you have compile time ranges and you prefer to
|
||||
* use template parameter. (e.g. for performance)
|
||||
* \sa FromRotation()
|
||||
*
|
||||
* ##### 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)
|
||||
*
|
||||
* ### Convenient user typedefs ###
|
||||
*
|
||||
* Convenient typedefs for EulerAngles exist for float and double scalar,
|
||||
@ -103,7 +92,7 @@ namespace Eigen
|
||||
*
|
||||
* More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles
|
||||
*
|
||||
* \tparam _Scalar the scalar type, i.e., the type of the angles.
|
||||
* \tparam _Scalar the scalar type, i.e. the type of the angles.
|
||||
*
|
||||
* \tparam _System the EulerSystem to use, which represents the axes of rotation.
|
||||
*/
|
||||
@ -111,8 +100,11 @@ namespace Eigen
|
||||
class EulerAngles : public RotationBase<EulerAngles<_Scalar, _System>, 3>
|
||||
{
|
||||
public:
|
||||
typedef RotationBase<EulerAngles<_Scalar, _System>, 3> Base;
|
||||
|
||||
/** the scalar type of the angles */
|
||||
typedef _Scalar Scalar;
|
||||
typedef typename NumTraits<Scalar>::Real RealScalar;
|
||||
|
||||
/** the EulerSystem to use, which represents the axes of rotation. */
|
||||
typedef _System System;
|
||||
@ -146,67 +138,56 @@ namespace Eigen
|
||||
public:
|
||||
/** Default constructor without initialization. */
|
||||
EulerAngles() {}
|
||||
/** Constructs and initialize Euler angles(\p alpha, \p beta, \p gamma). */
|
||||
/** Constructs and initialize an EulerAngles (\p alpha, \p beta, \p gamma). */
|
||||
EulerAngles(const Scalar& alpha, const Scalar& beta, const Scalar& gamma) :
|
||||
m_angles(alpha, beta, gamma) {}
|
||||
|
||||
/** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m.
|
||||
*
|
||||
* \note All angles will be in the range [-PI, PI].
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
|
||||
// TODO: Test this constructor
|
||||
/** Constructs and initialize an EulerAngles from the array data {alpha, beta, gamma} */
|
||||
explicit EulerAngles(const Scalar* data) : m_angles(data) {}
|
||||
|
||||
/** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m,
|
||||
* with options to choose for each angle the requested range.
|
||||
/** Constructs and initializes an EulerAngles from either:
|
||||
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
|
||||
* - a 3D vector expression representing Euler angles.
|
||||
*
|
||||
* 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].
|
||||
*
|
||||
* \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].
|
||||
*/
|
||||
* \note If \p other is a 3x3 rotation matrix, the angles range rules will be as follow:<BR>
|
||||
* Alpha and gamma angles will be in the range [-PI, PI].<BR>
|
||||
* As for Beta angle:
|
||||
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
|
||||
* - otherwise:
|
||||
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
|
||||
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles(
|
||||
const MatrixBase<Derived>& m,
|
||||
bool positiveRangeAlpha,
|
||||
bool positiveRangeBeta,
|
||||
bool positiveRangeGamma) {
|
||||
|
||||
System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
|
||||
}
|
||||
explicit EulerAngles(const MatrixBase<Derived>& other) { *this = other; }
|
||||
|
||||
/** Constructs and initialize Euler angles from a rotation \p rot.
|
||||
*
|
||||
* \note All angles will be in the range [-PI, PI], unless \p rot is an EulerAngles.
|
||||
* If rot is an EulerAngles, expected EulerAngles range is __undefined__.
|
||||
* (Use other functions here for enforcing range if this effect is desired)
|
||||
* \note If \p rot is an EulerAngles (even when it represented as RotationBase explicitly),
|
||||
* angles ranges are __undefined__.
|
||||
* Otherwise, alpha and gamma angles will be in the range [-PI, PI].<BR>
|
||||
* As for Beta angle:
|
||||
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
|
||||
* - otherwise:
|
||||
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
|
||||
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; }
|
||||
EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
|
||||
|
||||
/** Constructs and initialize Euler angles from a rotation \p rot,
|
||||
* 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].
|
||||
* Otherwise, the specified angle will be in the range [-PI, +PI].
|
||||
*
|
||||
* \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);
|
||||
}
|
||||
/*EulerAngles(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.
|
||||
|
||||
// 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)));
|
||||
}*/
|
||||
|
||||
/** \returns The angle values stored in a vector (alpha, beta, gamma). */
|
||||
const Vector3& angles() const { return m_angles; }
|
||||
@ -246,90 +227,48 @@ namespace Eigen
|
||||
return inverse();
|
||||
}
|
||||
|
||||
/** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m,
|
||||
* with options to choose for each angle the requested range (__only in compile time__).
|
||||
/** Set \c *this from either:
|
||||
* - a 3x3 rotation matrix expression(i.e. pure orthogonal matrix with determinant of +1),
|
||||
* - a 3D vector expression representing Euler angles.
|
||||
*
|
||||
* 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].
|
||||
*
|
||||
* \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)
|
||||
{
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
|
||||
|
||||
EulerAngles e;
|
||||
System::template CalcEulerAngles<
|
||||
PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma, _Scalar>(e, m);
|
||||
return e;
|
||||
}
|
||||
|
||||
/** Constructs and initialize Euler angles from a rotation \p rot,
|
||||
* with options to choose for each angle the requested range (__only in compile time__).
|
||||
*
|
||||
* 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].
|
||||
*
|
||||
* \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].
|
||||
* See EulerAngles(const MatrixBase<Derived, 3>&) for more information about
|
||||
* angles ranges output.
|
||||
*/
|
||||
template<
|
||||
bool PositiveRangeAlpha,
|
||||
bool PositiveRangeBeta,
|
||||
bool PositiveRangeGamma,
|
||||
typename Derived>
|
||||
static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot)
|
||||
template<class Derived>
|
||||
EulerAngles& operator=(const MatrixBase<Derived>& other)
|
||||
{
|
||||
return FromRotation<PositiveRangeAlpha, PositiveRangeBeta, PositiveRangeGamma>(rot.toRotationMatrix());
|
||||
}
|
||||
|
||||
/*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.
|
||||
|
||||
// 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)));
|
||||
}*/
|
||||
|
||||
/** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinant of +1). */
|
||||
template<typename Derived>
|
||||
EulerAngles& operator=(const MatrixBase<Derived>& m) {
|
||||
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)
|
||||
EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename Derived::Scalar>::value),
|
||||
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
|
||||
|
||||
System::CalcEulerAngles(*this, m);
|
||||
internal::eulerangles_assign_impl<System, Derived>::run(*this, other.derived());
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TODO: Assign and construct from another EulerAngles (with different system)
|
||||
|
||||
/** Set \c *this from a rotation. */
|
||||
/** Set \c *this from a rotation.
|
||||
*
|
||||
* See EulerAngles(const RotationBase<Derived, 3>&) for more information about
|
||||
* angles ranges output.
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles& operator=(const RotationBase<Derived, 3>& rot) {
|
||||
System::CalcEulerAngles(*this, rot.toRotationMatrix());
|
||||
return *this;
|
||||
}
|
||||
|
||||
// TODO: Support isApprox function
|
||||
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
|
||||
* determined by \a prec.
|
||||
*
|
||||
* \sa MatrixBase::isApprox() */
|
||||
bool isApprox(const EulerAngles& other,
|
||||
const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
|
||||
{ return angles().isApprox(other.angles(), prec); }
|
||||
|
||||
/** \returns an equivalent 3x3 rotation matrix. */
|
||||
Matrix3 toRotationMatrix() const
|
||||
{
|
||||
// TODO: Calc it faster
|
||||
return static_cast<QuaternionType>(*this).toRotationMatrix();
|
||||
}
|
||||
|
||||
@ -347,6 +286,15 @@ namespace Eigen
|
||||
s << eulerAngles.angles().transpose();
|
||||
return s;
|
||||
}
|
||||
|
||||
/** \returns \c *this with scalar type casted to \a NewScalarType */
|
||||
template <typename NewScalarType>
|
||||
EulerAngles<NewScalarType, System> cast() const
|
||||
{
|
||||
EulerAngles<NewScalarType, System> e;
|
||||
e.angles() = angles().cast<NewScalarType>();
|
||||
return e;
|
||||
}
|
||||
};
|
||||
|
||||
#define EIGEN_EULER_ANGLES_SINGLE_TYPEDEF(AXES, SCALAR_TYPE, SCALAR_POSTFIX) \
|
||||
@ -379,8 +327,29 @@ EIGEN_EULER_ANGLES_TYPEDEFS(double, d)
|
||||
{
|
||||
typedef _Scalar Scalar;
|
||||
};
|
||||
|
||||
// set from a rotation matrix
|
||||
template<class System, class Other>
|
||||
struct eulerangles_assign_impl<System,Other,3,3>
|
||||
{
|
||||
typedef typename Other::Scalar Scalar;
|
||||
static void run(EulerAngles<Scalar, System>& e, const Other& m)
|
||||
{
|
||||
System::CalcEulerAngles(e, m);
|
||||
}
|
||||
};
|
||||
|
||||
// set from a vector of Euler angles
|
||||
template<class System, class Other>
|
||||
struct eulerangles_assign_impl<System,Other,4,1>
|
||||
{
|
||||
typedef typename Other::Scalar Scalar;
|
||||
static void run(EulerAngles<Scalar, System>& e, const Other& vec)
|
||||
{
|
||||
e.angles() = vec;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // EIGEN_EULERANGLESCLASS_H
|
||||
|
||||
@ -18,7 +18,7 @@ namespace Eigen
|
||||
|
||||
namespace internal
|
||||
{
|
||||
// TODO: Check if already exists on the rest API
|
||||
// TODO: Add this trait to the Eigen internal API?
|
||||
template <int Num, bool IsPositive = (Num > 0)>
|
||||
struct Abs
|
||||
{
|
||||
@ -36,6 +36,12 @@ namespace Eigen
|
||||
{
|
||||
enum { value = Axis != 0 && Abs<Axis>::value <= 3 };
|
||||
};
|
||||
|
||||
template<typename System,
|
||||
typename Other,
|
||||
int OtherRows=Other::RowsAtCompileTime,
|
||||
int OtherCols=Other::ColsAtCompileTime>
|
||||
struct eulerangles_assign_impl;
|
||||
}
|
||||
|
||||
#define EIGEN_EULER_ANGLES_CLASS_STATIC_ASSERT(COND,MSG) typedef char static_assertion_##MSG[(COND)?1:-1]
|
||||
@ -69,7 +75,7 @@ namespace Eigen
|
||||
*
|
||||
* You can use this class to get two things:
|
||||
* - Build an Euler system, and then pass it as a template parameter to EulerAngles.
|
||||
* - Query some compile time data about an Euler system. (e.g. Whether it's tait bryan)
|
||||
* - Query some compile time data about an Euler system. (e.g. Whether it's Tait-Bryan)
|
||||
*
|
||||
* Euler rotation is a set of three rotation on fixed axes. (see \ref EulerAngles)
|
||||
* This meta-class store constantly those signed axes. (see \ref EulerAxis)
|
||||
@ -80,7 +86,7 @@ namespace Eigen
|
||||
* signed axes{+X,+Y,+Z,-X,-Y,-Z} are supported:
|
||||
* - all axes X, Y, Z in each valid order (see below what order is valid)
|
||||
* - rotation over the axis is supported both over the positive and negative directions.
|
||||
* - both tait bryan and proper/classic Euler angles (i.e. the opposite).
|
||||
* - both Tait-Bryan and proper/classic Euler angles (i.e. the opposite).
|
||||
*
|
||||
* Since EulerSystem support both positive and negative directions,
|
||||
* you may call this rotation distinction in other names:
|
||||
@ -90,7 +96,7 @@ namespace Eigen
|
||||
* Notice all axed combination are valid, and would trigger a static assertion.
|
||||
* Same unsigned axes can't be neighbors, e.g. {X,X,Y} is invalid.
|
||||
* This yield two and only two classes:
|
||||
* - _tait bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z}
|
||||
* - _Tait-Bryan_ - all unsigned axes are distinct, e.g. {X,Y,Z}
|
||||
* - _proper/classic Euler angles_ - The first and the third unsigned axes is equal,
|
||||
* and the second is different, e.g. {X,Y,X}
|
||||
*
|
||||
@ -112,9 +118,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 +144,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 */
|
||||
|
||||
IsTaitBryan = ((unsigned)AlphaAxisAbs != (unsigned)GammaAxisAbs) ? 1 : 0 /*!< weather the Euler system is tait bryan */
|
||||
// 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 /*!< whether the Euler system is Tait-Bryan */
|
||||
};
|
||||
|
||||
private:
|
||||
@ -180,127 +188,99 @@ 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);
|
||||
|
||||
const Scalar plusMinus = IsEven? 1 : -1;
|
||||
const Scalar minusPlus = IsOdd? 1 : -1;
|
||||
|
||||
const 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()) {// cos(beta) != 0
|
||||
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) {// cos(beta) == 0 and sin(beta) == 1
|
||||
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 {// cos(beta) == 0 and sin(beta) == -1
|
||||
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));
|
||||
}
|
||||
else
|
||||
{
|
||||
Scalar s2 = Vector2(mat(J,I), mat(K,I)).norm();
|
||||
res[1] = atan2(s2, mat(I,I));
|
||||
}
|
||||
|
||||
// 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
|
||||
const Scalar plusMinus = IsEven? 1 : -1;
|
||||
const Scalar minusPlus = IsOdd? 1 : -1;
|
||||
|
||||
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));
|
||||
const 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));
|
||||
|
||||
// There is a singularity when sin(beta) == 0
|
||||
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
|
||||
res[0] = atan2(mat(J, I), minusPlus * mat(K, I));
|
||||
res[2] = atan2(mat(I, J), plusMinus * mat(I, K));
|
||||
}
|
||||
else if(mat(I, I) > 0) {// sin(beta) == 0 and cos(beta) == 1
|
||||
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 {// sin(beta) == 0 and cos(beta) == -1
|
||||
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[2] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename Scalar>
|
||||
static void CalcEulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
||||
{
|
||||
CalcEulerAngles(res, mat, false, false, false);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
template<typename Scalar>
|
||||
static void CalcEulerAngles(
|
||||
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);
|
||||
}
|
||||
|
||||
template <typename _Scalar, class _System>
|
||||
friend class Eigen::EulerAngles;
|
||||
|
||||
template<typename System,
|
||||
typename Other,
|
||||
int OtherRows,
|
||||
int OtherCols>
|
||||
friend struct internal::eulerangles_assign_impl;
|
||||
};
|
||||
|
||||
#define EIGEN_EULER_SYSTEM_TYPEDEF(A, B, C) \
|
||||
|
||||
@ -23,7 +23,7 @@ int main()
|
||||
// Some Euler angles representation that our plane use.
|
||||
EulerAnglesZYZd planeAngles(0.78474, 0.5271, -0.513794);
|
||||
|
||||
MyArmyAngles planeAnglesInMyArmyAngles = MyArmyAngles::FromRotation<true, false, false>(planeAngles);
|
||||
MyArmyAngles planeAnglesInMyArmyAngles(planeAngles);
|
||||
|
||||
std::cout << "vehicle angles(MyArmy): " << vehicleAngles << std::endl;
|
||||
std::cout << "plane angles(ZYZ): " << planeAngles << std::endl;
|
||||
@ -37,7 +37,7 @@ int main()
|
||||
Quaterniond planeRotated = AngleAxisd(-0.342, Vector3d::UnitY()) * planeAngles;
|
||||
|
||||
planeAngles = planeRotated;
|
||||
planeAnglesInMyArmyAngles = MyArmyAngles::FromRotation<true, false, false>(planeRotated);
|
||||
planeAnglesInMyArmyAngles = planeRotated;
|
||||
|
||||
std::cout << "new plane angles(ZYZ): " << planeAngles << std::endl;
|
||||
std::cout << "new plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl;
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user