Skip to content
Snippets Groups Projects
Commit d16b5148 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

remove eigen_predicates things

parent bea7d5bc
No related branches found
No related tags found
2 merge requests!440New tag,!437Resolve "Strange things in WOLF core?"
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 0
− 1
View file @ d16b5148
......@@ -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
......
include/core/utils/eigen_predicates.h deleted 100644 → 0
+ 0
− 121
View file @ bea7d5bc
//--------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_ */
test/CMakeLists.txt
+ 0
− 4
View file @ d16b5148
......@@ -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)
......
test/gtest_eigen_predicates.cpp deleted 100644 → 0
+ 0
− 199
View file @ bea7d5bc
//--------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();
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment