diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp index d370d52874b269d0828b34524a10947694806f52..a8a0efd094462347f379bf4507d8dfa38e591038 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(); }