diff --git a/CMakeLists.txt b/CMakeLists.txt
index 176bdf1a2db64c9aac6da0dfb421d10c65fb8ba3..d6b6bc62bbddc3b731885696b9d9c53766630037 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -249,7 +249,6 @@ SET(HDRS_UTILS
include/core/utils/check_log.h
include/core/utils/converter.h
include/core/utils/eigen_assert.h
- include/core/utils/eigen_predicates.h
include/core/utils/graph_search.h
include/core/utils/loader.h
include/core/utils/logging.h
diff --git a/include/core/utils/eigen_predicates.h b/include/core/utils/eigen_predicates.h
deleted file mode 100644
index 3a83601e84d95eba96d1193c651c220eca0ebf64..0000000000000000000000000000000000000000
--- a/include/core/utils/eigen_predicates.h
+++ /dev/null
@@ -1,121 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file eigen_predicates.h
- * \brief Some utils for comparing Eigen types
- * \author Jeremie Deray
- * Created on: 24/10/2017
- */
-
-#ifndef _WOLF_EIGEN_PREDICATES_H_
-#define _WOLF_EIGEN_PREDICATES_H_
-
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-/// @brief check that each Matrix/Vector elem is approx equal to value +- precision
-///
-/// @note we overload this rather than using Eigen::Matrix::isConstant() since it is
-/// defined as :
-/// abs(x - y) <= (min)(abs(x), abs(y)) * prec
-/// which does not play nice with y = 0
-auto is_constant = [](const Eigen::MatrixXd& lhs, const double val, const double precision)
- {
- for (Eigen::MatrixXd::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXd::Index i = 0; i < lhs.rows(); ++i)
- if (std::abs(lhs.coeff(i, j) - val) > precision)
- return false;
- return true;
- };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXd& lhs, const double precision)
- { return is_constant(lhs, 0, precision); };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_diff_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return pred_zero(lhs - rhs, precision); };
-
-/// @brief check that the Matrix/Vector difference norm is approx 0 +- precision
-auto pred_diff_norm_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return std::abs((lhs - rhs).norm()) <= std::abs(precision); };
-
-/// @brief check that the lhs Matrix/Vector is elem-wise approx the rhs +-precision
-auto pred_is_approx = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return lhs.isApprox(rhs, precision); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to rhs
-/// is <= precision
-auto pred_quat_is_approx = [](const Eigen::Quaterniond& lhs, const Eigen::Quaterniond& rhs, const double precision)
- { return pred_zero( log_q(rhs * lhs.inverse()), precision ); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to identity
-/// is <= precision
-auto pred_quat_identity = [](const Eigen::Quaterniond& lhs, const double precision)
- { return pred_quat_is_approx(lhs, Eigen::Quaterniond::Identity(), precision); };
-
-/// @brief check that rotation angle to get from lhs angle to rhs is <= precision
-auto pred_angle_is_approx = [](const double lhs, const double rhs, const double precision)
- { return std::abs(pi2pi(lhs - rhs)) <= std::abs(precision); };
-
-/// @brief check that rotation angle to get from lhs angle to 0 is <= precision
-auto pred_angle_zero = [](const double lhs, const double precision)
- { return pred_angle_is_approx(lhs, 0, precision); };
-
-/// @brief check that the lhs pose is approx rhs +- precision
-///
-/// @note
-/// Comparison is :
-/// d = inv(lhs) * rhs
-/// d.translation().elem_wise() ~ 0 (+- precision)
-///
-/// if pose 3d :
-/// d.rotation_as_quaternion() ~ quaternion.getIdentity (+- precision)
-///
-/// else if pose 2d:
-/// d.rotation_angle() ~ 0 (+- precision)
-///
-/// else throw std::runtime
-///
-/// @see pred_zero for translation comparison
-/// @see pred_quat_identity for 3d rotation comparison
-/// @see pred_angle_zero for 2d rotation comparison
-//auto pred_pose_is_approx = [](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs, const double precision)
-// {
-// const Eigen::MatrixXd d = lhs.inverse() * rhs;
-// const bool tok = pred_zero(d.rightCols(1), precision);
-
-// const bool qok = (lhs.rows() == 3)?
-// pred_quat_identity(Eigen::Quaterniond(d.block(0,0,3,1)),
-// precision)
-// : (lhs.rows() == 2)? pred_angle_zero(getYaw(d), precision)
-// : throw std::runtime_error("Canno't compare pose in Dim > 3 !");
-
-// return tok && qok;
-// };
-
-} // namespace wolf
-
-#endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a3da6a740948958190273e4526f997a6872a0cb1..29fc45531a4b0de27c1c227883260b71913c96ab 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -68,10 +68,6 @@ target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
-# FeatureBase classes test
-wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
-
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
deleted file mode 100644
index 6dc7184ec5d1d39db31a2e26d06964023c26e65f..0000000000000000000000000000000000000000
--- a/test/gtest_eigen_predicates.cpp
+++ /dev/null
@@ -1,199 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "core/utils/utils_gtest.h"
-
-#include "core/utils/eigen_predicates.h"
-
-TEST(TestEigenPredicates, TestEigenDynPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Zero(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = Eigen::MatrixXd::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::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Zero();
- B = Eigen::Matrix4d::Random();
- C = Eigen::Matrix4d::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::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::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::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::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::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::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::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::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::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::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::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::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::Quaterniond A, B, C;
-
- /// @todo which version of Eigen provides this ?
-// A = Eigen::Quaterniond::UnitRandom();
-
- A.coeffs() = Eigen::Vector4d::Random().normalized();
- B.coeffs() = Eigen::Vector4d::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::Quaterniond A, B;
-
- A = Eigen::Quaterniond::Identity();
- B.coeffs() = Eigen::Vector4d::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)
-{
- double a = M_PI;
- double b = -M_PI;
- double 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)
-{
- double a = 0;
- double b = M_PI;
- double 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();
-}