From daf02ba762c5469ba07165482f31d126a8442b08 Mon Sep 17 00:00:00 2001 From: Marshall Vielmetti Date: Sun, 26 Jan 2025 16:28:44 -0600 Subject: [PATCH] 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()