diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 6200ebc3af36d86dea01c639f926007a20728982..d2e015c98754b08e3242868ba1ffce5f6c14250d 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 0000000000000000000000000000000000000000..6f32d00e438f71e98b3785f4471ac850f5e0ba19 --- /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(); +}