Merged eigen/eigen into default

This commit is contained in:
Benoit Steiner
2016-11-26 11:28:25 -08:00
17 changed files with 551 additions and 477 deletions

View File

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

View File

@@ -32,9 +32,10 @@ bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve )
{
typedef typename POLYNOMIAL::Index Index;
typedef typename POLYNOMIAL::Scalar Scalar;
typedef typename POLYNOMIAL::RealScalar RealScalar;
typedef typename SOLVER::RootsType RootsType;
typedef Matrix<Scalar,Deg,1> EvalRootsType;
typedef Matrix<RealScalar,Deg,1> EvalRootsType;
const Index deg = pols.size()-1;
@@ -57,7 +58,7 @@ bool aux_evalSolver( const POLYNOMIAL& pols, SOLVER& psolve )
cerr << endl;
}
std::vector<Scalar> rootModuli( roots.size() );
std::vector<RealScalar> rootModuli( roots.size() );
Map< EvalRootsType > aux( &rootModuli[0], roots.size() );
aux = roots.array().abs();
std::sort( rootModuli.begin(), rootModuli.end() );
@@ -83,7 +84,7 @@ void evalSolver( const POLYNOMIAL& pols )
{
typedef typename POLYNOMIAL::Scalar Scalar;
typedef PolynomialSolver<Scalar, Deg > PolynomialSolverType;
typedef PolynomialSolver<Scalar, Deg > PolynomialSolverType;
PolynomialSolverType psolve;
aux_evalSolver<Deg, POLYNOMIAL, PolynomialSolverType>( pols, psolve );
@@ -97,6 +98,7 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const
{
using std::sqrt;
typedef typename POLYNOMIAL::Scalar Scalar;
typedef typename POLYNOMIAL::RealScalar RealScalar;
typedef PolynomialSolver<Scalar, Deg > PolynomialSolverType;
@@ -107,15 +109,12 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const
// 1) the roots found are correct
// 2) the roots have distinct moduli
typedef typename POLYNOMIAL::Scalar Scalar;
typedef typename REAL_ROOTS::Scalar Real;
//Test realRoots
std::vector< Real > calc_realRoots;
psolve.realRoots( calc_realRoots );
VERIFY( calc_realRoots.size() == (size_t)real_roots.size() );
std::vector< RealScalar > calc_realRoots;
psolve.realRoots( calc_realRoots, test_precision<RealScalar>());
VERIFY_IS_EQUAL( calc_realRoots.size() , (size_t)real_roots.size() );
const Scalar psPrec = sqrt( test_precision<Scalar>() );
const RealScalar psPrec = sqrt( test_precision<RealScalar>() );
for( size_t i=0; i<calc_realRoots.size(); ++i )
{
@@ -138,7 +137,7 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const
bool hasRealRoot;
//Test absGreatestRealRoot
Real r = psolve.absGreatestRealRoot( hasRealRoot );
RealScalar r = psolve.absGreatestRealRoot( hasRealRoot );
VERIFY( hasRealRoot == (real_roots.size() > 0 ) );
if( hasRealRoot ){
VERIFY( internal::isApprox( real_roots.array().abs().maxCoeff(), abs(r), psPrec ) ); }
@@ -167,9 +166,11 @@ void evalSolverSugarFunction( const POLYNOMIAL& pols, const ROOTS& roots, const
template<typename _Scalar, int _Deg>
void polynomialsolver(int deg)
{
typedef internal::increment_if_fixed_size<_Deg> Dim;
typedef typename NumTraits<_Scalar>::Real RealScalar;
typedef internal::increment_if_fixed_size<_Deg> Dim;
typedef Matrix<_Scalar,Dim::ret,1> PolynomialType;
typedef Matrix<_Scalar,_Deg,1> EvalRootsType;
typedef Matrix<RealScalar,_Deg,1> RealRootsType;
cout << "Standard cases" << endl;
PolynomialType pols = PolynomialType::Random(deg+1);
@@ -182,15 +183,11 @@ void polynomialsolver(int deg)
evalSolver<_Deg,PolynomialType>( pols );
cout << "Test sugar" << endl;
EvalRootsType realRoots = EvalRootsType::Random(deg);
RealRootsType realRoots = RealRootsType::Random(deg);
roots_to_monicPolynomial( realRoots, pols );
evalSolverSugarFunction<_Deg>(
pols,
realRoots.template cast <
std::complex<
typename NumTraits<_Scalar>::Real
>
>(),
realRoots.template cast <std::complex<RealScalar> >().eval(),
realRoots );
}
@@ -214,5 +211,6 @@ void test_polynomialsolver()
internal::random<int>(9,13)
)) );
CALL_SUBTEST_11((polynomialsolver<float,Dynamic>(1)) );
CALL_SUBTEST_12((polynomialsolver<std::complex<double>,Dynamic>(internal::random<int>(2,13))) );
}
}