From 55b94ffb2fca46ee9dc8924612abeff3727a1135 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Tue, 24 Oct 2017 17:05:19 +0200
Subject: [PATCH] add eigen predicates gtests

---
 src/test/CMakeLists.txt             |   4 +
 src/test/gtest_eigen_predicates.cpp | 178 ++++++++++++++++++++++++++++
 2 files changed, 182 insertions(+)
 create mode 100644 src/test/gtest_eigen_predicates.cpp

diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 6200ebc3a..d2e015c98 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -48,6 +48,10 @@ target_link_libraries(gtest_capture_base ${PROJECT_NAME})
 #wolf_add_gtest(gtest_constraint_sparse gtest_constraint_sparse.cpp)
 #target_link_libraries(gtest_constraint_sparse ${PROJECT_NAME})
 
+# FeatureBase classes test
+wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
+target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
+
 # FeatureBase classes test
 wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
 target_link_libraries(gtest_feature_base ${PROJECT_NAME})
diff --git a/src/test/gtest_eigen_predicates.cpp b/src/test/gtest_eigen_predicates.cpp
new file mode 100644
index 000000000..6f32d00e4
--- /dev/null
+++ b/src/test/gtest_eigen_predicates.cpp
@@ -0,0 +1,178 @@
+#include "utils_gtest.h"
+
+#include "../eigen_predicates.h"
+
+TEST(TestEigenPredicates, TestEigenDynPredZero)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::MatrixXs::Zero(4,4);
+  B = Eigen::MatrixXs::Random(4,4);
+  C = Eigen::MatrixXs::Ones(4,4) * (wolf::Constants::EPS/2.);
+
+  EXPECT_TRUE(wolf::pred_zero(A,  wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
+  EXPECT_TRUE(wolf::pred_zero(C,  wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenFixPredZero)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::Matrix4s::Zero();
+  B = Eigen::Matrix4s::Random();
+  C = Eigen::Matrix4s::Ones() * (wolf::Constants::EPS/2.);
+
+  EXPECT_TRUE(wolf::pred_zero(A,  wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
+  EXPECT_TRUE(wolf::pred_zero(C,  wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::MatrixXs::Random(4,4);
+  B = Eigen::MatrixXs::Random(4,4);
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::Matrix4s::Random();
+  B = Eigen::Matrix4s::Random();
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::MatrixXs::Random(4,4);
+  B = Eigen::MatrixXs::Random(4,4);
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::Matrix4s::Random();
+  B = Eigen::Matrix4s::Random();
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::MatrixXs::Random(4,4);
+  B = Eigen::MatrixXs::Random(4,4);
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
+{
+  Eigen::MatrixXs A, B, C;
+
+  A = Eigen::Matrix4s::Random();
+  B = Eigen::Matrix4s::Random();
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
+{
+  Eigen::Quaternions A, B, C;
+
+  /// @todo which version of Eigen provides this ?
+//  A = Eigen::Quaternions::UnitRandom();
+
+  A.coeffs() = Eigen::Vector4s::Random().normalized();
+  B.coeffs() = Eigen::Vector4s::Random().normalized();
+  C = A;
+
+  EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_quat_is_approx(A, B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
+{
+  Eigen::Quaternions A, B;
+
+  A = Eigen::Quaternions::Identity();
+  B.coeffs() = Eigen::Vector4s::Random().normalized();
+
+  EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
+{
+  wolf::Scalar a = M_PI;
+  wolf::Scalar b = -M_PI;
+  wolf::Scalar c = 0;
+
+  EXPECT_TRUE(wolf::pred_angle_is_approx(a, b, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_angle_is_approx(a, c, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+TEST(TestEigenPredicates, TestEigenPredAngleIsZero)
+{
+  wolf::Scalar a = 0;
+  wolf::Scalar b = M_PI;
+  wolf::Scalar c = 2.*M_PI;
+
+  EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS));
+  EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS));
+  EXPECT_TRUE(wolf::pred_angle_zero(c, wolf::Constants::EPS));
+
+  PRINT_TEST_FINISHED;
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
-- 
GitLab