diff --git a/src/rotations.h b/src/rotations.h
index c25e246d273e457b3b3d12433da776a220444aa4..2ce22553796f83d6f3cdfbe497f57f5dab3ae326 100644
--- a/src/rotations.h
+++ b/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
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 4827de183a9a53fbb22bcc72d553aefbf2e6a33a..bac392115b91c812e8b079493ebe85b95ae3a8d2 100644
--- a/src/test/CMakeLists.txt
+++ b/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)
diff --git a/src/test/gtest_rotation.cpp b/src/test/gtest_rotation.cpp
index 6eb8d259682b2e56fd9baa43b3af269f291cc0e4..125d8a3a31f9126b7bd9069309c0e56e32351011 100644
--- a/src/test/gtest_rotation.cpp
+++ b/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)
{