GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-04-23 10:09:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge Hongkai Dai correct range calculation, and remove ranges from API.

Browse Source 
Docs updated.
This commit is contained in:
Tal Hadad 2016-10-14 16:03:28 +03:00
commit 078a202621
4 changed files with 165 additions and 318 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

180
unsupported/Eigen/src/EulerAngles/EulerAngles.h
View File

@ -55,33 +55,25 @@ namespace Eigen
* Additionally, some axes related computation is done in compile time.
*
* #### Euler angles ranges in conversions ####
* Rotations representation as EulerAngles are not singular (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 reason for infinite representation, but it's not the only reason for non-singularity.
*
* 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, PI].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, 2*PI]
* - If the beta axis is negative, the beta angle will be in the range [-2*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,
@ -152,61 +144,43 @@ namespace Eigen
/** Constructs and initialize Euler angles from a 3x3 rotation matrix \p m.
*
* \note All angles will be in the range [-PI, PI].
* \note 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, PI].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, 2*PI]
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
*/
template<typename Derived>
EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
/** 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].
* 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].
*/
template<typename Derived>
EulerAngles(
const MatrixBase<Derived>& m,
bool positiveRangeAlpha,
bool positiveRangeBeta,
bool positiveRangeGamma) {
System::CalcEulerAngles(*this, m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
}
EulerAngles(const MatrixBase<Derived>& m) { System::CalcEulerAngles(*this, m); }
/** 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, PI].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, 2*PI]
* - If the beta axis is negative, the beta angle will be in the range [-2*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) {
/*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.
System::CalcEulerAngles(*this, rot.toRotationMatrix(), positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma);
}
// 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,68 +220,11 @@ 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 a rotation matrix(i.e. pure orthogonal matrix with determinant of +1).
*
* 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].
* 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 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].
*/
template<
bool PositiveRangeAlpha,
bool PositiveRangeBeta,
bool PositiveRangeGamma,
typename Derived>
static EulerAngles FromRotation(const RotationBase<Derived, 3>& rot)
{
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)
@ -318,7 +235,11 @@ namespace Eigen
// 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());
@ -330,6 +251,7 @@ namespace Eigen
/** \returns an equivalent 3x3 rotation matrix. */
Matrix3 toRotationMatrix() const
{
// TODO: Calc it faster
return static_cast<QuaternionType>(*this).toRotationMatrix();
}

156
unsupported/Eigen/src/EulerAngles/EulerSystem.h
View File

@ -69,7 +69,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 +80,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 +90,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 +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 */
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 */
IsOdd = ((AlphaAxisAbs)%3 == (BetaAxisAbs - 1)%3) ? 0 : 1, /*!< weather the Euler system is odd */
IsEven = IsOdd ? 0 : 1, /*!< weather the Euler system is even */
// 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,123 +182,89 @@ 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);
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 {
res[0] += Scalar(EIGEN_PI);
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;
}
res[1] = atan2(-mat(I,K), -c2);
}
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);
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 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 {
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));
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>
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>

4
unsupported/doc/examples/EulerAngles.cpp
View File

@ -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;

123
unsupported/test/EulerAngles.cpp
View File

@ -13,62 +13,38 @@
using namespace Eigen;
// 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)
template<typename EulerSystem, typename Scalar>
void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
bool positiveRangeAlpha, bool positiveRangeBeta, bool positiveRangeGamma)
void verify_euler(const Matrix<Scalar,3,1>& ea)
{
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);
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);
betaRangeStart = -PI;
betaRangeEnd = PI;
}
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();
@ -76,62 +52,41 @@ void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
EulerAnglesType e(ea[0], ea[1], ea[2]);
Matrix3 m(e);
Vector3 eabis = EulerAnglesType(m, positiveRangeAlpha, positiveRangeBeta, positiveRangeGamma).angles();
Vector3 eabis = static_cast<EulerAnglesType>(m).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
VERIFY_APPROXED_RANGE(-PI, eabis[0], PI);
VERIFY_APPROXED_RANGE(betaRangeStart, eabis[1], betaRangeEnd);
VERIFY_APPROXED_RANGE(-PI, eabis[2], PI);
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)));
}
VERIFY(0 < eabis[0] || VERIFY_IS_MUCH_SMALLER_THAN(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
}
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);
eabis = static_cast<EulerAnglesType>(q).angles();
QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
VERIFY_IS_APPROX(std::abs(q.dot(qbis)), ONE);
//VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
}
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
@ -150,6 +105,8 @@ template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
verify_euler<EulerSystemZXZ>(ea);
verify_euler<EulerSystemZYX>(ea);
verify_euler<EulerSystemZYZ>(ea);
// TODO: Test negative axes as well! (only test if the angles get negative when needed)
}
template<typename Scalar> void eulerangles()