Attachment 342 Details for Bug 609 – Avoid if statement and improve consistency of eulerAngles method

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:

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>() );
>   }
> }

Actions: View | Diff

Attachments on bug 609: 342 | 360