diff --git a/src/landmark_apriltag.cpp b/src/landmark_apriltag.cpp index 1ff8dfedc8d3bb94247f60af6ff805844d69da00..49373a3109531b9f12ce5a0dfd1907048fc02228 100644 --- a/src/landmark_apriltag.cpp +++ b/src/landmark_apriltag.cpp @@ -47,7 +47,7 @@ LandmarkBasePtr LandmarkApriltag::create(const YAML::Node& _lmk_node) { // we have been given 3 Euler angles in degrees Eigen::Vector3s euler = M_TORAD * ( _lmk_node["orientation"] .as<Eigen::Vector3s>() ); - Eigen::Matrix3s R = e2R(euler(0), euler(1), euler(2)); + Eigen::Matrix3s R = e2R(euler); Eigen::Quaternions quat = R2q(R); vquat = quat.coeffs(); } diff --git a/src/rotations.h b/src/rotations.h index e6c43ef529ae7cff606f37b37f994a9f8d48fee7..2ce22553796f83d6f3cdfbe497f57f5dab3ae326 100644 --- a/src/rotations.h +++ b/src/rotations.h @@ -162,51 +162,17 @@ e2q(const Eigen::MatrixBase<D>& _euler) } - -/** \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) +e2R(const Eigen::MatrixBase<Derived>& _e) { - return e2R(_e(0), _e(1), _e(2)); + MatrixSizeCheck<3, 1>::check(_e); + + return e2q(_e).matrix(); } @@ -226,9 +192,9 @@ R2e(const Eigen::MatrixBase<Derived>& _R) { Eigen::Matrix<typename Derived::Scalar, 3, 1> e; - e(0) = atan2(_R(2,1),_R(2,2)); + 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)); + e(2) = atan2( _R(1,0), _R(0,0)); return e; } @@ -241,26 +207,26 @@ 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 w = _q(3); + Scalar x = _q(0); + Scalar y = _q(1); + Scalar z = _q(2); - 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 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); + 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)); @@ -685,6 +651,7 @@ diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) return minus(q1, q2); } + } // namespace wolf #endif /* ROTATIONS_H_ */ diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 4f41f23f703ee28fbb523fffe547ab8a7c58015d..899b11eb0f2f4073b6a67e6fdc213d9a5a5fb445 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 07b1272d54a93d51a89eee80833f2e79ac1f8bce..7305ddf108cb801b0d6b6f31aada905686c44b2d 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,28 +673,86 @@ TEST(log_q, small) } } +//<<<<<<< HEAD +//======= +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); + +} + +//>>>>>>> master TEST(Conversions, e2q_q2R_R2e) { Vector3s e, eo; Quaternions q; Matrix3s R; - e.setRandom(); +//<<<<<<< HEAD +// e.setRandom(); +//======= +//>>>>>>> master 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); +//<<<<<<< HEAD +// 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); +// +//======= + ASSERT_MATRIX_APPROX(eo, e, 1e-10); +} - WOLF_TRACE("euler o ", eo.transpose()); +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); - +//>>>>>>> master } TEST(Conversions, e2R_R2q_q2e) @@ -713,22 +761,31 @@ TEST(Conversions, e2R_R2q_q2e) Quaternions q; Matrix3s R; - e.setRandom(); +//<<<<<<< HEAD +// e.setRandom(); +// e << 0.1, 0.2, 0.3; +// R = e2R(e(0), e(1), e(2)); +//======= e << 0.1, 0.2, 0.3; - R = e2R(e(0), e(1), e(2)); + R = e2R(e); +//>>>>>>> master 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()); - - +//<<<<<<< HEAD +// 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); +// +//======= ASSERT_MATRIX_APPROX(eo, e, 1e-10); - +//>>>>>>> master }