Bugzilla – Attachment 342 Details for
Bug 609
Euler Angles not Angle reversible, documentation example lacking, possible gimble lock issue resolveable
Home
|
New
|
Browse
|
Search
|
[?]
|
Reports
|
Requests
|
Help
|
Log In
[x]
|
Forgot Password
Login:
[x]
This bugzilla service is closed. All entries have been migrated to
https://gitlab.com/libeigen/eigen
[patch]
Avoid if statement and improve consistency of eulerAngles method
improve_eulerangles.diff (text/plain), 6.80 KB, created by
Gael Guennebaud
on 2013-06-06 22:44:15 UTC
(
hide
)
Description:
Avoid if statement and improve consistency of eulerAngles method
Filename:
MIME Type:
Creator:
Gael Guennebaud
Created:
2013-06-06 22:44:15 UTC
Size:
6.80 KB
patch
obsolete
>diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h >--- a/Eigen/src/Geometry/EulerAngles.h >+++ b/Eigen/src/Geometry/EulerAngles.h >@@ -22,64 +22,84 @@ namespace Eigen { > * \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode > * "2" represents the z axis and "0" the x axis, etc. The returned angles are such that > * we have the following equality: > * \code > * mat == AngleAxisf(ea[0], Vector3f::UnitZ()) > * * AngleAxisf(ea[1], Vector3f::UnitX()) > * * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode > * This corresponds to the right-multiply conventions (with right hand side frames). >+ * >+ * The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi]. >+ * >+ * \sa class AngleAxis > */ > template<typename Derived> > inline Matrix<typename MatrixBase<Derived>::Scalar,3,1> > MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const > { > using std::atan2; >+ using std::sin; >+ using std::cos; > /* Implemented from Graphics Gems IV */ > EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3) > > Matrix<Scalar,3,1> res; > typedef Matrix<typename Derived::Scalar,2,1> Vector2; > const Scalar epsilon = NumTraits<Scalar>::dummy_precision(); > > const Index odd = ((a0+1)%3 == a1) ? 0 : 1; > const Index i = a0; > const Index j = (a0 + 1 + odd)%3; > const Index k = (a0 + 2 - odd)%3; >- >+ > if (a0==a2) > { >- Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm(); >- res[1] = atan2(s, coeff(i,i)); >- if (s > epsilon) >+ res[0] = atan2(coeff(j,i), coeff(k,i)); >+ if((odd && res[0]<0) || ((!odd) && res[0]>0)) > { >- res[0] = atan2(coeff(j,i), coeff(k,i)); >- res[2] = atan2(coeff(i,j),-coeff(i,k)); >+ res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] + M_PI; >+ Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); >+ res[1] = -atan2(s2, coeff(i,i)); > } > else > { >- res[0] = Scalar(0); >- res[2] = (coeff(i,i)>0?1:-1)*atan2(-coeff(k,j), coeff(j,j)); >+ Scalar s2 = Vector2(coeff(j,i), coeff(k,i)).norm(); >+ res[1] = atan2(s2, coeff(i,i)); > } >- } >+ >+ // With a=(0,1,0), we have i=0; j=1; k=2, and after computing the first two angles, >+ // we can compute their respective rotation, and apply its inverse to M. Since the result must >+ // be a rotation around x, we have: >+ // >+ // c2 s1.s2 c1.s2 1 0 0 >+ // 0 c1 -s1 * M = 0 c3 s3 >+ // -s2 s1.c2 c1.c2 0 -s3 c3 >+ // >+ // Thus: m11.c1 - m21.s1 = c3 & m12.c1 - m22.s1 = s3 >+ >+ Scalar s1 = sin(res[0]); >+ Scalar c1 = cos(res[0]); >+ res[2] = atan2(c1*coeff(j,k)-s1*coeff(k,k), c1*coeff(j,j) - s1 * coeff(k,j)); >+ } > else > { >- Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm(); >- res[1] = atan2(-coeff(i,k), c); >- if (c > epsilon) >- { >- res[0] = atan2(coeff(j,k), coeff(k,k)); >- res[2] = atan2(coeff(i,j), coeff(i,i)); >+ res[0] = atan2(coeff(j,k), coeff(k,k)); >+ Scalar c2 = Vector2(coeff(i,i), coeff(i,j)).norm(); >+ if((odd && res[0]<0) || ((!odd) && res[0]>0)) { >+ res[0] = (res[0] > 0) ? res[0] - M_PI : res[0] + M_PI; >+ res[1] = atan2(-coeff(i,k), -c2); > } > else >- { >- res[0] = Scalar(0); >- res[2] = (coeff(i,k)>0?1:-1)*atan2(-coeff(k,j), coeff(j,j)); >- } >+ res[1] = atan2(-coeff(i,k), c2); >+ Scalar s1 = sin(res[0]); >+ Scalar c1 = cos(res[0]); >+ res[2] = atan2(s1*coeff(k,i)-c1*coeff(j,i), c1*coeff(j,j) - s1 * coeff(k,j)); > } > if (!odd) > res = -res; >+ > return res; > } > > } // end namespace Eigen > > #endif // EIGEN_EULERANGLES_H >diff --git a/test/geo_eulerangles.cpp b/test/geo_eulerangles.cpp >--- a/test/geo_eulerangles.cpp >+++ b/test/geo_eulerangles.cpp >@@ -1,38 +1,34 @@ > // This file is part of Eigen, a lightweight C++ template library > // for linear algebra. > // >-// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr> >+// Copyright (C) 2008-2012 Gael Guennebaud <gael.guennebaud@inria.fr> > // > // 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/. > > #include "main.h" > #include <Eigen/Geometry> > #include <Eigen/LU> > #include <Eigen/SVD> > >-template<typename Scalar> void eulerangles(void) >+template<typename Scalar> void check_all_var(const Matrix<Scalar,3,1>& ea) > { > typedef Matrix<Scalar,3,3> Matrix3; > typedef Matrix<Scalar,3,1> Vector3; >- typedef Quaternion<Scalar> Quaternionx; > typedef AngleAxis<Scalar> AngleAxisx; >- >- Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); >- Quaternionx q1; >- q1 = AngleAxisx(a, Vector3::Random().normalized()); >- Matrix3 m; >- m = q1; >- >+ > #define VERIFY_EULER(I,J,K, X,Y,Z) { \ >- Vector3 ea = m.eulerAngles(I,J,K); \ >- VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \ >+ Matrix3 m(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \ >+ Vector3 eabis = m.eulerAngles(I,J,K); \ >+ Matrix3 mbis(AngleAxisx(eabis[0], Vector3::Unit##X()) * AngleAxisx(eabis[1], Vector3::Unit##Y()) * AngleAxisx(eabis[2], Vector3::Unit##Z())); \ >+ VERIFY_IS_APPROX(m, mbis); \ >+ if(I!=K || ea[1]!=0) VERIFY_IS_APPROX(ea, eabis); \ > } > VERIFY_EULER(0,1,2, X,Y,Z); > VERIFY_EULER(0,1,0, X,Y,X); > VERIFY_EULER(0,2,1, X,Z,Y); > VERIFY_EULER(0,2,0, X,Z,X); > > VERIFY_EULER(1,2,0, Y,Z,X); > VERIFY_EULER(1,2,1, Y,Z,Y); >@@ -40,15 +36,53 @@ template<typename Scalar> void eulerangl > VERIFY_EULER(1,0,1, Y,X,Y); > > VERIFY_EULER(2,0,1, Z,X,Y); > VERIFY_EULER(2,0,2, Z,X,Z); > VERIFY_EULER(2,1,0, Z,Y,X); > VERIFY_EULER(2,1,2, Z,Y,Z); > } > >+template<typename Scalar> void eulerangles(void) >+{ >+ 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; >+ >+ Scalar a = internal::random<Scalar>(-Scalar(M_PI), Scalar(M_PI)); >+ Quaternionx q1; >+ q1 = AngleAxisx(a, Vector3::Random().normalized()); >+ Matrix3 m; >+ m = q1; >+ >+ Vector3 ea = m.eulerAngles(0,1,2); >+ check_all_var(ea); >+ ea = m.eulerAngles(0,1,0); >+ check_all_var(ea); >+ >+ ea = (Array3::Random() + Array3(1,1,0))*M_PI*Array3(0.5,0.5,1); >+ check_all_var(ea); >+ >+ ea[2] = ea[0] = internal::random<Scalar>(0,M_PI); >+ check_all_var(ea); >+ >+ ea[0] = ea[1] = internal::random<Scalar>(0,M_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); >+} >+ > void test_geo_eulerangles() > { > for(int i = 0; i < g_repeat; i++) { > CALL_SUBTEST_1( eulerangles<float>() ); > CALL_SUBTEST_2( eulerangles<double>() ); > } > }
You cannot view the attachment while viewing its details because your browser does not support IFRAMEs.
View the attachment on a separate page
.
View Attachment As Diff
View Attachment As Raw
Actions:
View
|
Diff
Attachments on
bug 609
: 342 |
360