mirror of
https://gitlab.com/libeigen/eigen.git
synced 2025-01-24 14:45:14 +08:00
Use RotationBase, test quaternions and support ranges.
This commit is contained in:
Tal Hadad
parent
b091b7e6ea
commit
bfed274df3
@ -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<typename Derived>
|
||||
inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; }
|
||||
|
||||
template<typename Derived>
|
||||
inline EulerAngles(
|
||||
const MatrixBase<Derived>& m,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll) {
|
||||
|
||||
fromRotation(m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
inline EulerAngles(const RotationBase<Derived, 3>& rot) { *this = rot; }
|
||||
|
||||
template<typename Derived>
|
||||
inline EulerAngles(
|
||||
const RotationBase<Derived, 3>& 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<typename Derived>
|
||||
// TODO: Add booleans which let the user control desired output angles range( (-PI, PI) or [0, 2*PI) )
|
||||
EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m)
|
||||
EulerAngles& fromRotation(const MatrixBase<Derived>& m)
|
||||
{
|
||||
System::eulerAngles(*this, m);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** Set \c *this from a rotation matrix(i.e. pure orthogonal matrix with determinent of +1).
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles& operator=(const MatrixBase<Derived>& mat){
|
||||
return fromRotationMatrix(mat);
|
||||
template<
|
||||
bool PositiveRangeHeading,
|
||||
bool PositiveRangePitch,
|
||||
bool PositiveRangeRoll,
|
||||
typename Derived>
|
||||
EulerAngles& fromRotation(const MatrixBase<Derived>& m)
|
||||
{
|
||||
System::eulerAngles<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(*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<typename Derived>
|
||||
EulerAngles& fromRotation(
|
||||
const MatrixBase<Derived>& m,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll)
|
||||
{
|
||||
System::eulerAngles(*this, m, positiveRangeHeading, positiveRangePitch, positiveRangeRoll);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot)
|
||||
{
|
||||
return fromRotation(rot.toRotationMatrix());
|
||||
}
|
||||
|
||||
template<
|
||||
bool PositiveRangeHeading,
|
||||
bool PositiveRangePitch,
|
||||
bool PositiveRangeRoll,
|
||||
typename Derived>
|
||||
EulerAngles& fromRotation(const RotationBase<Derived, 3>& rot)
|
||||
{
|
||||
return fromRotation<PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll>(rot.toRotationMatrix());
|
||||
}
|
||||
|
||||
template<typename Derived>
|
||||
EulerAngles& fromRotation(
|
||||
const RotationBase<Derived, 3>& 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<typename Derived>
|
||||
EulerAngles& operator=(const MatrixBase<Derived>& mat) {
|
||||
return fromRotation(mat);
|
||||
}
|
||||
|
||||
// TODO: Assign and construct from another EulerAngle (with different system)
|
||||
|
||||
/** Set \c *this from a rotation.
|
||||
*/
|
||||
template<typename Derived>
|
||||
EulerAngles& operator=(const RotationBase<Derived, 3>& 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<QuaternionType>(*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();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -19,7 +19,7 @@ namespace Eigen
|
||||
namespace internal
|
||||
{
|
||||
// TODO: Check if already exists on the rest API
|
||||
template <int Num, bool IsPossitive = (Num > 0)>
|
||||
template <int Num, bool IsPositive = (Num > 0)>
|
||||
struct Abs
|
||||
{
|
||||
enum { value = Num };
|
||||
@ -73,6 +73,26 @@ namespace Eigen
|
||||
|
||||
template <bool Cond1, bool Cond2>
|
||||
struct NegateIfXor : NegateIf<Cond1 != Cond2> {};
|
||||
|
||||
template <typename Type, Type value, bool Cond>
|
||||
struct AddConstIf
|
||||
{
|
||||
template <typename T>
|
||||
static void run(T& t)
|
||||
{
|
||||
t += T(value);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename Type, Type value>
|
||||
struct AddConstIf<Type, value, false>
|
||||
{
|
||||
template <typename T>
|
||||
static void run(T&)
|
||||
{
|
||||
// no op
|
||||
}
|
||||
};
|
||||
|
||||
template <int Axis>
|
||||
struct IsValidAxis
|
||||
@ -196,17 +216,50 @@ namespace Eigen
|
||||
public:
|
||||
|
||||
template<typename Scalar>
|
||||
static void eulerAngles(EulerAngles<Scalar, EulerSystem>& res, const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
||||
static void eulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
||||
{
|
||||
eulerAngles(res, mat, false, false, false);
|
||||
}
|
||||
|
||||
template<
|
||||
typename Scalar,
|
||||
bool PositiveRangeHeading,
|
||||
bool PositiveRangePitch,
|
||||
bool PositiveRangeRoll>
|
||||
static void eulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat)
|
||||
{
|
||||
eulerAngles(res, mat, PositiveRangeHeading, PositiveRangePitch, PositiveRangeRoll);
|
||||
}
|
||||
|
||||
template<typename Scalar>
|
||||
static void eulerAngles(
|
||||
EulerAngles<Scalar, EulerSystem>& res,
|
||||
const typename EulerAngles<Scalar, EulerSystem>::Matrix3& mat,
|
||||
bool positiveRangeHeading,
|
||||
bool positiveRangePitch,
|
||||
bool positiveRangeRoll)
|
||||
{
|
||||
eulerAngles_imp(
|
||||
res.coeffs(), mat,
|
||||
typename internal::conditional<IsTaitBryan, internal::true_type, internal::false_type>::type());
|
||||
|
||||
internal::NegateIfXor<IsHeadingOpposite, IsEven>::run(res.h());
|
||||
|
||||
internal::NegateIfXor<IsPitchOpposite, IsEven>::run(res.p());
|
||||
|
||||
internal::NegateIfXor<IsRollOpposite, IsEven>::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);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -14,14 +14,53 @@
|
||||
using namespace Eigen;
|
||||
|
||||
template<typename EulerSystem, typename Scalar>
|
||||
void verify_euler(const Matrix<Scalar,3,1>& ea)
|
||||
void verify_euler_ranged(const Matrix<Scalar,3,1>& ea,
|
||||
bool positiveRangeHeading, bool positiveRangePitch, bool positiveRangeRoll)
|
||||
{
|
||||
typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
typedef AngleAxis<Scalar> AngleAxisx;
|
||||
typedef Quaternion<Scalar> QuaternionType;
|
||||
typedef AngleAxis<Scalar> 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<Scalar,3,1>& 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<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_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<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, positiveRangeHeading, positiveRangePitch, positiveRangeRoll).coeffs();
|
||||
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);
|
||||
}
|
||||
|
||||
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
|
||||
{
|
||||
verify_euler<EulerSystemXYZ, Scalar>(ea);
|
||||
verify_euler<EulerSystemXYX, Scalar>(ea);
|
||||
verify_euler<EulerSystemXZY, Scalar>(ea);
|
||||
verify_euler<EulerSystemXZX, Scalar>(ea);
|
||||
verify_euler<EulerSystemXYZ>(ea);
|
||||
verify_euler<EulerSystemXYX>(ea);
|
||||
verify_euler<EulerSystemXZY>(ea);
|
||||
verify_euler<EulerSystemXZX>(ea);
|
||||
|
||||
verify_euler<EulerSystemYZX, Scalar>(ea);
|
||||
verify_euler<EulerSystemYZY, Scalar>(ea);
|
||||
verify_euler<EulerSystemYXZ, Scalar>(ea);
|
||||
verify_euler<EulerSystemYXY, Scalar>(ea);
|
||||
verify_euler<EulerSystemYZX>(ea);
|
||||
verify_euler<EulerSystemYZY>(ea);
|
||||
verify_euler<EulerSystemYXZ>(ea);
|
||||
verify_euler<EulerSystemYXY>(ea);
|
||||
|
||||
verify_euler<EulerSystemZXY, Scalar>(ea);
|
||||
verify_euler<EulerSystemZXZ, Scalar>(ea);
|
||||
verify_euler<EulerSystemZYX, Scalar>(ea);
|
||||
verify_euler<EulerSystemZYZ, Scalar>(ea);
|
||||
verify_euler<EulerSystemZXY>(ea);
|
||||
verify_euler<EulerSystemZXZ>(ea);
|
||||
verify_euler<EulerSystemZYX>(ea);
|
||||
verify_euler<EulerSystemZYZ>(ea);
|
||||
}
|
||||
|
||||
template<typename Scalar> void eulerangles()
|
||||
@ -85,11 +158,11 @@ template<typename Scalar> void eulerangles()
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
typedef Array<Scalar,3,1> Array3;
|
||||
typedef Quaternion<Scalar> Quaternionx;
|
||||
typedef AngleAxis<Scalar> AngleAxisx;
|
||||
typedef AngleAxis<Scalar> AngleAxisType;
|
||||
|
||||
Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI));
|
||||
Quaternionx q1;
|
||||
q1 = AngleAxisx(a, Vector3::Random().normalized());
|
||||
q1 = AngleAxisType(a, Vector3::Random().normalized());
|
||||
Matrix3 m;
|
||||
m = q1;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user