diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 0ffb2fbd2..39e707a95 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -149,6 +149,17 @@ check_invariants(const T& a, const T& b, double tol = 1e-9) { } } // \ namespace group +#define GTSAM_MULTIPLICATIVE_GROUP2(T1,A1,T2,A2,GROUP) \ +namespace group { \ +template GROUP compose(const GROUP &g, const GROUP & h) { return g * h;} \ +template GROUP between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ +template GROUP inverse(const GROUP &g) { return g.inverse();} \ +namespace traits { \ +template struct identity > { static const GROUP value; typedef GROUP value_type;};\ +template const GROUP identity >::value = GROUP::Identity();\ +template struct flavor > { typedef multiplicative_tag type;};\ +}} + /** * Group Concept */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 89dcd3024..eaef53230 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -111,47 +111,8 @@ struct DefaultChart > { } // \namespace gtsam::manifold::traits } // \namespace gtsam::manifold -namespace group { +GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) -template -Eigen::Quaternion compose(const Eigen::Quaternion &g, - const Eigen::Quaternion & h) { - return g * h; -} - -template -Eigen::Quaternion between(const Eigen::Quaternion &g, - const Eigen::Quaternion & h) { - return g.inverse() * h; -} - -template -Eigen::Quaternion inverse(const Eigen::Quaternion &g) { - return g.inverse(); -} - -namespace traits { - -/// Declare the trait that specifies a quaternion's identity element -template -struct identity > { - static const Eigen::Quaternion value; - typedef Eigen::Quaternion value_type; -}; - -/// Out of line definition of identity -template -const Eigen::Quaternion identity >::value = - Eigen::Quaternion::Identity(); - -/// Define the trait that asserts quaternions are a multiplicative group -template -struct flavor > { - typedef multiplicative_tag type; -}; - -} // \namespace gtsam::group::traits -} // \namespace gtsam::group } // \namespace gtsam /**