From ea6da716df33785c04952184ea7fa0704cae00a3 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 30 Jan 2020 12:23:59 +0100
Subject: [PATCH] Add a test for issue 271 regarding quat norm
---
test/gtest_rotation.cpp | 25 +++++++++++++++++++++++++
1 file changed, 25 insertions(+)
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index d370d5287..a8a0efd09 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -687,6 +687,30 @@ TEST(Conversions, q2R_R2q)
ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
}
+TEST(Conversions, R2q_norm_of_q)
+{
+ Isometry3d T = Translation3d(1,2,3) * AngleAxisd(M_PI_4, Vector3d(1,2,3).normalized());
+ Eigen::Vector4d cqt = R2q(T.linear()).coeffs();
+ Eigen::Matrix3d R = T.linear();
+ Eigen::Vector4d cqt2 = R2q(R).coeffs();
+ std::cout << "cqt: " << cqt.transpose() << std::endl;
+ std::cout << "cqt2: " << cqt2.transpose() << std::endl;
+ std::cout << "cqt.norm(): " << cqt.norm() << std::endl;
+ std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl;
+
+ ASSERT_NEAR(cqt.norm(), 1, 1e-8);
+ ASSERT_NEAR(cqt2.norm(), 1, 1e-8);
+
+ // Theck T.linear() vs T.rotation()
+ T.linear().setRandom();
+
+ Matrix3d TL = T.linear();
+ Matrix3d TR = T.rotation();
+
+ WOLF_INFO("TL = ", TL);
+ WOLF_INFO("TR = ", TR);
+}
+
TEST(Conversions, e2q_q2e)
{
Vector3d e, eo;
@@ -768,5 +792,6 @@ int main(int argc, char **argv)
*/
testing::InitGoogleTest(&argc, argv);
+ ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
return RUN_ALL_TESTS();
}
--
GitLab