diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 4fa1108c9..68752bdff 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -66,7 +66,7 @@ struct Manifold { void Print(const ManifoldType& m) { m.print(); } - void Equals(const ManifoldType& m1, + bool Equals(const ManifoldType& m1, const ManifoldType& m2, double tol = 1e-8) { return m1.equals(m2, tol); @@ -116,7 +116,7 @@ struct LieGroup { void Print(const ManifoldType& m) { m.print(); } - void Equals(const ManifoldType& m1, + bool Equals(const ManifoldType& m1, const ManifoldType& m2, double tol = 1e-8) { return m1.equals(m2, tol); @@ -201,6 +201,7 @@ struct LieGroup { static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) { return ManifoldType::Expmap(v, Hv); } + }; } // namespace internal @@ -256,11 +257,17 @@ public: // and the versions with Jacobians. v = traits_x::Local(p,q,Hp,Hq); q = traits_x::Retract(p,v,Hp,Hv); + + traits_x::Print(p); + traits_x::Print(p, "p"); + b = traits_x::Equals(p,q); + b = traits_x::Equals(p,q,1e-9); } private: ManifoldType p,q; ChartJacobian Hp,Hq,Hv; TangentVector v; + bool b; }; /** @@ -277,12 +284,17 @@ public: BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::value), "This type's structure_category trait does not assert it as a group (or derived)"); - e = traits_x::identity; + e = traits_x::Identity(); e = traits_x::Compose(g, h); e = traits_x::Between(g, h); e = traits_x::Inverse(g); operator_usage(flavor); // todo: how do we test the act concept? or do we even need to? + + traits_x::Print(g); + traits_x::Print(g, "g"); + b = traits_x::Equals(g,h); + b = traits_x::Equals(g,h,1e-9); } private: @@ -298,14 +310,15 @@ private: flavor_tag flavor; G e, g, h; + bool b; }; /// Check invariants template BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // check_group_invariants(const G& a, const G& b, double tol = 1e-9) { - G e = traits_x::identity; - return traits_x::Equals(traits_x::Compose(a, traits_x::inverse(a)), e, tol) + G e = traits_x::Identity(); + return traits_x::Equals(traits_x::Compose(a, traits_x::Inverse(a)), e, tol) && traits_x::Equals(traits_x::Between(a, b), traits_x::Compose(traits_x::Inverse(a), b), tol) && traits_x::Equals(traits_x::Compose(a, traits_x::Between(a, b)), b, tol); } diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 8e1989d53..9ee25349d 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -111,12 +111,12 @@ Matrix numericalDerivative11(boost::function h, const X& x, double delta = 1e-5) { using namespace traits; - BOOST_STATIC_ASSERT_MSG( (typename boost::is_same< typename traits_x::structure_category, gtsam::manifold_tag>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); typedef DefaultChart ChartY; typedef typename ChartY::vector TangentY; - BOOST_STATIC_ASSERT_MSG( (typename boost::is_same::structure_category, gtsam::manifold_tag>::value), + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); static const int N = traits::dimension::value; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 4c226e166..886928ad8 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -70,7 +70,9 @@ template struct traits_x { typedef group_tag structure_category; GTSAM_ADDITIVE_GROUP(CYCLIC_TYPE); - static const CYCLIC_TYPE identity = CYCLIC_TYPE::Identity(); + static CYCLIC_TYPE Identity() { return CYCLIC_TYPE::Identity(); } + static bool Equals(const CYCLIC_TYPE& a, const CYCLIC_TYPE& b, double tol=1e-9) { return a.equals(b,tol); } + static void Print(const CYCLIC_TYPE& c, const std::string &s="") { c.print(s); } }; } // \namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 41cadc7d9..c0d05bf6a 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -29,8 +29,11 @@ struct traits_x { typedef QUATERNION_TYPE ManifoldType; typedef QUATERNION_TYPE Q; typedef lie_group_tag structure_category; + typedef multiplicative_group_tag group_flavor; - static const Q identity = Q::Identity(); + enum { dimension = 3 }; + + static Q Identity() { return Q::Identity(); } // Define manifold traits typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f83404e74..8a7a5f5e5 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -28,7 +29,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3{ +class GTSAM_EXPORT Unit3 { private: @@ -37,6 +38,8 @@ private: public: + enum { dimension = 2 }; + /// @name Constructors /// @{ @@ -160,24 +163,14 @@ private: }; // Define GTSAM traits -namespace traits { +template <> struct traits_x : public internal::Manifold {}; -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Unit3 value() { - return Unit3(); - } -}; - -} +//template<> +//struct GTSAM_EXPORT zero { +// static Unit3 value() { +// return Unit3(); +// } +//}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 1b2f5bd7e..6faeb0c92 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -27,7 +27,7 @@ typedef Cyclic<3> G; // Let's use the cyclic group of order 3 //****************************************************************************** TEST(Cyclic, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); - EXPECT_LONGS_EQUAL(0, group::traits::identity::value); + EXPECT_LONGS_EQUAL(0, traits_x::Identity()); } //****************************************************************************** @@ -37,37 +37,37 @@ TEST(Cyclic, Constructor) { //****************************************************************************** TEST(Cyclic, Compose) { - EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits_x::Compose(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, traits_x::Compose(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, traits_x::Compose(G(0),G(2))); - EXPECT_LONGS_EQUAL(2, group::compose(G(2),G(0))); - EXPECT_LONGS_EQUAL(0, group::compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(2, traits_x::Compose(G(2),G(0))); + EXPECT_LONGS_EQUAL(0, traits_x::Compose(G(2),G(1))); + EXPECT_LONGS_EQUAL(1, traits_x::Compose(G(2),G(2))); } //****************************************************************************** TEST(Cyclic, Between) { - EXPECT_LONGS_EQUAL(0, group::between(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, group::between(G(0),G(1))); - EXPECT_LONGS_EQUAL(2, group::between(G(0),G(2))); + EXPECT_LONGS_EQUAL(0, traits_x::Between(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, traits_x::Between(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, traits_x::Between(G(0),G(2))); - EXPECT_LONGS_EQUAL(1, group::between(G(2),G(0))); - EXPECT_LONGS_EQUAL(2, group::between(G(2),G(1))); - EXPECT_LONGS_EQUAL(0, group::between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, traits_x::Between(G(2),G(0))); + EXPECT_LONGS_EQUAL(2, traits_x::Between(G(2),G(1))); + EXPECT_LONGS_EQUAL(0, traits_x::Between(G(2),G(2))); } //****************************************************************************** TEST(Cyclic, Ivnverse) { - EXPECT_LONGS_EQUAL(0, group::inverse(G(0))); - EXPECT_LONGS_EQUAL(2, group::inverse(G(1))); - EXPECT_LONGS_EQUAL(1, group::inverse(G(2))); + EXPECT_LONGS_EQUAL(0, traits_x::Inverse(G(0))); + EXPECT_LONGS_EQUAL(2, traits_x::Inverse(G(1))); + EXPECT_LONGS_EQUAL(1, traits_x::Inverse(G(2))); } //****************************************************************************** TEST(Cyclic , Invariants) { G g(2), h(5); -// group::check_invariants(g,h); + check_group_invariants(g,h); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index c83e7a16f..3d96e342e 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -22,7 +22,7 @@ using namespace std; using namespace gtsam; typedef Quaternion Q; // Typedef -typedef traits::ChartJacobian QuaternionJacobian; +typedef traits_x::ChartJacobian QuaternionJacobian; //****************************************************************************** TEST(Quaternion , Concept) { diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 2a2e3b40f..9c99628e6 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9));