From 151b8608763a4f3f3e32702bbb5a99aaa7c7a856 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Tue, 25 Dec 2018 02:52:48 +0100 Subject: [PATCH] Fix e2R API and one test in gtest_rotations --- src/rotations.h | 4 +- src/test/CMakeLists.txt | 2 +- src/test/gtest_rotation.cpp | 109 +++++++++++++++++++++++------------- 3 files changed, 75 insertions(+), 40 deletions(-) diff --git a/src/rotations.h b/src/rotations.h index 2b655dbbe..a564e57f6 100644 --- a/src/rotations.h +++ b/src/rotations.h @@ -204,8 +204,10 @@ e2R(const T roll, */ template<typename Derived> inline Eigen::Matrix<typename Derived::Scalar, 3, 3> -e2R(Eigen::MatrixBase<Derived> _e) +e2R(const Eigen::MatrixBase<Derived>& _e) { + MatrixSizeCheck<3, 1>::check(_e); + return e2R(_e(0), _e(1), _e(2)); } diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 4827de183..bac392115 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 07b1272d5..aa129e9d3 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,80 @@ 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(0), e(1), e(2)); + + eo = q2e(q); + ASSERT_MATRIX_APPROX(eo, e, 1e-10); + + eo = q2e(q.coeffs()); + ASSERT_MATRIX_APPROX(eo, e, 1e-10); + + + 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.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); + 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(0), e(1), e(2)); + eo = R2e(R); ASSERT_MATRIX_APPROX(eo, e, 1e-10); + R = e2R(e); + eo = R2e(R); + ASSERT_MATRIX_APPROX(eo, e, 1e-10); } TEST(Conversions, e2R_R2q_q2e) @@ -713,22 +755,13 @@ TEST(Conversions, e2R_R2q_q2e) 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); - } -- GitLab