diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 4fe3f519b..52e8a4d01 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -16,6 +16,9 @@ #include #include +//FIXME temporary until all conflicts with namespace traits resolved +#define traits traits_foo + namespace gtsam { template struct traits {}; @@ -35,7 +38,9 @@ struct vector_space_tag: public lie_group_tag {}; struct multiplicative_group_tag {}; struct additive_group_tag {}; -template +// a fictitious example +class Transformation; +template<> struct traits { // Typedefs required by all manifold types. @@ -113,8 +118,8 @@ struct traits { template BOOST_CONCEPT_REQUIRES(((Testable >)),(bool)) // check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { - traits::TangentVector v0 = traits::Local(a,a); - traits::TangentVector v = traits::Local(a,b); + typename traits::TangentVector v0 = traits::Local(a,a); + typename traits::TangentVector v = traits::Local(a,b); T c = traits::Retract(a,v); return v0.norm() < tol && traits::Equals(b,c,tol); } @@ -174,7 +179,6 @@ class IsGroup { public: typedef typename traits::structure_category structure_category_tag; typedef typename traits::group_flavor flavor_tag; - static const size_t dim = traits::dimension; //typedef typename traits::identity::value_type identity_value_type; BOOST_CONCEPT_USAGE(IsGroup) { @@ -216,7 +220,7 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { #define GTSAM_ADDITIVE_GROUP(GROUP) \ - typedef additive_gropu_tag group_flavor; \ + typedef additive_group_tag group_flavor; \ static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \ static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \ static GROUP Inverse(const GROUP &g) { return -g;} @@ -259,7 +263,7 @@ public: private: LG g, h; TangentVector v; - OptionalJacobian Hg, Hh; + ChartJacobian Hg, Hh; }; diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index f10a7fdb0..3e9ab0ec7 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -64,16 +64,14 @@ public: #define CYCLIC_TEMPLATE size_t N #define CYCLIC_TYPE Cyclic -GTSAM_GROUP_IDENTITY(CYCLIC_TEMPLATE, CYCLIC_TYPE) -GTSAM_ADDITIVE_GROUP(CYCLIC_TEMPLATE, CYCLIC_TYPE) -/// Define cyclic group to be a model of the Group concept -namespace traits { -template -struct structure_category > { - typedef group_tag type; +/// Define cyclic group traits to be a model of the Group concept +template +struct traits { + typedef group_tag structure_category; + GTSAM_ADDITIVE_GROUP(CYCLIC_TYPE); + static const CYCLIC_TYPE identity = CYCLIC_TYPE::Identity(); }; -} } // \namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index b043600b6..b9bfeee47 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -17,7 +17,6 @@ #include #include -#include #define QUATERNION_TEMPLATE typename _Scalar, int _Options #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> @@ -37,7 +36,7 @@ struct traits { typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; typedef OptionalJacobian<3, 3> ChartJacobian; - static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=NULL, ChartJacobian Hh=NULL) { + static Q Compose(const Q &g, const Q & h, ChartJacobian Hg=boost::none, ChartJacobian Hh=boost::none) { if (Hg) { //TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? ) *Hg = h.toRotationMatrix().transpose(); @@ -48,7 +47,7 @@ struct traits { } return g * h; } - static Q Between(const Q &g, const Q & h, ChartJacobian Hg=NULL, ChartJacobian Hh=NULL) { + static Q Between(const Q &g, const Q & h, ChartJacobian Hg=boost::none, ChartJacobian Hh=boost::none) { Q d = g.inverse() * h; if (Hg) { //TODO : check Jacobian consistent with chart @@ -60,7 +59,7 @@ struct traits { } return d; } - static Q Inverse(const Q &g, ChartJacobian H=NULL) { + static Q Inverse(const Q &g, ChartJacobian H=boost::none) { if (H) { //TODO : check Jacobian consistent with chart *H = -g.toRotationMatrix(); @@ -70,7 +69,7 @@ struct traits { /// Exponential map, simply be converting omega to axis/angle representation // TODO: implement Jacobian - static Q Expmap(const Eigen::Ref& omega, ChartJacobian H=NULL) { + static Q Expmap(const Eigen::Ref& omega, ChartJacobian H=boost::none) { if (omega.isZero()) return Q::Identity(); else { @@ -81,7 +80,7 @@ struct traits { /// We use our own Logmap, as there is a slight bug in Eigen // TODO: implement Jacobian - static TangentVector Logmap(const Q& q, ChartJacobian H=NULL) { + static TangentVector Logmap(const Q& q, ChartJacobian H=boost::none) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, @@ -109,7 +108,7 @@ struct traits { } } - static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=NULL, ChartJacobian Hother=NULL) { + static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none) { return Logmap(Between(origin,other,Horigin,Hother)); // TODO: incorporate Jacobian of Logmap }