Merge branch 'rotations' into 'master'

Rotations

See merge request mobile_robotics/wolf!231

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,156 @@ 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 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(const Eigen::MatrixBase<Derived>& _e)
+{
+    MatrixSizeCheck<3, 1>::check(_e);
+
+    return e2q(_e).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) );
+}
+
+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);
+    Scalar x  = _q(0);
+    Scalar y  = _q(1);
+    Scalar z  = _q(2);
+
+    Scalar xx = x*x;
+    Scalar xy = x*y;
+    Scalar xz = x*z;
+    Scalar yy = y*y;
+    Scalar yz = y*z;
+    Scalar zz = z*z;
+    Scalar wx = w*x;
+    Scalar wy = w*y;
+    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 +255,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 +285,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 +333,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 +351,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 +369,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 +381,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 +393,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 +405,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 +431,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 +471,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 +506,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 +545,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 +567,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 +584,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 +601,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/CMakeLists.txt

@@ -94,7 +94,7 @@ target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
   
 # Rotation test
 wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
-target_link_libraries(gtest_rotation ${PROJECT_NAME})
+#target_link_libraries(gtest_rotation ${PROJECT_NAME})
 
 # SensorBase test
 wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)

src/test/gtest_rotation.cpp

@@ -441,25 +441,6 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
     "\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
 }
 
-TEST(q2R, q2R_R2q)
-{
-    Vector3s v; v.setRandom();
-    Quaternions q = v2q(v);
-    Matrix3s R = v2R(v);
-
-    Quaternions q_R = R2q(R);
-    Quaternions qq_R(R);
-
-    ASSERT_NEAR(q.norm(),    1, wolf::Constants::EPS);
-    ASSERT_NEAR(q_R.norm(),  1, wolf::Constants::EPS);
-    ASSERT_NEAR(qq_R.norm(), 1, wolf::Constants::EPS);
-
-    ASSERT_MATRIX_APPROX(q.coeffs(), R2q(R).coeffs(), wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(),   wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(R,          q2R(q),          wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(R,          qq_R.matrix(),   wolf::Constants::EPS);
-}
-
 TEST(Plus, Random)
 {
     Quaternions q;
@@ -495,20 +476,29 @@ TEST(Plus, Identity_plus_small)
 
 TEST(Minus_and_diff, Random)
 {
-    Quaternions q1, q2;
+    Quaternions q1, q2, qo;
     q1              .coeffs().setRandom().normalize();
     q2              .coeffs().setRandom().normalize();
 
+
     Vector3s vr      = log_q(q1.conjugate() * q2);
     Vector3s vl      = log_q(q2 * q1.conjugate());
 
     ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12);
-    ASSERT_QUATERNION_APPROX(plus(q1, minus(q1, q2)), q2, 1e-12);
     ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12);
-    ASSERT_QUATERNION_APPROX(plus(q1, diff(q1, q2)), q2, 1e-12);
-
     ASSERT_MATRIX_APPROX(minus_left(q1, q2), vl, 1e-12);
-    ASSERT_QUATERNION_APPROX(plus_left(minus_left(q1, q2), q1), q2, 1e-12);
+
+    qo = plus(q1, minus(q1, q2));
+    if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
+    ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
+
+    qo = plus(q1, diff(q1, q2));
+    if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
+    ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
+
+    qo = plus_left(minus_left(q1, q2), q1);
+    if (q2.w() * qo.w() < 0) q2.coeffs() = -(q2.coeffs()); // allow q = -q
+    ASSERT_QUATERNION_APPROX(qo, q2, 1e-12);
 }
 
 TEST(Jacobians, Jr)
@@ -683,6 +673,84 @@ TEST(log_q, small)
     }
 }
 
+TEST(Conversions, q2R_R2q)
+{
+    Vector3s v; v.setRandom();
+    Quaternions q = v2q(v);
+    Matrix3s R = v2R(v);
+
+    Quaternions q_R = R2q(R);
+    Quaternions qq_R(R);
+
+    ASSERT_NEAR(q.norm(),    1, wolf::Constants::EPS);
+    ASSERT_NEAR(q_R.norm(),  1, wolf::Constants::EPS);
+    ASSERT_NEAR(qq_R.norm(), 1, wolf::Constants::EPS);
+
+    ASSERT_MATRIX_APPROX(q.coeffs(), R2q(R).coeffs(), wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(q.coeffs(), qq_R.coeffs(),   wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(R,          q2R(q),          wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(R,          qq_R.matrix(),   wolf::Constants::EPS);
+}
+
+TEST(Conversions, e2q_q2e)
+{
+    Vector3s e, eo;
+    Quaternions q;
+
+    e << 0.1, .2, .3;
+
+    q = e2q(e);
+
+    eo = q2e(q);
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+
+    eo = q2e(q.coeffs());
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+
+}
+
+TEST(Conversions, e2q_q2R_R2e)
+{
+    Vector3s e, eo;
+    Quaternions q;
+    Matrix3s R;
+
+    e << 0.1, .2, .3;
+    q = e2q(e);
+    R = q2R(q);
+
+    eo = R2e(R);
+
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+}
+
+TEST(Conversions, e2R_R2e)
+{
+    Vector3s e, eo;
+    Matrix3s R;
+
+    e << 0.1, 0.2, 0.3;
+
+    R  = e2R(e);
+    eo = R2e(R);
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+}
+
+TEST(Conversions, e2R_R2q_q2e)
+{
+    Vector3s e, eo;
+    Quaternions q;
+    Matrix3s R;
+
+    e << 0.1, 0.2, 0.3;
+    R = e2R(e);
+    q = R2q(R);
+
+    eo = q2e(q.coeffs());
+
+    ASSERT_MATRIX_APPROX(eo, e, 1e-10);
+}
+
 
 int main(int argc, char **argv)
 {