From 118d561e96de6015561023a1dfa4ac1ece2bd8c2 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Mon, 8 Dec 2014 23:57:26 +0100
Subject: [PATCH] Created GTSAM_GROUP_IDENTITY as it handles templates
 differently

---
 gtsam/base/concepts.h       | 16 ++++++++++++----
 gtsam/geometry/Cyclic.h     |  1 +
 gtsam/geometry/Quaternion.h |  5 +++--
 3 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h
index 119d3b16a..ab4e589b4 100644
--- a/gtsam/base/concepts.h
+++ b/gtsam/base/concepts.h
@@ -157,14 +157,24 @@ check_invariants(const T& a, const T& b, double tol = 1e-9) {
 }
 } // \ namespace group
 
+#define GTSAM_GROUP_IDENTITY0(GROUP) \
+namespace group { namespace traits { \
+template<> struct identity<GROUP > { static const GROUP value; typedef GROUP value_type;};\
+const GROUP identity<GROUP >::value = GROUP::Identity();\
+}}
+
+#define GTSAM_GROUP_IDENTITY(TEMPLATE,GROUP) \
+namespace group { namespace traits { \
+template<TEMPLATE> struct identity<GROUP > { static const GROUP value; typedef GROUP value_type;};\
+template<TEMPLATE> const GROUP identity<GROUP >::value = GROUP::Identity();\
+}}
+
 #define GTSAM_ADDITIVE_GROUP(TEMPLATE,GROUP) \
 namespace group { \
 template<TEMPLATE> GROUP compose(const GROUP &g, const GROUP & h) { return g + h;} \
 template<TEMPLATE> GROUP between(const GROUP &g, const GROUP & h) { return h - g;} \
 template<TEMPLATE> GROUP inverse(const GROUP &g) { return -g;} \
 namespace traits { \
-template<TEMPLATE> struct identity<GROUP > { static const GROUP value; typedef GROUP value_type;};\
-template<TEMPLATE> const GROUP identity<GROUP >::value = GROUP::Identity();\
 template<TEMPLATE> struct flavor<GROUP > { typedef additive_tag type;};\
 }}
 
@@ -174,8 +184,6 @@ template<TEMPLATE> GROUP compose(const GROUP &g, const GROUP & h) { return g * h
 template<TEMPLATE> GROUP between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
 template<TEMPLATE> GROUP inverse(const GROUP &g) { return g.inverse();} \
 namespace traits { \
-template<TEMPLATE> struct identity<GROUP > { static const GROUP value; typedef GROUP value_type;};\
-template<TEMPLATE> const GROUP identity<GROUP >::value = GROUP::Identity();\
 template<TEMPLATE> struct flavor<GROUP > { typedef multiplicative_tag type;};\
 }}
 
diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h
index 6b9820852..f10a7fdb0 100644
--- a/gtsam/geometry/Cyclic.h
+++ b/gtsam/geometry/Cyclic.h
@@ -64,6 +64,7 @@ public:
 
 #define CYCLIC_TEMPLATE size_t N
 #define CYCLIC_TYPE Cyclic<N>
+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
diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h
index db9628241..8e6437541 100644
--- a/gtsam/geometry/Quaternion.h
+++ b/gtsam/geometry/Quaternion.h
@@ -32,7 +32,7 @@ struct MakeQuaternionChart {
 
   /// Exponential map given axis/angle representation of Lie algebra
   static Q Expmap(const _Scalar& angle, const Eigen::Ref<const Omega>& axis) {
-    return Q(Eigen::AngleAxisd(angle, axis));
+    return Q(Eigen::AngleAxis<_Scalar>(angle, axis));
   }
 
   /// Exponential map, simply be converting omega to axis/angle representation
@@ -41,7 +41,7 @@ struct MakeQuaternionChart {
       return Q::Identity();
     else {
       _Scalar angle = omega.norm();
-      return Q(Eigen::AngleAxisd(angle, omega / angle));
+      return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
     }
   }
 
@@ -85,6 +85,7 @@ struct MakeQuaternionChart {
 
 #define QUATERNION_TEMPLATE typename _Scalar, int _Options
 #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
+GTSAM_GROUP_IDENTITY(QUATERNION_TEMPLATE, QUATERNION_TYPE)
 GTSAM_MULTIPLICATIVE_GROUP(QUATERNION_TEMPLATE, QUATERNION_TYPE)
 
 #define QUATERNION_TANGENT Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1>