diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 77c1621bf..1e7960b56 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -146,30 +146,6 @@ TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. and we also define a limited number of `gtsam::tags` to select the correct implementation of certain functions at compile time (tag dispatching). Charts are done more conventionally, so we start there... -Interfaces ----------- - -Because Charts are always written by the user (or automatically generated, see below for vector spaces), -we enforce the Chart concept using an abstract base class, acting as an interface: - -``` -#!c++ -template -struct Chart { - typedef T ManifoldType; - typedef typename traits::TangentVector::type TangentVector; - static TangentVector Local(const ManifoldType& p, const ManifoldType& q) {return Derived::local(p,q);} - static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) {return Derived::retract(p,v);} - protected: - Chart(){ (void)&Local; (void)&Retract; } // enforce early instantiation. -} -``` - -The [CRTP](http://en.wikipedia.org/wiki/Curiously_recurring_template_pattern) and the protected constructor -automatically check for the existence of the methods in the Derived class, whenever a new Chart is created by - - struct MyChart : Chart { ... } - Traits ------ diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index a308c50a1..83c2fa4e8 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -50,17 +51,20 @@ namespace gtsam { * @tparam T is the type this constrains to be testable - assumes print() and equals() */ template - class TestableConcept { - static bool checkTestableConcept(const T& d) { + class Testable { + T t; + bool r1,r2; + public: + + BOOST_CONCEPT_USAGE(Testable) { // check print function, with optional string - d.print(std::string()); - d.print(); + t.print(std::string()); + t.print(); // check print, with optional threshold double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; + r1 = t.equals(t, tol); + r2 = t.equals(t); } }; @@ -129,6 +133,7 @@ namespace gtsam { * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable + * @deprecated please use BOOST_CONCEPT_ASSERT and */ -#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; +#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::Testable; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::Testable _gtsam_Testable_##T; diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index dc4fca036..3f1222cec 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -11,7 +11,9 @@ //#include "manifold.h" //#include "chart.h" #include +#include #include +#include #include #include @@ -24,9 +26,7 @@ namespace traits { * @brief Associate a unique tag with each of the main GTSAM concepts */ //@{ -template -struct structure_category; -// specializations should be derived from one of the following tags +template struct structure_category; //@} /** @@ -34,81 +34,98 @@ struct structure_category; * @brief Possible values for traits::structure_category::type */ //@{ -struct manifold_tag { -}; -struct group_tag { -}; -struct lie_group_tag: public manifold_tag, public group_tag { -}; -struct vector_space_tag: public lie_group_tag { -}; +struct manifold_tag {}; +struct group_tag {}; +struct lie_group_tag: public manifold_tag, public group_tag {}; +struct vector_space_tag: public lie_group_tag {}; //@} }// namespace traits +namespace manifold { + +/** @name Free functions any Manifold needs to define */ +//@{ +//@} + namespace traits { /** @name Manifold Traits */ //@{ -template struct TangentVector; -template struct DefaultChart; +template struct dimension; +template struct TangentVector; +template struct DefaultChart; //@} -}// namespace traits +}// \ namespace traits -/* - template - class ManifoldConcept { - public: - typedef T Manifold; - typedef typename traits::TangentVector::type TangentVector; - typedef typename traits::DefaultChart::type DefaultChart; - static const size_t dim = traits::dimension::value; +/// Check invariants for Manifold type +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_invariants(const T& a, const T& b) { + typedef typename traits::DefaultChart::type Chart; + return true; +} - BOOST_CONCEPT_USAGE(ManifoldConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - // no direct usage for manifold since most usage is through a chart - } - private: - Manifold p; - TangentVector v; - }; +} // \ namespace manifold - template - class ChartConcept { - public: - typedef C Chart; - typedef typename traits::Manifold::type Manifold; - typedef typename traits::TangentVector::type TangentVector; - - BOOST_CONCEPT_USAGE(ChartConcept) { - v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p - q = Chart::retract(p,v); // returns retracted update of p with v - } - - private: - Manifold p,q; - TangentVector v; - - }; +/** + * Chart concept */ +template +class IsChart { +public: + typedef typename T::ManifoldType ManifoldType; + typedef typename manifold::traits::TangentVector::type V; + + BOOST_CONCEPT_USAGE(IsChart) { + // make sure Derived methods in Chart are defined + v = T::Local(p,q); + q = T::Retract(p,v); + } +private: + ManifoldType p,q; + V v; +}; + +/** + * Manifold concept + */ +template +class IsManifold { +public: + typedef typename traits::structure_category::type structure_category_tag; + static const size_t dim = manifold::traits::dimension::value; + typedef typename manifold::traits::TangentVector::type TangentVector; + typedef typename manifold::traits::DefaultChart::type DefaultChart; + + BOOST_CONCEPT_USAGE(IsManifold) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + BOOST_CONCEPT_ASSERT((IsChart)); + } +private: + T p,q; + TangentVector v; +}; namespace group { /** @name Free functions any Group needs to define */ //@{ -template G compose(const G&g, const G& h); -template G between(const G&g, const G& h); -template G inverse(const G&g); +template T compose(const T&g, const T& h); +template T between(const T&g, const T& h); +template T inverse(const T&g); //@} namespace traits { /** @name Group Traits */ //@{ -template struct identity; -template struct flavor; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ @@ -119,94 +136,131 @@ struct multiplicative_tag { }; //@} -} // \ namespace traits +}// \ namespace traits + +/// Check invariants +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_invariants(const T& a, const T& b, double tol = 1e-9) { + T e = traits::identity::value; + return compose(a, inverse(a)).equals(e, tol) + && between(a, b).equals(compose(inverse(a), b), tol) + && compose(a, between(a, b)).equals(b, tol); +} } // \ namespace group /** * Group Concept */ -template -class Group { +template +class IsGroup { public: - typedef typename traits::structure_category::type structure_category_tag; - typedef typename group::traits::identity::value_type identity_value_type; - typedef typename group::traits::flavor::type flavor_tag; + typedef typename traits::structure_category::type structure_category_tag; + typedef typename group::traits::identity::value_type identity_value_type; + typedef typename group::traits::flavor::type flavor_tag; - BOOST_CONCEPT_USAGE(Group) { + void operator_usage(group::traits::multiplicative_tag) { + g = g * h; + } + void operator_usage(group::traits::additive_tag) { + g = g + h; + g = h - g; + g = -g; + } + + BOOST_CONCEPT_USAGE(IsGroup) { using group::compose; using group::between; using group::inverse; - BOOST_STATIC_ASSERT( - boost::is_base_of::value); - e = group::traits::identity::value; - d = compose(g, h); - d = between(g, h); - ig = inverse(g); - test = operator_usage(g, h, flavor); + 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 = group::traits::identity::value; + g = compose(g, h); + g = between(g, h); + g = inverse(g); + operator_usage(flavor); } -// TODO: these all require default constructors :-( -// Also, requires equal which is not required of a group -// Group():e(group::traits::identity::value) { -// } -// -// bool check_invariants(const G& a, const G& b) { -// return (equal(compose(a, inverse(a)), e)) -// && (equal(between(a, b), compose(inverse(a), b))) -// && (equal(compose(a, between(a, b)), b)) -// && operator_usage(a, b, flavor); -// } - private: flavor_tag flavor; - G e, g, h, gh, ig, d; - bool test, test2; - - bool operator_usage(const G& a, const G& b, - group::traits::multiplicative_tag) { -// return group::compose(a, b) == a * b; - return true; - } - bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { - return group::compose(a, b) == a + b; - } - + T e, g, h; }; -/* - template - class LieGroupConcept : public GroupConcept, public ManifoldConcept { +namespace lie_group { - BOOST_CONCEPT_USAGE(LieGroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - } - }; +/** @name Free functions any Group needs to define */ +//@{ +// TODO need Jacobians +//template T compose(const T&g, const T& h); +//template T between(const T&g, const T& h); +//template T inverse(const T&g); +//@} - template - class VectorSpaceConcept : public LieGroupConcept { - typedef typename traits::DefaultChart::type Chart; - typedef typename GroupConcept::identity identity; +namespace traits { - BOOST_CONCEPT_USAGE(VectorSpaceConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - r = p+q; - r = -p; - r = p-q; - } +/** @name Lie Group Traits */ +//@{ +//@} - bool check_invariants(const V& a, const V& b) { - return equal(compose(a, b), a+b) - && equal(inverse(a), -a) - && equal(between(a, b), b-a) - && equal(Chart::retract(a, b), a+b) - && equal(Chart::local(a, b), b-a); - } +}// \ namespace traits - private: - V g,q,r; - }; +/// Check invariants +//template +//BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) check_invariants(const T& a, +// const T& b) { +// bool check_invariants(const V& a, const V& b) { +// return equal(Chart::retract(a, b), a + b) +// && equal(Chart::local(a, b), b - a); +// } +//} +}// \ namespace lie_group + +/** + * Lie Group Concept */ +template +class IsLieGroup: public IsGroup, public IsManifold { +public: + + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(IsLieGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a Lie group (or derived)"); + // TODO Check with Jacobian +// using lie_group::compose; +// using lie_group::between; +// using lie_group::inverse; +// g = compose(g, h); +// g = between(g, h); +// g = inverse(g); + } +private: + + T g, h; +}; + +template +class IsVectorSpace: public IsLieGroup { +public: + + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(IsVectorSpace) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a vector space (or derived)"); + r = p + q; + r = -p; + r = p - q; + } + +private: + T p, q, r; +}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 00ea2c853..3cfa6b2c8 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -26,7 +26,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((Group)); + BOOST_CONCEPT_ASSERT((IsGroup)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); G g(2), h(3); // EXPECT(Group().check_invariants(g,h)) @@ -81,6 +81,12 @@ TEST(Cyclic, Ivnverse) { EXPECT_LONGS_EQUAL(1, group::inverse(G(5))); } +//****************************************************************************** +TEST(Cyclic , Invariants) { + G g(2), h(5); + group::check_invariants(g,h); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index a35576259..89dcd3024 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -19,47 +19,134 @@ namespace gtsam { -/// Typedef to an Eigen Quaternion, we disable alignment because -/// geometry objects are stored in boost pool allocators, in Values -/// containers, and and these pool allocators do not support alignment. -typedef Eigen::Quaternion Quaternion; +namespace traits { + +/// Define Eigen::Quaternion to be a model of the Lie Group concept +template +struct structure_category > { + typedef lie_group_tag type; +}; + +} // \namespace gtsam::traits + +namespace manifold { + +/// Chart for Eigen Quaternions +template +struct QuaternionChart { + + // required + typedef Eigen::Quaternion ManifoldType; + + // internal + typedef ManifoldType Q; + typedef typename traits::TangentVector::type Omega; + + /// Exponential map, simply be converting omega to AngleAxis + static Q Expmap(const Omega& omega) { + double theta = omega.norm(); + if (std::abs(theta) < 1e-10) + return Q::Identity(); + return Q(Eigen::AngleAxisd(theta, omega / theta)); + } + + /// retract, simply be converting omega to AngleAxis + static Q Retract(const Q& p, const Omega& omega) { + return p * Expmap(omega); + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static Omega Logmap(const Q& q) { + using std::acos; + using std::sqrt; + static const double twoPi = 2.0 * M_PI, + // define these compile time constants to avoid std::abs: + NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + + const double qw = q.w(); + if (qw > NearlyOne) { + // Taylor expansion of (angle / s) at 1 + return (2 - 2 * (qw - 1) / 3) * q.vec(); + } else if (qw < NearlyNegativeOne) { + // Angle is zero, return zero vector + return Vector3::Zero(); + } else { + // Normal, away from zero case + double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + return (angle / s) * q.vec(); + } + } + + /// local is our own, as there is a slight bug in Eigen + static Omega Local(const Q& q1, const Q& q2) { + return Logmap(q1.inverse() * q2); + } +}; namespace traits { -/// Define Quaternion to be a model of the Group concept -template<> -struct structure_category { - typedef group_tag type; + +/// Define the trait that asserts Quaternion manifold has dimension 3 +template +struct dimension > : public boost::integral_constant< + int, 3> { }; -} // \namespace gtsam::traits + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct TangentVector > { + typedef Eigen::Matrix type; +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct DefaultChart > { + typedef QuaternionChart type; +}; + +} // \namespace gtsam::manifold::traits +} // \namespace gtsam::manifold namespace group { -Quaternion compose(const Quaternion&g, const Quaternion& h) { +template +Eigen::Quaternion compose(const Eigen::Quaternion &g, + const Eigen::Quaternion & h) { return g * h; } -Quaternion between(const Quaternion&g, const Quaternion& h) { +template +Eigen::Quaternion between(const Eigen::Quaternion &g, + const Eigen::Quaternion & h) { return g.inverse() * h; } -Quaternion inverse(const Quaternion&g) { +template +Eigen::Quaternion inverse(const Eigen::Quaternion &g) { return g.inverse(); } namespace traits { -/// Define the trait that specifies Quaternion's identity element -template<> -struct identity { - static const Quaternion value; - typedef Quaternion value_type; +/// Declare the trait that specifies a quaternion's identity element +template +struct identity > { + static const Eigen::Quaternion value; + typedef Eigen::Quaternion value_type; }; -const Quaternion identity::value = Quaternion(0); +/// Out of line definition of identity +template +const Eigen::Quaternion identity >::value = + Eigen::Quaternion::Identity(); -/// Define the trait that asserts Quaternion is an additive group -template<> -struct flavor { +/// Define the trait that asserts quaternions are a multiplicative group +template +struct flavor > { typedef multiplicative_tag type; }; @@ -67,6 +154,13 @@ struct flavor { } // \namespace gtsam::group } // \namespace gtsam +/** + * GSAM typedef to an Eigen::Quaternion, we disable alignment because + * geometry objects are stored in boost pool allocators, in Values containers, + * and and these pool allocators do not support alignment. + */ +typedef Eigen::Quaternion Quaternion; + /** * @file testCyclic.cpp * @brief Unit tests for cyclic group @@ -75,6 +169,7 @@ struct flavor { //#include #include +#include #include using namespace std; @@ -83,25 +178,56 @@ using namespace gtsam; typedef Quaternion Q; // Typedef //****************************************************************************** -TEST(Quaternion, Concept) { - BOOST_CONCEPT_ASSERT((Group)); +TEST(Quaternion , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** -TEST(Quaternion, Constructor) { - Q g(0); +TEST(Quaternion , Constructor) { + Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** -TEST(Quaternion, Compose) { +TEST(Quaternion , Invariants) { + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + // group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!) } //****************************************************************************** -TEST(Quaternion, Between) { +TEST(Quaternion , Local) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 expected(0, 0, 0.1); + Vector3 actual = Chart::Local(q1, q2); + EXPECT(assert_equal((Vector)expected,actual)); } //****************************************************************************** -TEST(Quaternion, Ivnverse) { +TEST(Quaternion , Retract) { + Vector3 z_axis(0, 0, 1); + Q q(Eigen::AngleAxisd(0, z_axis)); + Q expected(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 v(0, 0, 0.1); + Q actual = Chart::Retract(q, v); + EXPECT(actual.isApprox(expected)); +} + +//****************************************************************************** +TEST(Quaternion , Compose) { +} + +//****************************************************************************** +TEST(Quaternion , Between) { +} + +//****************************************************************************** +TEST(Quaternion , Inverse) { } //******************************************************************************