Resolve "r2q function returning a non unitary qutarnion when passed a Eigen::Isometry3d::linear() object"

Closes #271 (closed)

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();
 }