diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 905f25c8b26a3fa1caa79cb6e72be4f6d07cfcc1..9c46dd20c96535308e5f851e218641498787a631 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -135,15 +135,15 @@ TEST(Test, Foo)
                }, \
                C_expect, C_actual);
 
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
-                   return lhs.angularDistance(rhs) < precision; \
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+                   return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
                }, \
-               C_expect, C_actual);
+               Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
 
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
-                   return lhs.angularDistance(rhs) < precision; \
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+                   return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
                }, \
-               C_expect, C_actual);
+               Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
 
 #define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
                    return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index a876103795f7afe0401c4a5db48290dd95c3775c..614d6f7f4cc04430d4a595209a94039bbbacd5cb 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -19,19 +19,10 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_SE3.cpp
- *
- *  Created on: Feb 2, 2019
- *      \author: jsola
- */
-
 
 #include "core/math/SE3.h"
 #include "core/utils/utils_gtest.h"
 
-
-
 using namespace Eigen;
 using namespace wolf;
 using namespace SE3;
@@ -150,7 +141,6 @@ TEST(SE3, inverseComposite)
     VectorComposite pose_vc_out_bis = inverse(pose_vc);
     ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8);
     ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8);
-
 }
 
 TEST(SE3, composeBlocks)
@@ -194,7 +184,6 @@ TEST(SE3, composeEigenVectors)
 
     compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
 
-
     Vector7d x1; x1 << p1, q1.coeffs();
     Vector7d x2; x2 << p2, q2.coeffs();
     Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
@@ -287,7 +276,6 @@ TEST(SE3, exp_0_Composite)
 
     ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
     ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
-
 }
 
 TEST(SE3, plus_0_Composite)