From 6648da20fe6056acb9a2506906b6b1a806c1d2fd Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 23 Dec 2014 14:53:15 +0100 Subject: [PATCH] Made clear template argument is a class, made explicit only fixed-size types handled for now --- gtsam/base/Lie.h | 103 ++++++++++++++++++++++------------------------- 1 file changed, 49 insertions(+), 54 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index b2b1486a9..03309722d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -32,38 +32,38 @@ namespace internal { /// A helper that implements the traits interface for GTSAM lie groups. /// To use this for your gtsam type, define: /// template<> struct traits : public LieGroup { }; -template -struct LieGroup : Testable { +template +struct LieGroup : Testable { typedef lie_group_tag structure_category; /// @name Group /// @{ typedef multiplicative_group_tag group_flavor; - static T Identity() { return T::identity();} - static T Compose(const T& m1, const T& m2) { return m1 * m2;} - static T Between(const T& m1, const T& m2) { return m1.inverse() * m2;} - static T Inverse(const T& m) { return m.inverse();} + static Class Identity() { return Class::identity();} + static Class Compose(const Class& m1, const Class& m2) { return m1 * m2;} + static Class Between(const Class& m1, const Class& m2) { return m1.inverse() * m2;} + static Class Inverse(const Class& m) { return m.inverse();} /// @} /// @name Manifold /// @{ - typedef T ManifoldType; - enum { dimension = ManifoldType::dimension }; + typedef Class ManifoldType; + enum { dimension = Class::dimension }; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - static int GetDimension(const ManifoldType& m){ return m.dim(); } - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other, - ChartJacobian Horigin = boost::none, - ChartJacobian Hother = boost::none) { + BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic, + "LieGroup not yet specialized for dynamically sized types."); + + static int GetDimension(const Class&) {return dimension;} + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { return origin.localCoordinates(other, Horigin, Hother); } - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v, - ChartJacobian Horigin = boost::none, - ChartJacobian Hv = boost::none) { + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { return origin.retract(v, Horigin, Hv); } @@ -72,30 +72,25 @@ struct LieGroup : Testable { /// @name Lie Group /// @{ - static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm = boost::none) { - return ManifoldType::Logmap(m, Hm); + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + return Class::Logmap(m, Hm); } - static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { - return ManifoldType::Expmap(v, Hv); + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Class::Expmap(v, Hv); } - static ManifoldType Compose(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1, - ChartJacobian H2 = boost::none) { + static Class Compose(const Class& m1, const Class& m2, ChartJacobian H1, + ChartJacobian H2 = boost::none) { return m1.compose(m2, H1, H2); } - static ManifoldType Between(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1, - ChartJacobian H2 = boost::none) { + static Class Between(const Class& m1, const Class& m2, ChartJacobian H1, + ChartJacobian H2 = boost::none) { return m1.between(m2, H1, H2); } - static ManifoldType Inverse(const ManifoldType& m, - ChartJacobian H) { + static Class Inverse(const Class& m, ChartJacobian H) { return m.inverse(H); } @@ -111,33 +106,33 @@ struct LieGroup : Testable { */ /** Compute l0 s.t. l2=l1*l0 */ -template -inline T between_default(const T& l1, const T& l2) { +template +inline Class between_default(const Class& l1, const Class& l2) { return l1.inverse().compose(l2); } /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ -template -inline Vector logmap_default(const T& l0, const T& lp) { - return T::Logmap(l0.between(lp)); +template +inline Vector logmap_default(const Class& l0, const Class& lp) { + return Class::Logmap(l0.between(lp)); } /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ -template -inline T expmap_default(const T& t, const Vector& d) { - return t.compose(T::Expmap(d)); +template +inline Class expmap_default(const Class& t, const Vector& d) { + return t.compose(Class::Expmap(d)); } /** * Lie Group Concept */ -template -class IsLieGroup: public IsGroup, public IsManifold { +template +class IsLieGroup: public IsGroup, public IsManifold { public: - typedef typename traits_x::structure_category structure_category_tag; - typedef typename traits_x::ManifoldType ManifoldType; - typedef typename traits_x::TangentVector TangentVector; - typedef typename traits_x::ChartJacobian ChartJacobian; + typedef typename traits_x::structure_category structure_category_tag; + typedef typename traits_x::ManifoldType ManifoldType; + typedef typename traits_x::TangentVector TangentVector; + typedef typename traits_x::ChartJacobian ChartJacobian; BOOST_CONCEPT_USAGE(IsLieGroup) { BOOST_STATIC_ASSERT_MSG( @@ -145,18 +140,18 @@ public: "This type's trait does not assert it is a Lie group (or derived)"); // group opertations with Jacobians - g = traits_x::Compose(g, h, Hg, Hh); - g = traits_x::Between(g, h, Hg, Hh); - g = traits_x::Inverse(g, Hg); + g = traits_x::Compose(g, h, Hg, Hh); + g = traits_x::Between(g, h, Hg, Hh); + g = traits_x::Inverse(g, Hg); // log and exp map without Jacobians - g = traits_x::Expmap(v); - v = traits_x::Logmap(g); - // log and exp map with Jacobians - //g = traits_x::Expmap(v, Hg); - //v = traits_x::Logmap(g, Hg); + g = traits_x::Expmap(v); + v = traits_x::Logmap(g); + // log and expnential map with Jacobians + g = traits_x::Expmap(v, Hg); + v = traits_x::Logmap(g, Hg); } private: - LG g, h; + T g, h; TangentVector v; ChartJacobian Hg, Hh; };