devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Geometry/EulerAngles: introduce canonicalEulerAngles

Browse Source
This commit is contained in:
Juraj Oršulić
2023-05-19 15:42:22 +00:00
committed by Rasmus Munk Larsen
parent 7d9bb90f15
commit c18f94e3b0
3 changed files with 308 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

200
test/geo_eulerangles.cpp
View File

@@ -2,11 +2,15 @@
// for linear algebra.
//
// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2023 Juraj Oršulić, University of Zagreb <juraj.orsulic@fer.hr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// Silence warnings about using the deprecated non-canonical .eulerAngles(), which are still being tested.
#define EIGEN_NO_DEPRECATED_WARNING
#include "main.h"
#include <Eigen/Geometry>
#include <Eigen/LU>
@@ -14,53 +18,89 @@
template<typename Scalar>
void verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k)
void verify_euler(const Matrix<Scalar, 3, 1>& ea, int i, int j, int k)
{
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Matrix<Scalar, 3, 3> Matrix3;
typedef Matrix<Scalar, 3, 1> Vector3;
typedef AngleAxis<Scalar> AngleAxisx;
using std::abs;
Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k)));
Vector3 eabis = m.eulerAngles(i, j, k);
Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(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 || !numext::is_exactly_zero(ea[1])) && (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));
const Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k)));
// Test non-canonical eulerAngles
{
Vector3 eabis = m.eulerAngles(i, j, k);
Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k)));
VERIFY_IS_APPROX(m, mbis);
// 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));
}
// Test canonicalEulerAngles
{
Vector3 eabis = m.canonicalEulerAngles(i, j, k);
Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k)));
VERIFY_IS_APPROX(m, mbis);
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[0]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI));
if (i != k)
{
// Tait-Bryan sequence
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI / 2), eabis[1]);
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI / 2));
}
else
{
// Proper Euler sequence
// approx_or_less_than does not work for 0
VERIFY(0 < eabis[1] || test_isMuchSmallerThan(eabis[1], Scalar(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));
}
}
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
template<typename Scalar> void check_all_var(const Matrix<Scalar, 3, 1>& ea)
{
verify_euler(ea, 0,1,2);
verify_euler(ea, 0,1,0);
verify_euler(ea, 0,2,1);
verify_euler(ea, 0,2,0);
auto verify_permutation = [](const Matrix<Scalar, 3, 1>& eap)
{
verify_euler(eap, 0, 1, 2);
verify_euler(eap, 0, 1, 0);
verify_euler(eap, 0, 2, 1);
verify_euler(eap, 0, 2, 0);
verify_euler(ea, 1,2,0);
verify_euler(ea, 1,2,1);
verify_euler(ea, 1,0,2);
verify_euler(ea, 1,0,1);
verify_euler(eap, 1, 2, 0);
verify_euler(eap, 1, 2, 1);
verify_euler(eap, 1, 0, 2);
verify_euler(eap, 1, 0, 1);
verify_euler(ea, 2,0,1);
verify_euler(ea, 2,0,2);
verify_euler(ea, 2,1,0);
verify_euler(ea, 2,1,2);
verify_euler(eap, 2, 0, 1);
verify_euler(eap, 2, 0, 2);
verify_euler(eap, 2, 1, 0);
verify_euler(eap, 2, 1, 2);
};
int i, j, k;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
for (k = 0; k < 3; k++)
{
Matrix<Scalar,3,1> eap(ea(i), ea(j), ea(k));
verify_permutation(eap);
}
}
template<typename Scalar> void eulerangles()
{
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Array<Scalar,3,1> Array3;
typedef Matrix<Scalar, 3, 3> Matrix3;
typedef Matrix<Scalar, 3, 1> Vector3;
typedef Array<Scalar, 3, 1> Array3;
typedef Quaternion<Scalar> Quaternionx;
typedef AngleAxis<Scalar> AngleAxisx;
@@ -69,43 +109,97 @@ template<typename Scalar> void eulerangles()
q1 = AngleAxisx(a, Vector3::Random().normalized());
Matrix3 m;
m = q1;
Vector3 ea = m.eulerAngles(0,1,2);
Vector3 ea = m.eulerAngles(0, 1, 2);
check_all_var(ea);
ea = m.eulerAngles(0,1,0);
ea = m.eulerAngles(0, 1, 0);
check_all_var(ea);
// Check with purely random Quaternion:
q1.coeffs() = Quaternionx::Coefficients::Random().normalized();
m = q1;
ea = m.eulerAngles(0,1,2);
ea = m.eulerAngles(0, 1, 2);
check_all_var(ea);
ea = m.eulerAngles(0,1,0);
ea = m.eulerAngles(0, 1, 0);
check_all_var(ea);
// Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi].
ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1);
// Check with random angles in range [-pi:pi]x[-pi:pi]x[-pi:pi].
ea = Array3::Random() * Scalar(EIGEN_PI);
check_all_var(ea);
ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
auto test_with_some_zeros = [](const Vector3& eaz)
{
check_all_var(eaz);
Vector3 ea_glz = eaz;
ea_glz[0] = Scalar(0);
check_all_var(ea_glz);
ea_glz[0] = internal::random<Scalar>(-0.001, 0.001);
check_all_var(ea_glz);
ea_glz[2] = Scalar(0);
check_all_var(ea_glz);
ea_glz[2] = internal::random<Scalar>(-0.001, 0.001);
check_all_var(ea_glz);
};
// Check gimbal lock configurations and a bit noisy gimbal locks
Vector3 ea_gl = ea;
ea_gl[1] = EIGEN_PI/2;
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_gl[1] = -EIGEN_PI/2;
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_gl[1] = EIGEN_PI/2;
ea_gl[2] = ea_gl[0];
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_gl[1] = -EIGEN_PI/2;
test_with_some_zeros(ea_gl);
ea_gl[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
// Similar to above, but with pi instead of pi/2
Vector3 ea_pi = ea;
ea_pi[1] = EIGEN_PI;
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_pi[1] = -EIGEN_PI;
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_pi[1] = EIGEN_PI;
ea_pi[2] = ea_pi[0];
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea_pi[1] = -EIGEN_PI;
test_with_some_zeros(ea_gl);
ea_pi[1] += internal::random<Scalar>(-0.001, 0.001);
test_with_some_zeros(ea_gl);
ea[2] = ea[0] = internal::random<Scalar>(0, Scalar(EIGEN_PI));
check_all_var(ea);
ea[0] = ea[1] = internal::random<Scalar>(0,Scalar(EIGEN_PI));
ea[0] = ea[1] = internal::random<Scalar>(0, Scalar(EIGEN_PI));
check_all_var(ea);
ea[1] = 0;
check_all_var(ea);
ea.head(2).setZero();
check_all_var(ea);
ea.setZero();
check_all_var(ea);
}
EIGEN_DECLARE_TEST(geo_eulerangles)
{
for(int i = 0; i < g_repeat; i++) {
for(int i = 0; i < g_repeat; i++)
{
CALL_SUBTEST_1( eulerangles<float>() );
CALL_SUBTEST_2( eulerangles<double>() );
}