From 783f7a3279b453dcf19a64768186fc403267935b Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 7 Oct 2022 16:43:43 +0200
Subject: [PATCH] fixed quaternion asserts for gtest
---
include/core/utils/utils_gtest.h | 12 ++++++------
test/gtest_SE3.cpp | 12 ------------
2 files changed, 6 insertions(+), 18 deletions(-)
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 905f25c8b..9c46dd20c 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 a87610379..614d6f7f4 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)
--
GitLab