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

Add safty for near pole cases and test them better.

Browse Source
This commit is contained in:
Tal Hadad 2016-10-17 20:42:08 +03:00
parent 58f5d7d058
commit 7402cfd4cc
1 changed files with 25 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
unsupported/test/EulerAngles.cpp
View File

@ -37,6 +37,14 @@ void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
const Scalar HALF_PI = Scalar(EIGEN_PI / 2); const Scalar HALF_PI = Scalar(EIGEN_PI / 2);
const Scalar PI = Scalar(EIGEN_PI); 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())
);
const Scalar precision = test_precision<Scalar>() / longitudeRadius;
Scalar betaRangeStart, betaRangeEnd; Scalar betaRangeStart, betaRangeEnd;
if (EulerSystem::IsTaitBryan) if (EulerSystem::IsTaitBryan)
{ {
@ -80,7 +88,7 @@ void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
"mbis: " << mbis << std::endl << "mbis: " << mbis << std::endl <<
"X: " << (m * Vector3::UnitX()).transpose() << std::endl << "X: " << (m * Vector3::UnitX()).transpose() << std::endl <<
"X: " << (mbis * Vector3::UnitX()).transpose() << std::endl;*/ "X: " << (mbis * Vector3::UnitX()).transpose() << std::endl;*/
VERIFY_IS_APPROX(m, mbis); VERIFY(m.isApprox(mbis, precision));
// Test if ea and eabis are the same // Test if ea and eabis are the same
// Need to check both singular and non-singular cases // Need to check both singular and non-singular cases
@ -98,7 +106,7 @@ void verify_euler(const EulerAngles<Scalar, EulerSystem>& e)
const QuaternionType q(e); const QuaternionType q(e);
eabis = static_cast<EulerAnglesType>(q).angles(); eabis = static_cast<EulerAnglesType>(q).angles();
const 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(internal::isApprox<Scalar>(std::abs(q.dot(qbis)), ONE, precision));
//VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same //VERIFY_IS_APPROX(eabis, eabis2);// Verify that the euler angles are still the same
} }
@ -143,9 +151,10 @@ template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
template<typename Scalar> void check_singular_cases(const Scalar& singularBeta) template<typename Scalar> void check_singular_cases(const Scalar& singularBeta)
{ {
typedef Matrix<Scalar,3,1> Vector3; typedef Matrix<Scalar,3,1> Vector3;
const Scalar epsilon = std::numeric_limits<Scalar>::epsilon();
const Scalar PI = Scalar(EIGEN_PI); const Scalar PI = Scalar(EIGEN_PI);
for (Scalar epsilon = std::numeric_limits<Scalar>::epsilon(); epsilon < 1; epsilon *= Scalar(1.2))
{
check_all_var(Vector3(PI/4, singularBeta, PI/3)); 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 - epsilon, PI/3));
check_all_var(Vector3(PI/4, singularBeta - Scalar(1.5)*epsilon, PI/3)); check_all_var(Vector3(PI/4, singularBeta - Scalar(1.5)*epsilon, PI/3));
@ -155,6 +164,10 @@ template<typename Scalar> void check_singular_cases(const Scalar& singularBeta)
check_all_var(Vector3(PI*Scalar(-0.6), singularBeta + Scalar(1.5)*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.5), singularBeta + 2*epsilon, PI*Scalar(0.4)));
check_all_var(Vector3(PI*Scalar(0.9), singularBeta + epsilon, Scalar(0.8)*PI)); 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() template<typename Scalar> void eulerangles_manual()