Reorganize rotations.h and add some new fcn

src/rotations.h

@@ -21,7 +21,8 @@ namespace wolf
  * @return formatted angle
  */
 template<typename T>
-inline T pi2pi(const T angle)
+inline T
+pi2pi(const T angle)
 {
     using std::fmod;
 
@@ -36,7 +37,8 @@ inline T pi2pi(const T angle)
  * @return angle in radians
  */
 template<typename T>
-inline T toRad(const T deg)
+inline T
+toRad(const T deg)
 {
     return (T)M_TORAD * deg;
 }
@@ -47,7 +49,8 @@ inline T toRad(const T deg)
  * @return angle in degrees
  */
 template<typename T>
-inline T toDeg(const T rad)
+inline T
+toDeg(const T rad)
 {
     return (T)M_TODEG * rad;
 }
@@ -61,7 +64,8 @@ inline T toDeg(const T rad)
  * @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBase<Derived>& _v)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+skew(const Eigen::MatrixBase<Derived>& _v)
 {
     MatrixSizeCheck<3,1>::check(_v);
 
@@ -82,7 +86,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> skew(const Eigen::MatrixBas
  * @return a 3-vector v such that skew(v) = _m
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase<Derived>& _m)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+vee(const Eigen::MatrixBase<Derived>& _m)
 {
     MatrixSizeCheck<3,3>::check(_m);
 
@@ -91,6 +96,190 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase
     return (Eigen::Matrix<T, 3, 1>() << _m(2, 1), _m(0, 2), _m(1, 0)).finished();
 }
 
+
+////////////////////////////////////////////////////
+// Rotation conversions q, R, Euler, back and forth
+//
+//  Euler angles are denoted 'e' and are in the ZYX convention
+
+/** \brief quaternion to rotation matrix conversion
+ *
+ * @param _q a right-handed unit quaternion
+ * @return the equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+q2R(const Eigen::QuaternionBase<Derived>& _q)
+{
+    return _q.matrix();
+}
+
+/** \brief quaternion to rotation matrix conversion
+ *
+ * @param _q a right-handed unit quaternion
+ * @return the equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+q2R(const Eigen::MatrixBase<Derived>& _q)
+{
+    MatrixSizeCheck<4,1>::check(_q);
+    Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
+    return q2R( q );
+}
+
+/** \brief rotation matrix to quaternion conversion
+ *
+ * @param _R a rotation matrix
+ * @return the equivalent right-handed unit quaternion
+ */
+template<typename Derived>
+inline Eigen::Quaternion<typename Derived::Scalar>
+R2q(const Eigen::MatrixBase<Derived>& _R)
+{
+    MatrixSizeCheck<3,3>::check(_R);
+
+    return Eigen::Quaternion<typename Derived::Scalar>(_R);
+}
+
+/** \brief Euler angles to quaternion
+ * \param _euler = (roll pitch yaw) in ZYX convention
+ * \return equivalent quaternion
+ */
+template<typename D>
+inline Eigen::Quaternion<typename D::Scalar>
+e2q(const Eigen::MatrixBase<D>& _euler)
+{
+    MatrixSizeCheck<3,1>::check(_euler);
+
+    typedef typename D::Scalar T;
+
+    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(_euler(0), Eigen::Matrix<T, 3, 1>::UnitX());
+    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(_euler(1), Eigen::Matrix<T, 3, 1>::UnitY());
+    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(_euler(2), Eigen::Matrix<T, 3, 1>::UnitZ());
+
+    return Eigen::Quaternion<T>(az * ay * ax);
+}
+
+
+
+/** \brief Euler angles to quaternion
+ * \param roll in ZYX convention
+ * \param pitch in ZYX convention
+ * \param yaw in ZYX convention
+ * \return equivalent quaternion
+ */
+template<typename T>
+inline Eigen::Quaternion<T>
+e2q(const T roll,
+    const T pitch,
+    const T yaw)
+{
+    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll,  Eigen::Matrix<T, 3, 1>::UnitX());
+    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
+    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw,   Eigen::Matrix<T, 3, 1>::UnitZ());
+
+    return (az * ay * ax);
+}
+
+
+/** \brief Euler angles to rotation matrix
+ * \param roll in ZYX convention
+ * \param pitch in ZYX convention
+ * \param yaw in ZYX convention
+ * \return equivalent rotation matrix
+ */
+template<typename T>
+inline Eigen::Matrix<T, 3, 3>
+e2R(const T roll,
+    const T pitch,
+    const T yaw)
+{
+    return e2q(roll, pitch, yaw).matrix();
+}
+
+/** \brief Euler angles to rotation matrix
+ * \param _e = (roll pitch yaw) in ZYX convention
+ * \return equivalent rotation matrix
+ */
+template<typename Derived>
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+e2R(Eigen::MatrixBase<Derived> _e)
+{
+    return e2R(_e(0), _e(1), _e(2));
+}
+
+
+template <typename Derived>
+inline typename Eigen::MatrixBase<Derived>::Scalar
+getYaw(const Eigen::MatrixBase<Derived>& R)
+{
+    MatrixSizeCheck<3, 3>::check(R);
+
+    using std::atan2;
+    return atan2( R(1, 0), R(0, 0) );
+}
+
+template<typename Derived>
+inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
+R2e(const Eigen::MatrixBase<Derived>& _R)
+{
+    Eigen::Matrix<typename Derived::Scalar, 3, 1> e;
+
+    e(0) = atan2(_R(2,1),_R(2,2));
+    e(1) = atan2(-_R(2,0), sqrt(_R(0,0)*_R(0,0) + _R(1,0)*_R(1,0)));
+    e(2) = atan2(_R(1,0), _R(0,0));
+
+    return e;
+}
+
+
+template<typename Derived>
+inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
+q2e(const Eigen::MatrixBase<Derived>& _q)
+{
+    typedef typename Derived::Scalar T;
+    Eigen::Matrix<T, 3, 1> e;
+
+    Scalar w  = _q(3,0);
+    Scalar x  = _q(0,0);
+    Scalar y  = _q(1,0);
+    Scalar z  = _q(2,0);
+
+    Scalar wx = w*x;
+    Scalar xx = x*x;
+    Scalar xy = x*y;
+    Scalar xz = x*z;
+    Scalar wy = w*y;
+    Scalar yy = y*y;
+    Scalar yz = y*z;
+    Scalar zz = z*z;
+    Scalar wz = w*z;
+
+    Scalar r00 = T(1) - T(2) * (yy + zz);
+    Scalar r10 = T(2) * ( xy + wz );
+    Scalar r20 = T(2) * ( xz - wy );
+    Scalar r21 = T(2) * ( yz + wx );
+    Scalar r22 = T(1) - T(2) * (xx + yy);
+
+    e(0) = atan2( r21, r22);
+    e(1) = atan2(-r20, sqrt(r00*r00 + r10*r10));
+    e(2) = atan2( r10, r00);
+
+    return e;
+}
+
+
+template<typename Derived>
+inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1>
+q2e(const Eigen::QuaternionBase<Derived>& _q)
+{
+    return q2e(_q.coeffs());
+}
+
+
+
+
 ///////////////////////////////////////////////////////////////
 // Rotation conversions - exp and log maps
 
@@ -100,7 +289,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 1> vee(const Eigen::MatrixBase
  * @return the right-handed unit quaternion performing the rotation encoded by _v
  */
 template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase<Derived>& _v)
+inline Eigen::Quaternion<typename Derived::Scalar>
+exp_q(const Eigen::MatrixBase<Derived>& _v)
 {
     using std::sqrt;
     using std::cos;
@@ -129,7 +319,8 @@ inline Eigen::Quaternion<typename Derived::Scalar> exp_q(const Eigen::MatrixBase
  * @return a rotation vector v such that _q = exp_q(v)
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_q(const Eigen::QuaternionBase<Derived>& _q)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+log_q(const Eigen::QuaternionBase<Derived>& _q)
 {
 
     // Will try this implementation once Eigen accepts it!
@@ -176,7 +367,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_q(const Eigen::Quaterni
  * @return the rotation matrix performing the rotation encoded by _v
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBase<Derived>& _v)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+exp_R(const Eigen::MatrixBase<Derived>& _v)
 {
     using std::sqrt;
 
@@ -193,26 +385,14 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> exp_R(const Eigen::MatrixBa
         return Eigen::Matrix<T, 3, 3>::Identity() + skew(_v);
 }
 
-/** \brief rotation matrix to quaternion conversion
- *
- * @param _R a rotation matrix
- * @return the equivalent right-handed unit quaternion
- */
-template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar> R2q(const Eigen::MatrixBase<Derived>& _R)
-{
-    MatrixSizeCheck<3,3>::check(_R);
-
-    return Eigen::Quaternion<typename Derived::Scalar>(_R);
-}
-
 /** \brief Rotation matrix logarithmic map
  *
  * @param _R a 3D rotation matrix
  * @return the rotation vector v such that _R = exp_R(v)
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBase<Derived>& _R)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+log_R(const Eigen::MatrixBase<Derived>& _R)
 {
     return log_q(R2q(_R));
 }
@@ -223,7 +403,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 1> log_R(const Eigen::MatrixBa
  * @return the equivalent right-handed unit quaternion
  */
 template<typename Derived>
-inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<Derived>& _v)
+inline Eigen::Quaternion<typename Derived::Scalar>
+v2q(const Eigen::MatrixBase<Derived>& _v)
 {
     return exp_q(_v);
 }
@@ -234,7 +415,8 @@ inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<D
  * @return the equivalent rotation vector
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1> q2v(const Eigen::QuaternionBase<Derived>& _q)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+q2v(const Eigen::QuaternionBase<Derived>& _q)
 {
     return log_q(_q);
 }
@@ -245,7 +427,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 1> q2v(const Eigen::Quaternion
  * @return the equivalent rotation matrix
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> v2R(const Eigen::MatrixBase<Derived>& _v)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+v2R(const Eigen::MatrixBase<Derived>& _v)
 {
     return exp_R(_v);
 }
@@ -256,34 +439,13 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> v2R(const Eigen::MatrixBase
  * @return the equivalent rotation vector
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 1> R2v(const Eigen::MatrixBase<Derived>& _R)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+R2v(const Eigen::MatrixBase<Derived>& _R)
 {
     return log_R(_R);
 }
 
-/** \brief quaternion to rotation matrix conversion
- *
- * @param _q a right-handed unit quaternion
- * @return the equivalent rotation matrix
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::QuaternionBase<Derived>& _q)
-{
-    return _q.matrix();
-}
 
-/** \brief quaternion to rotation matrix conversion
- *
- * @param _q a right-handed unit quaternion
- * @return the equivalent rotation matrix
- */
-template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase<Derived>& _q)
-{
-    MatrixSizeCheck<4,1>::check(_q);
-    Eigen::Quaternion<typename Derived::Scalar> q(_q(3),_q(0),_q(1),_q(2));
-    return q2R( q );
-}
 
 /////////////////////////////////////////////////////////////////
 // Jacobians of SO(3)
@@ -303,7 +465,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> q2R(const Eigen::MatrixBase
  */
 
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+jac_SO3_right(const Eigen::MatrixBase<Derived>& _theta)
 {
     using std::sqrt;
     using std::cos;
@@ -342,7 +505,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right(const Eigen::
  *      log( R * exp(d_theta) ) = log(R) + Jrinv(theta) * d_theta
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+jac_SO3_right_inv(const Eigen::MatrixBase<Derived>& _theta)
 {
     using std::sqrt;
     using std::cos;
@@ -376,7 +540,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_right_inv(const Eig
  *      exp(theta+d_theta) = exp(Jl(theta)*d_theta)*exp(theta)
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+jac_SO3_left(const Eigen::MatrixBase<Derived>& _theta)
 {
     using std::sqrt;
     using std::cos;
@@ -414,7 +579,8 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left(const Eigen::M
  *      log( exp(d_theta) * R ) = log(R) + Jlinv(theta) * d_theta
  */
 template<typename Derived>
-inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
+inline Eigen::Matrix<typename Derived::Scalar, 3, 3>
+jac_SO3_left_inv(const Eigen::MatrixBase<Derived>& _theta)
 {
     using std::sqrt;
     using std::cos;
@@ -435,11 +601,12 @@ inline Eigen::Matrix<typename Derived::Scalar, 3, 3> jac_SO3_left_inv(const Eige
 }
 
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void compose(const Eigen::QuaternionBase<D1>& _q1,
-                    const Eigen::QuaternionBase<D2>& _q2,
-                    Eigen::QuaternionBase<D3>& _q_comp,
-                    Eigen::MatrixBase<D4>& _J_comp_q1,
-                    Eigen::MatrixBase<D5>& _J_comp_q2)
+inline void
+compose(const Eigen::QuaternionBase<D1>& _q1,
+        const Eigen::QuaternionBase<D2>& _q2,
+        Eigen::QuaternionBase<D3>& _q_comp,
+        Eigen::MatrixBase<D4>& _J_comp_q1,
+        Eigen::MatrixBase<D5>& _J_comp_q2)
 {
     MatrixSizeCheck<3, 3>::check(_J_comp_q1);
     MatrixSizeCheck<3, 3>::check(_J_comp_q2);
@@ -451,11 +618,12 @@ inline void compose(const Eigen::QuaternionBase<D1>& _q1,
 }
 
 template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void between(const Eigen::QuaternionBase<D1>& _q1,
-                    const Eigen::QuaternionBase<D2>& _q2,
-                    Eigen::QuaternionBase<D3>& _q_between,
-                    Eigen::MatrixBase<D4>& _J_between_q1,
-                    Eigen::MatrixBase<D5>& _J_between_q2)
+inline void
+between(const Eigen::QuaternionBase<D1>& _q1,
+        const Eigen::QuaternionBase<D2>& _q2,
+        Eigen::QuaternionBase<D3>& _q_between,
+        Eigen::MatrixBase<D4>& _J_between_q1,
+        Eigen::MatrixBase<D5>& _J_between_q2)
 {
     MatrixSizeCheck<3, 3>::check(_J_between_q1);
     MatrixSizeCheck<3, 3>::check(_J_between_q2);
@@ -467,86 +635,56 @@ inline void between(const Eigen::QuaternionBase<D1>& _q1,
 }
 
 template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar> plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
+inline Eigen::Quaternion<typename D1::Scalar>
+plus_right(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
 {
     MatrixSizeCheck<3,1>::check(v);
     return q * exp_q(v);
 }
 
 template<typename D1, typename D2>
-inline  Eigen::Matrix<typename D2::Scalar, 3, 1> minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+inline  Eigen::Matrix<typename D2::Scalar, 3, 1>
+minus_right(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
 {
     return log_q(q1.conjugate() * q2);
 }
 
 template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar> plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q)
+inline Eigen::Quaternion<typename D1::Scalar>
+plus_left(const Eigen::MatrixBase<D2>& v, const Eigen::QuaternionBase<D1>& q)
 {
     MatrixSizeCheck<3,1>::check(v);
     return exp_q(v) * q;
 }
 
 template<typename D1, typename D2>
-inline  Eigen::Matrix<typename D2::Scalar, 3, 1> minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+inline  Eigen::Matrix<typename D2::Scalar, 3, 1>
+minus_left(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
 {
     return log_q(q2 * q1.conjugate());
 }
 
 template<typename D1, typename D2>
-inline Eigen::Quaternion<typename D1::Scalar> plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
+inline Eigen::Quaternion<typename D1::Scalar>
+plus(const Eigen::QuaternionBase<D1>& q, const Eigen::MatrixBase<D2>& v)
 {
     return plus_right(q, v);
 }
 
 template<typename D1, typename D2>
-inline  Eigen::Matrix<typename D2::Scalar, 3, 1> minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+inline  Eigen::Matrix<typename D2::Scalar, 3, 1>
+minus(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
 {
     return minus_right(q1, q2);
 }
 
 template<typename D1, typename D2>
-inline  Eigen::Matrix<typename D2::Scalar, 3, 1> diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
+inline  Eigen::Matrix<typename D2::Scalar, 3, 1>
+diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2)
 {
     return minus(q1, q2);
 }
 
-template<typename D>
-inline Eigen::Quaternion<typename D::Scalar> e2q(const Eigen::MatrixBase<D>& _euler)
-{
-    MatrixSizeCheck<3,1>::check(_euler);
-
-    typedef typename D::Scalar T;
-
-    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(_euler(0), Eigen::Matrix<T, 3, 1>::UnitX());
-    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(_euler(1), Eigen::Matrix<T, 3, 1>::UnitY());
-    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(_euler(2), Eigen::Matrix<T, 3, 1>::UnitZ());
-
-    return Eigen::Quaternion<T>(az * ay * ax);
-}
-
-
-
-template<typename T>
-inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll,
-                                                 const T pitch,
-                                                 const T yaw)
-{
-    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll,  Eigen::Matrix<T, 3, 1>::UnitX());
-    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
-    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw,   Eigen::Matrix<T, 3, 1>::UnitZ());
-
-    return (az * ay * ax).toRotationMatrix().matrix();
-}
-
-template <typename Derived>
-inline typename Eigen::MatrixBase<Derived>::Scalar
-getYaw(const Eigen::MatrixBase<Derived>& R)
-{
-    MatrixSizeCheck<3, 3>::check(R);
-
-    using std::atan2;
-    return atan2( R(1, 0), R(0, 0) );
-}
 
 } // namespace wolf
 

src/test/gtest_rotation.cpp

@@ -683,6 +683,54 @@ TEST(log_q, small)
     }
 }
 
+TEST(Conversions, e2q_q2R_R2e)
+{
+    Vector3s e, eo;
+    Quaternions q;
+    Matrix3s R;
+
+    e.setRandom();
+    e << 0.1, .2, .3;
+    q = e2q(e);
+    R = q2R(q);
+
+    eo = R2e(R);
+
+    WOLF_TRACE("euler    ", e.transpose());
+    WOLF_TRACE("quat     ", q.coeffs().transpose());
+    WOLF_TRACE("R \n", R);
+
+    WOLF_TRACE("euler o  ", eo.transpose());
+
+
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+
+}
+
+TEST(Conversions, e2R_R2q_q2e)
+{
+    Vector3s e, eo;
+    Quaternions q;
+    Matrix3s R;
+
+    e.setRandom();
+    e << 0.1, 0.2, 0.3;
+    R = e2R(e(0), e(1), e(2));
+    q = R2q(R);
+
+    eo = q2e(q.coeffs());
+
+    WOLF_TRACE("euler    ", e.transpose());
+    WOLF_TRACE("R \n", R);
+    WOLF_TRACE("quat     ", q.coeffs().transpose());
+
+    WOLF_TRACE("euler o  ", eo.transpose());
+
+
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+
+}
+
 
 int main(int argc, char **argv)
 {