mirror of
https://gitlab.com/libeigen/eigen.git
synced 2024-12-21 07:19:46 +08:00
Fix calc bug, docs and better testing.
Test code changes: * better coded * rand and manual numbers * singularity checking
This commit is contained in:
Tal Hadad
parent
078a202621
commit
58f5d7d058
@ -36,7 +36,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,10 +55,12 @@ 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>
|
||||
* 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 reason for infinite representation, but it's not the only reason for non-singularity.
|
||||
* This is the general reason for infinite representation,
|
||||
* but it's not the only general reason for not having a single representation.
|
||||
*
|
||||
* 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:
|
||||
@ -66,10 +68,10 @@ namespace Eigen
|
||||
* (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].
|
||||
* - 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, 2*PI]
|
||||
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
|
||||
* - 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]
|
||||
*
|
||||
* \sa EulerAngles(const MatrixBase<Derived>&)
|
||||
* \sa EulerAngles(const RotationBase<Derived, 3>&)
|
||||
@ -95,7 +97,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.
|
||||
*/
|
||||
@ -146,10 +148,10 @@ namespace Eigen
|
||||
*
|
||||
* \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].
|
||||
* - 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, 2*PI]
|
||||
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
|
||||
* - 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) { System::CalcEulerAngles(*this, m); }
|
||||
@ -160,10 +162,10 @@ namespace Eigen
|
||||
* 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].
|
||||
* - 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, 2*PI]
|
||||
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
|
||||
* - 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) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }
|
||||
|
||||
@ -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
|
||||
{
|
||||
@ -186,25 +186,25 @@ namespace Eigen
|
||||
|
||||
typedef typename Derived::Scalar Scalar;
|
||||
|
||||
Scalar plusMinus = IsEven? 1 : -1;
|
||||
Scalar minusPlus = IsOdd? 1 : -1;
|
||||
const Scalar plusMinus = IsEven? 1 : -1;
|
||||
const 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);
|
||||
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()) {
|
||||
// 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) {
|
||||
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);
|
||||
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 {
|
||||
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);
|
||||
@ -222,30 +222,30 @@ namespace Eigen
|
||||
|
||||
typedef typename Derived::Scalar Scalar;
|
||||
|
||||
Scalar plusMinus = IsEven? 1 : -1;
|
||||
Scalar minusPlus = IsOdd? 1 : -1;
|
||||
const Scalar plusMinus = IsEven? 1 : -1;
|
||||
const 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);
|
||||
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));
|
||||
|
||||
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {
|
||||
// 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) {
|
||||
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 {
|
||||
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[1] = 0;
|
||||
res[2] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<typename Scalar>
|
||||
|
||||
@ -15,13 +15,17 @@ 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)
|
||||
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(const Matrix<Scalar,3,1>& ea)
|
||||
const char X = EULER_X;
|
||||
const char Y = EULER_Y;
|
||||
const char Z = EULER_Z;
|
||||
|
||||
template<typename Scalar, typename EulerSystem>
|
||||
void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
|
||||
{
|
||||
typedef EulerAngles<Scalar, EulerSystem> EulerAnglesType;
|
||||
typedef Matrix<Scalar,3,3> Matrix3;
|
||||
@ -41,17 +45,24 @@ void verify_euler(const Matrix<Scalar,3,1>& ea)
|
||||
}
|
||||
else
|
||||
{
|
||||
betaRangeStart = -PI;
|
||||
betaRangeEnd = PI;
|
||||
if (!EulerSystem::IsBetaOpposite)
|
||||
{
|
||||
betaRangeStart = 0;
|
||||
betaRangeEnd = PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
betaRangeStart = -PI;
|
||||
betaRangeEnd = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const Vector3 I = EulerAnglesType::AlphaAxisVector();
|
||||
const Vector3 J = EulerAnglesType::BetaAxisVector();
|
||||
const Vector3 K = EulerAnglesType::GammaAxisVector();
|
||||
|
||||
EulerAnglesType e(ea[0], ea[1], ea[2]);
|
||||
|
||||
Matrix3 m(e);
|
||||
const Matrix3 m(e);
|
||||
VERIFY_IS_APPROX(Scalar(m.determinant()), ONE);
|
||||
|
||||
Vector3 eabis = static_cast<EulerAnglesType>(m).angles();
|
||||
|
||||
@ -60,8 +71,16 @@ void verify_euler(const Matrix<Scalar,3,1>& ea)
|
||||
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);
|
||||
const Matrix3 mbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
|
||||
VERIFY_IS_APPROX(Scalar(mbis.determinant()), ONE);
|
||||
/*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_IS_APPROX(m, mbis);
|
||||
|
||||
// Test if ea and eabis are the same
|
||||
// Need to check both singular and non-singular cases
|
||||
@ -69,47 +88,107 @@ void verify_euler(const Matrix<Scalar,3,1>& ea)
|
||||
// 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 || 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] || VERIFY_IS_MUCH_SMALLER_THAN(eabis[0], Scalar(1)));
|
||||
}*/
|
||||
// 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);
|
||||
const QuaternionType q(e);
|
||||
eabis = static_cast<EulerAnglesType>(q).angles();
|
||||
QuaternionType qbis(AngleAxisType(eabis[0], I) * AngleAxisType(eabis[1], J) * AngleAxisType(eabis[2], K));
|
||||
const 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)
|
||||
template<signed char A, signed char B, signed char C, typename Scalar>
|
||||
void verify_euler_vec(const Matrix<Scalar,3,1>& ea)
|
||||
{
|
||||
verify_euler<EulerSystemXYZ>(ea);
|
||||
verify_euler<EulerSystemXYX>(ea);
|
||||
verify_euler<EulerSystemXZY>(ea);
|
||||
verify_euler<EulerSystemXZX>(ea);
|
||||
|
||||
verify_euler<EulerSystemYZX>(ea);
|
||||
verify_euler<EulerSystemYZY>(ea);
|
||||
verify_euler<EulerSystemYXZ>(ea);
|
||||
verify_euler<EulerSystemYXY>(ea);
|
||||
|
||||
verify_euler<EulerSystemZXY>(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)
|
||||
verify_euler(EulerAngles<Scalar, EulerSystem<A, B, C> >(ea[0], ea[1], ea[2]));
|
||||
}
|
||||
|
||||
template<typename Scalar> void eulerangles()
|
||||
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_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_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_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 check_singular_cases(const Scalar& singularBeta)
|
||||
{
|
||||
typedef Matrix<Scalar,3,1> Vector3;
|
||||
const Scalar epsilon = std::numeric_limits<Scalar>::epsilon();
|
||||
const Scalar PI = Scalar(EIGEN_PI);
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
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;
|
||||
@ -158,8 +237,14 @@ template<typename Scalar> void eulerangles()
|
||||
|
||||
void test_EulerAngles()
|
||||
{
|
||||
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…
Reference in New Issue
Block a user