Skip to content
Snippets Groups Projects
Commit 64f3b4fe authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Rotations

parent 02ff391e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/rotations.h
+ 210
− 106
View file @ 64f3b4fe
......@@ -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
+ 1
− 1
View file @ 64f3b4fe
......@@ -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
+ 92
− 24
View file @ 64f3b4fe
......@@ -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)
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment