From 2abfc17ee4c4d9ca2a3fdec90cfb1bc6b32b019f Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Fri, 24 Jan 2025 13:33:21 -0600 Subject: [PATCH 1/2] neq and tests --- include/manif/impl/lie_group_base.h | 14 ++++++++++++++ test/se2/gtest_se2.cpp | 8 ++++++++ test/se3/gtest_se3.cpp | 11 +++++++++++ test/so2/gtest_so2.cpp | 8 ++++++++ test/so3/gtest_so3.cpp | 8 ++++++++ 5 files changed, 49 insertions(+) diff --git a/include/manif/impl/lie_group_base.h b/include/manif/impl/lie_group_base.h index 945669a2..95cd1091 100644 --- a/include/manif/impl/lie_group_base.h +++ b/include/manif/impl/lie_group_base.h @@ -278,6 +278,20 @@ struct LieGroupBase template bool operator ==(const LieGroupBase<_DerivedOther>& m) const; + + /** + * @brief Inequality operator. + * @param[in] An element of the same Lie group. + * @return false if the Lie group element m is 'close' to this, + * true otherwise. + * @see operator==. + */ + template + bool operator!=( + const LieGroupBase<_DerivedOther> &m) const { + return !(*this == m); + } + /** * @brief Right oplus operator. * @see rplus. diff --git a/test/se2/gtest_se2.cpp b/test/se2/gtest_se2.cpp index 0f3181e8..97e61204 100644 --- a/test/se2/gtest_se2.cpp +++ b/test/se2/gtest_se2.cpp @@ -550,6 +550,14 @@ TEST(TEST_SE2, TEST_SE2_NORMALIZE) ); } +TEST(TEST_SE2, TEST_SE2_NEQ) +{ + SE2d one(SE2d::DataType(0,0,1,0)); + SE2d two(4, 2, 1, 0); + + EXPECT_NE(one, two); +} + #endif MANIF_TEST(SE2d); diff --git a/test/se3/gtest_se3.cpp b/test/se3/gtest_se3.cpp index ebbbfde0..08f4a366 100644 --- a/test/se3/gtest_se3.cpp +++ b/test/se3/gtest_se3.cpp @@ -461,6 +461,17 @@ TEST(TEST_SE3, TEST_SE3_NORMALIZE) ); } +TEST(TEST_SE3, TEST_SE3_NEQ) +{ + SE3d one(SE3d::Translation(1,2,3), + Eigen::Quaterniond::Identity()); + + SE3d::DataType values; values << 0,0,0, 0,0,0,1; + SE3d two(values); + + EXPECT_NE(one, two); +} + #endif MANIF_TEST(SE3d); diff --git a/test/so2/gtest_so2.cpp b/test/so2/gtest_so2.cpp index 4f7295ff..34878793 100644 --- a/test/so2/gtest_so2.cpp +++ b/test/so2/gtest_so2.cpp @@ -576,6 +576,14 @@ TEST(TEST_SO2, TEST_SO2_NORMALIZE) ); } +TEST(TEST_SO2, TEST_SO2_NEQ) +{ + SO2d one(0, 1); + SO2d two = SO2d::Identity(); + + EXPECT_NE(one, two); +} + #endif MANIF_TEST(SO2d); diff --git a/test/so3/gtest_so3.cpp b/test/so3/gtest_so3.cpp index 09606f39..cb7b3de8 100644 --- a/test/so3/gtest_so3.cpp +++ b/test/so3/gtest_so3.cpp @@ -617,6 +617,14 @@ TEST(TEST_SO3, TEST_SO3_NORMALIZE) ); } +TEST(TEST_SO3, TEST_SO3_NOTEQ) +{ + SO3d one(0,0,0,1); + SO3d two(1,0,0,0); + + EXPECT_NE(one, two); +} + #endif MANIF_TEST(SO3d); From daf02ba762c5469ba07165482f31d126a8442b08 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Sun, 26 Jan 2025 16:28:44 -0600 Subject: [PATCH 2/2] added to tangent base and evalIsApprox --- include/manif/impl/tangent_base.h | 16 ++++++++++++++++ test/common_tester.h | 4 ++++ 2 files changed, 20 insertions(+) diff --git a/include/manif/impl/tangent_base.h b/include/manif/impl/tangent_base.h index 6d5178a4..56c37eb5 100644 --- a/include/manif/impl/tangent_base.h +++ b/include/manif/impl/tangent_base.h @@ -946,6 +946,22 @@ bool operator ==( return t.isApprox(v); } +template +bool operator !=( + const TangentBase<_Derived>& ta, + const TangentBase<_DerivedOther>& tb) +{ + return !(ta == tb); +} + +template +bool operator !=( + const TangentBase<_Derived>& t, + const Eigen::MatrixBase<_EigenDerived>& v) +{ + return !(t == v); +} + // Utils template diff --git a/test/common_tester.h b/test/common_tester.h index bc7ce126..592e8594 100644 --- a/test/common_tester.h +++ b/test/common_tester.h @@ -522,6 +522,7 @@ class CommonTester EXPECT_TRUE(LieGroup::Identity().isApprox(LieGroup::Identity(), tol_)); EXPECT_TRUE(LieGroup::Identity() == LieGroup::Identity()); + EXPECT_FALSE(LieGroup::Identity() != LieGroup::Identity()); EXPECT_TRUE(getState().isApprox(getState(), tol_)); EXPECT_FALSE(getState().isApprox(LieGroup::Random(), tol_)); @@ -529,12 +530,14 @@ class CommonTester // cppcheck-suppress duplicateExpression EXPECT_TRUE(getState() == getState()); EXPECT_FALSE(getState() == LieGroup::Random()); + EXPECT_TRUE(getState() != LieGroup::Random()); // Tangent EXPECT_TRUE(Tangent::Zero().isApprox(Tangent::Zero(), tol_)); EXPECT_TRUE(Tangent::Zero() == Tangent::Zero()); + EXPECT_FALSE(Tangent::Zero() != Tangent::Zero()); EXPECT_TRUE(getDelta().isApprox(getDelta(), tol_)); EXPECT_FALSE(getDelta().isApprox(Tangent::Random(), tol_)); @@ -542,6 +545,7 @@ class CommonTester // cppcheck-suppress duplicateExpression EXPECT_TRUE(getDelta() == getDelta()); EXPECT_FALSE(getDelta() == Tangent::Random()); + EXPECT_TRUE(getDelta() != Tangent::Random()); } void evalUnaryMinus()