diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 85aff3e6a4a533a2303f5e1a6c45dd484cffc8d1..04eae1ca305801665b5e3fcea22610da7f06026e 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -165,6 +165,7 @@ TEST(Test, Foo)
                C_expect, C_actual);
 
 #define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+                   assert(lhs.size() == 3 and rhs.size() == 3); \
                    Eigen::VectorXd er = lhs - rhs; \
                    er(2) = pi2pi((double)er(2)); \
                    return er.isMuchSmallerThan(1, precision); \
@@ -172,7 +173,7 @@ TEST(Test, Foo)
                C_expect, C_actual);
 
 #define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
-                   if (lhs.size() != 3 or rhs.size() != 3){ return false;} \
+                   assert(lhs.size() == 3 and rhs.size() == 3); \
                    Eigen::VectorXd er = lhs - rhs; \
                    er(2) = pi2pi((double)er(2)); \
                    return er.isMuchSmallerThan(1, precision); \