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

Geometry/EulerAngles: make sure that returned solution has canonical ranges

Browse Source
This commit is contained in:
Juraj Oršulić 2023-04-19 19:12:24 +00:00 committed by Rasmus Munk Larsen
parent a347dbbab2
commit 7f06bcae2c
3 changed files with 70 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Eigen/src/Core/MatrixBase.h
View File

@ -400,7 +400,7 @@ template<typename Derived> class MatrixBase
inline PlainObject unitOrthogonal(void) const; inline PlainObject unitOrthogonal(void) const;
EIGEN_DEVICE_FUNC EIGEN_DEVICE_FUNC
inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const; inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2, bool canonical = true) const;
// put this as separate enum value to work around possible GCC 4.3 bug (?) // put this as separate enum value to work around possible GCC 4.3 bug (?)
enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical) enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical)

28
Eigen/src/Geometry/EulerAngles.h
View File

@ -30,13 +30,19 @@ namespace Eigen {
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames). * This corresponds to the right-multiply conventions (with right hand side frames).
* *
* The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi]. * When canonical == true (the default):
* For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi].
* For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi].
*
* When canonical == false:
* The returned angles follow a non-standard range convention used by legacy versions of Eigen, [0:pi]x[-pi:pi]x[-pi:pi].
* Set canonical to false to retain legacy behaviour.
* *
* \sa class AngleAxis * \sa class AngleAxis
*/ */
template<typename Derived> template<typename Derived>
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2, bool canonical) const
{ {
EIGEN_USING_STD(atan2) EIGEN_USING_STD(atan2)
EIGEN_USING_STD(sin) EIGEN_USING_STD(sin)
@ -107,6 +113,24 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
} }
if (!odd) if (!odd)
res = -res; res = -res;
if (canonical)
{
// If Tait-Bryan angles, make sure that the result is in the canonical range (middle axis angle in [-pi/2, pi/2]).
if (a0 != a2 && res.cwiseAbs()[1] > Scalar(EIGEN_PI / 2))
{
res -= Scalar(EIGEN_PI) * res.cwiseSign();
res[1] = -res[1];
}
// If proper Euler angles, make sure that the result is in the canonical range (middle axis angle in [0, pi]).
if (a0 == a2 && res[1] < Scalar(0))
{
res[0] -= Scalar(EIGEN_PI) * res.cwiseSign()[0];
res[2] -= Scalar(EIGEN_PI) * res.cwiseSign()[2];
res[1] = -res[1];
}
}
return res; return res;
} }

61
test/geo_eulerangles.cpp
View File

@ -20,22 +20,47 @@ void verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k)
typedef Matrix<Scalar,3,1> Vector3; typedef Matrix<Scalar,3,1> Vector3;
typedef AngleAxis<Scalar> AngleAxisx; typedef AngleAxis<Scalar> AngleAxisx;
using std::abs; using std::abs;
Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k))); const 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))); // Test the new default canonical ranges behaviour of eulerAngles (canonical = true)
VERIFY_IS_APPROX(m, mbis); {
/* If I==K, and ea[1]==0, then there no unique solution. */ Vector3 eabis = m.eulerAngles(i, j, k);
/* The remark apply in the case where I!=K, and |ea[1]| is close to pi/2. */ Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit(i)) * AngleAxisx(eabis[1], Vector3::Unit(j)) * AngleAxisx(eabis[2], Vector3::Unit(k)));
if((i!=k || !numext::is_exactly_zero(ea[1])) && (i == k || !internal::isApprox(abs(ea[1]), Scalar(EIGEN_PI / 2), test_precision<Scalar>())) ) VERIFY_IS_APPROX(m, mbis);
VERIFY((ea-eabis).norm() <= test_precision<Scalar>());
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[0]);
// approx_or_less_than does not work for 0 VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI));
VERIFY(0 < eabis[0] || test_isMuchSmallerThan(eabis[0], Scalar(1))); if (i != k)
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[0], Scalar(EIGEN_PI)); {
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[1]); // Tait-Bryan sequence
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI)); VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI / 2), eabis[1]);
VERIFY_IS_APPROX_OR_LESS_THAN(-Scalar(EIGEN_PI), eabis[2]); VERIFY_IS_APPROX_OR_LESS_THAN(eabis[1], Scalar(EIGEN_PI / 2));
VERIFY_IS_APPROX_OR_LESS_THAN(eabis[2], Scalar(EIGEN_PI)); }
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));
}
// Test legacy behaviour of eulerAngles (canonical = false)
{
Vector3 eabis = m.eulerAngles(i, j, k, false);
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));
}
} }
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)
@ -83,8 +108,8 @@ template<typename Scalar> void eulerangles()
ea = m.eulerAngles(0,1,0); ea = m.eulerAngles(0,1,0);
check_all_var(ea); check_all_var(ea);
// Check with random angles in range [0:pi]x[-pi:pi]x[-pi:pi]. // Check with random angles in range [-pi:pi]x[-pi:pi]x[-pi:pi].
ea = (Array3::Random() + Array3(1,0,0))*Scalar(EIGEN_PI)*Array3(0.5,1,1); ea = Array3::Random() * Scalar(EIGEN_PI)*Array3(1,1,1);
check_all_var(ea); check_all_var(ea);
ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI)); ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));