Skip to content
Snippets Groups Projects
Commit ea6da716 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add a test for issue 271 regarding quat norm

parent 6e4f94dd
No related branches found
No related tags found
1 merge request!333Resolve "r2q function returning a non unitary qutarnion when passed a Eigen::Isometry3d::linear() object"
Pipeline #4810 passed
......@@ -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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment