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

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

Browse Source 
This reverts commit 7f06bcae2c4aae657fded7c7b999d69ee68962d9
This commit is contained in:
Rasmus Munk Larsen 2023-04-27 00:06:09 +00:00
parent 2d0c6ad873
commit 50813489e5
3 changed files with 21 additions and 70 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;
EIGEN_DEVICE_FUNC
inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2, bool canonical = true) const;
inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const;
// 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)

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

@ -30,19 +30,13 @@ namespace Eigen {
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames).
*
* 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.
* The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
*
* \sa class AngleAxis
*/
template<typename Derived>
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2, bool canonical) const
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
{
EIGEN_USING_STD(atan2)
EIGEN_USING_STD(sin)
@ -113,24 +107,6 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2, bool canonical) c
}
if (!odd)
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;
}

61
test/geo_eulerangles.cpp
View File

@ -20,47 +20,22 @@ void verify_euler(const Matrix<Scalar,3,1>& ea, int i, int j, int k)
typedef Matrix<Scalar,3,1> Vector3;
typedef AngleAxis<Scalar> AngleAxisx;
using std::abs;
const Matrix3 m(AngleAxisx(ea[0], Vector3::Unit(i)) * AngleAxisx(ea[1], Vector3::Unit(j)) * AngleAxisx(ea[2], Vector3::Unit(k)));
// Test the new default canonical ranges behaviour of eulerAngles (canonical = true)
{
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);
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));
}
// 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));
}
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));
}
template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea)
@ -108,8 +83,8 @@ template<typename Scalar> void eulerangles()
ea = m.eulerAngles(0,1,0);
check_all_var(ea);
// Check with random angles in range [-pi:pi]x[-pi:pi]x[-pi:pi].
ea = Array3::Random() * Scalar(EIGEN_PI)*Array3(1,1,1);
// 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_all_var(ea);
ea[2] = ea[0] = internal::random<Scalar>(0,Scalar(EIGEN_PI));