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