From 6c6abe0b6c929389c5f8daeeab02d89e3aa674a9 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 24 May 2015 12:38:53 -0700
Subject: [PATCH] compose_pow and DirectProduct

---
 gtsam/base/Group.h | 65 +++++++++++++++++++++++++++++++++++++++-------
 1 file changed, 56 insertions(+), 9 deletions(-)

diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h
index f56ba9685..16c1e6a23 100644
--- a/gtsam/base/Group.h
+++ b/gtsam/base/Group.h
@@ -95,6 +95,16 @@ struct GroupTraits {
   static Class Identity() { return Class(); }
 };
 
+/// A helper class that implements the traits interface for multiplicative groups.
+/// Assumes existence of operator* and inverse method
+template<class Class>
+struct MultiplicativeGroupTraits : GroupTraits<Class> {
+    typedef multiplicative_group_tag group_flavor; \
+    static Class Compose(const Class &g, const Class & h) { return g * h;} \
+    static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \
+    static Class Inverse(const Class &g) { return g.inverse();}
+};
+
 /// A helper class that implements the traits interface for additive groups.
 /// Assumes existence of three additive operators
 template<class Class>
@@ -105,17 +115,54 @@ struct AdditiveGroupTraits : GroupTraits<Class> {
     static Class Inverse(const Class &g) { return -g;}
 };
 
-/// A helper class that implements the traits interface for multiplicative groups.
-/// Assumes existence of operators * and /, as well as inverse method
-template<class Class>
-struct MultiplicativeGroupTraits : GroupTraits<Class> {
-    typedef multiplicative_group_tag group_flavor; \
-    static Class Compose(const Class &g, const Class & h) { return g * h;} \
-    static Class Between(const Class &g, const Class & h) { return g.inverse() * h;} \
-    static Class Inverse(const Class &g) { return g.inverse();}
-};
 }  // namespace internal
 
+/// compose multiple times
+template<typename G>
+BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
+compose_pow(const G& g, size_t n) {
+  if (n == 0)
+  return traits<G>::Identity();
+  else if (n == 1)
+  return g;
+  else
+  return traits<G>::Compose(compose_pow(g, n - 1), g);
+}
+
+/// Template to construct the direct product of two arbitrary groups
+/// Assumes nothing except group structure from G and H
+template<typename G, typename H>
+class DirectProduct: public std::pair<G, H> {
+  BOOST_CONCEPT_ASSERT((IsGroup<G>));
+  BOOST_CONCEPT_ASSERT((IsGroup<H>));
+
+  const G& g() const {return this->first;}
+  const H& h() const {return this->second;}
+
+public:
+  // Construct from two subgroup elements
+  DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {
+  }
+  /// Default constructor yields identity
+  DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {
+  }
+  static DirectProduct Identity() {
+    return DirectProduct();
+  }
+  DirectProduct operator*(const DirectProduct& other) const {
+    return DirectProduct(traits<G>::Compose(g(),other.g()), traits<H>::Compose(h(),other.h()));
+  }
+  DirectProduct inverse() const {
+    return DirectProduct(g().inverse(), h().inverse());
+  }
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<typename G, typename H>
+struct traits<DirectProduct<G, H> > : internal::MultiplicativeGroupTraits<
+    DirectProduct<G, H> > {
+};
+
 /// Template to construct the direct sum of two additive groups
 /// Assumes existence of three additive operators for both groups
 template<typename G, typename H>