Merged in feature/DirectProduct (pull request #143)

Added machinery to create product types

.cproject

@@ -1035,6 +1035,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testCyclic.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testCyclic.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -3190,6 +3198,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testGroup.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testGroup.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -3414,6 +3430,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testLie.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testLie.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam/base/Group.h

@@ -13,16 +13,20 @@
  * @file Group.h
  *
  * @brief Concept check class for variable types with Group properties
- * @date Nov 5, 2011
+ * @date November, 2011
  * @author Alex Cunningham
+ * @author Frank Dellaert
  */
 
 #pragma once
 
+#include <gtsam/base/Testable.h>
+
 #include <boost/concept_check.hpp>
 #include <boost/concept/requires.hpp>
 #include <boost/type_traits/is_base_of.hpp>
 #include <boost/static_assert.hpp>
+#include <utility>
 
 namespace gtsam {
 
@@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
       && traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
 }
 
-/// Macro to add group traits, additive flavor
+namespace internal {
-#define GTSAM_ADDITIVE_GROUP(GROUP) \
-    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;}
 
-/// Macro to add group traits, multiplicative flavor
+/// A helper class that implements the traits interface for multiplicative groups.
-#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \
+/// Assumes existence of identity, operator* and inverse method
-    typedef additive_group_tag group_flavor; \
+template<class Class>
-    static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \
+struct MultiplicativeGroupTraits {
-    static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
+  typedef group_tag structure_category;
-    static GROUP Inverse(const GROUP &g) { return g.inverse();}
+  typedef multiplicative_group_tag group_flavor;
+  static Class Identity() { return Class::identity(); }
+  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 gtsam
+/// Both multiplicative group traits and Testable
+template<class Class>
+struct MultiplicativeGroup : MultiplicativeGroupTraits<Class>, Testable<Class> {};
+
+/// A helper class that implements the traits interface for additive groups.
+/// Assumes existence of identity and three additive operators
+template<class Class>
+struct AdditiveGroupTraits {
+  typedef group_tag structure_category;
+  typedef additive_group_tag group_flavor;
+  static Class Identity() { return Class::identity(); }
+  static Class Compose(const Class &g, const Class & h) { return g + h;}
+  static Class Between(const Class &g, const Class & h) { return h - g;}
+  static Class Inverse(const Class &g) { return -g;}
+};
+
+/// Both additive group traits and Testable
+template<class Class>
+struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
+
+}  // 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 and Testable 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>));
+
+public:
+  /// Default constructor yields identity
+  DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
+
+  // identity
+  static DirectProduct identity() { return DirectProduct(); }
+
+  DirectProduct operator*(const DirectProduct& other) const {
+    return DirectProduct(traits<G>::Compose(this->first, other.first),
+        traits<H>::Compose(this->second, other.second));
+  }
+  DirectProduct inverse() const {
+    return DirectProduct(this->first.inverse(), this->second.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>
+class DirectSum: public std::pair<G, H> {
+  BOOST_CONCEPT_ASSERT((IsGroup<G>));  // TODO(frank): check additive
+  BOOST_CONCEPT_ASSERT((IsGroup<H>));  // TODO(frank): check additive
+
+  const G& g() const { return this->first; }
+  const H& h() const { return this->second;}
+
+public:
+  /// Default constructor yields identity
+  DirectSum():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
+
+  // identity
+  static DirectSum identity() { return DirectSum(); }
+
+  DirectSum operator+(const DirectSum& other) const {
+    return DirectSum(g()+other.g(), h()+other.h());
+  }
+  DirectSum operator-(const DirectSum& other) const {
+    return DirectSum(g()-other.g(), h()-other.h());
+  }
+  DirectSum operator-() const {
+    return DirectSum(- g(), - h());
+  }
+};
+
+// Define direct sums to be a model of the Additive Group concept
+template<typename G, typename H>
+struct traits<DirectSum<G, H> > :
+  internal::AdditiveGroupTraits<DirectSum<G, H> > {};
+
+}  // namespace gtsam
 
 /**
  * Macros for using the IsGroup

gtsam/base/Lie.h

@@ -136,8 +136,10 @@ namespace internal {
 /// A helper class that implements the traits interface for GTSAM lie groups.
 /// To use this for your gtsam type, define:
 /// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
+/// Assumes existence of: identity, dimension, localCoordinates, retract,
+/// and additionally Logmap, Expmap, compose, between, and inverse
 template<class Class>
-struct LieGroupTraits : Testable<Class> {
+struct LieGroupTraits {
   typedef lie_group_tag structure_category;
 
   /// @name Group
@@ -167,12 +169,10 @@ struct LieGroupTraits : Testable<Class> {
       ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
     return origin.retract(v, Horigin, Hv);
   }
-
   /// @}
 
   /// @name Lie Group
   /// @{
-
   static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
     return Class::Logmap(m, Hm);
   }
@@ -195,11 +195,12 @@ struct LieGroupTraits : Testable<Class> {
       ChartJacobian H = boost::none) {
     return m.inverse(H);
   }
-
   /// @}
-
 };
 
+/// Both LieGroupTraits and Testable
+template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
+
 } // \ namepsace internal
 
 /**

gtsam/base/Manifold.h

@@ -46,7 +46,7 @@ struct manifold_tag {};
  * There may be multiple possible retractions for a given manifold, which can be chosen
  * between depending on the computational complexity.  The important criteria for
  * the creation for the retract and localCoordinates functions is that they be
- * inverse operations. The new notion of a Chart guarantees that.
+ * inverse operations.
  *
  */
 
@@ -90,9 +90,9 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
 
 /// A helper that implements the traits interface for GTSAM manifolds.
 /// To use this for your class type, define:
-/// template<> struct traits<Class> : public Manifold<Class> { };
+/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
 template<class Class>
-struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
+struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
 
   // Check that Class has the necessary machinery
   BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@@ -116,6 +116,9 @@ struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
   }
 };
 
+/// Both ManifoldTraits and Testable
+template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
+
 } // \ namespace internal
 
 /// Check invariants for Manifold type
@@ -165,6 +168,53 @@ struct FixedDimension {
       "FixedDimension instantiated for dymanically-sized type.");
 };
 
+/// Helper class to construct the product manifold of two other manifolds, M1 and M2
+/// Assumes nothing except manifold structure from M1 and M2
+template<typename M1, typename M2>
+class ProductManifold: public std::pair<M1, M2> {
+  BOOST_CONCEPT_ASSERT((IsManifold<M1>));
+  BOOST_CONCEPT_ASSERT((IsManifold<M2>));
+
+protected:
+  enum { dimension1 = traits<M1>::dimension };
+  enum { dimension2 = traits<M2>::dimension };
+
+public:
+  enum { dimension = dimension1 + dimension2 };
+  inline static size_t Dim() { return dimension;}
+  inline size_t dim() const { return dimension;}
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  /// Default constructor yields identity
+  ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
+
+  // Construct from two original manifold values
+  ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
+
+  /// Retract delta to manifold
+  ProductManifold retract(const TangentVector& xi) const {
+    M1 m1 = traits<M1>::Retract(this->first,  xi.template head<dimension1>());
+    M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
+    return ProductManifold(m1,m2);
+  }
+
+  /// Compute the coordinates in the tangent space
+  TangentVector localCoordinates(const ProductManifold& other) const {
+    typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first,  other.first);
+    typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<typename M1, typename M2>
+struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
+};
+
 } // \ namespace gtsam
 
 ///**

gtsam/base/ProductLieGroup.h

@@ -0,0 +1,197 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------1------------------------------------------- */
+
+/**
+ * @file ProductLieGroup.h
+ * @date May, 2015
+ * @author Frank Dellaert
+ * @brief Group product of two Lie Groups
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>
+#include <utility>  // pair
+
+namespace gtsam {
+
+/// Template to construct the product Lie group of two other Lie groups
+/// Assumes Lie group structure for G and H
+template<typename G, typename H>
+class ProductLieGroup: public std::pair<G, H> {
+  BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
+  typedef std::pair<G, H> Base;
+
+protected:
+  enum {dimension1 = traits<G>::dimension};
+  enum {dimension2 = traits<H>::dimension};
+
+public:
+  /// Default constructor yields identity
+  ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
+
+  // Construct from two subgroup elements
+  ProductLieGroup(const G& g, const H& h):Base(g,h) {}
+
+  // Construct from base
+  ProductLieGroup(const Base& base):Base(base) {}
+
+  /// @name Group
+  /// @{
+  typedef multiplicative_group_tag group_flavor;
+  static ProductLieGroup identity() {return ProductLieGroup();}
+
+  ProductLieGroup operator*(const ProductLieGroup& other) const {
+    return ProductLieGroup(traits<G>::Compose(this->first,other.first),
+        traits<H>::Compose(this->second,other.second));
+  }
+  ProductLieGroup inverse() const {
+    return ProductLieGroup(this->first.inverse(), this->second.inverse());
+  }
+  ProductLieGroup compose(const ProductLieGroup& g) const {
+    return (*this) * g;
+  }
+  ProductLieGroup between(const ProductLieGroup& g) const {
+    return this->inverse() * g;
+  }
+  /// @}
+
+  /// @name Manifold
+  /// @{
+  enum {dimension = dimension1 + dimension2};
+  inline static size_t Dim() {return dimension;}
+  inline size_t dim() const {return dimension;}
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  static ProductLieGroup Retract(const TangentVector& v) {
+    return ProductLieGroup::ChartAtOrigin::Retract(v);
+  }
+  static TangentVector LocalCoordinates(const ProductLieGroup& g) {
+    return ProductLieGroup::ChartAtOrigin::Local(g);
+  }
+  ProductLieGroup retract(const TangentVector& v) const {
+    return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
+  }
+  TangentVector localCoordinates(const ProductLieGroup& g) const {
+    return ProductLieGroup::ChartAtOrigin::Local(between(g));
+  }
+  /// @}
+
+  /// @name Lie Group
+  /// @{
+protected:
+  typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
+  typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
+  typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
+
+public:
+  ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
+    H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
+    if (H1) {
+      H1->setZero();
+      H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    if (H2) *H2 = Jacobian::Identity();
+    return ProductLieGroup(g,h);
+  }
+  ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
+      ChartJacobian H2 = boost::none) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
+    H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
+    if (H1) {
+      H1->setZero();
+      H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    if (H2) *H2 = Jacobian::Identity();
+    return ProductLieGroup(g,h);
+  }
+  ProductLieGroup inverse(ChartJacobian D) const {
+    Jacobian1 D_g_first; Jacobian2 D_h_second;
+    G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
+    H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
+    if (D) {
+      D->setZero();
+      D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
+      D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
+    }
+    return ProductLieGroup(g,h);
+  }
+  static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
+    if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
+    G g = traits<G>::Expmap(v.template head<dimension1>());
+    H h = traits<H>::Expmap(v.template tail<dimension2>());
+    return ProductLieGroup(g,h);
+  }
+  static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
+    if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
+    typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
+    typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
+    TangentVector v;
+    v << v1, v2;
+    return v;
+  }
+  struct ChartAtOrigin {
+    static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
+      return Logmap(m, Hm);
+    }
+    static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
+      return Expmap(v, Hv);
+    }
+  };
+  ProductLieGroup expmap(const TangentVector& v) const {
+    return compose(ProductLieGroup::Expmap(v));
+  }
+  TangentVector logmap(const ProductLieGroup& g) const {
+    return ProductLieGroup::Logmap(between(g));
+  }
+  static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
+    return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
+  }
+  static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
+    return ProductLieGroup::ChartAtOrigin::Local(g,H1);
+  }
+  ProductLieGroup retract(const TangentVector& v, //
+      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+    Jacobian D_g_v;
+    ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
+    ProductLieGroup h = compose(g,H1,H2);
+    if (H2) *H2 = (*H2) * D_g_v;
+    return h;
+  }
+  TangentVector localCoordinates(const ProductLieGroup& g, //
+      ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+    ProductLieGroup h = between(g,H1,H2);
+    Jacobian D_v_h;
+    TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
+    if (H1) *H1 = D_v_h * (*H1);
+    if (H2) *H2 = D_v_h * (*H2);
+    return v;
+  }
+  /// @}
+
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<typename G, typename H>
+struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
+    ProductLieGroup<G, H> > {
+};
+} // namespace gtsam
+

gtsam/base/VectorSpace.h

@@ -20,7 +20,7 @@ template<typename T> struct traits;
 
 namespace internal {
 
-/// VectorSpace Implementation for Fixed sizes
+/// VectorSpaceTraits Implementation for Fixed sizes
 template<class Class, int N>
 struct VectorSpaceImpl {
 
@@ -83,7 +83,7 @@ struct VectorSpaceImpl {
   /// @}
 };
 
-/// VectorSpace implementation for dynamic types.
+/// VectorSpaceTraits implementation for dynamic types.
 template<class Class>
 struct VectorSpaceImpl<Class,Eigen::Dynamic> {
 
@@ -159,11 +159,11 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
   /// @}
 };
 
-/// A helper that implements the traits interface for GTSAM lie groups.
+/// A helper that implements the traits interface for GTSAM vector space types.
 /// To use this for your gtsam type, define:
-/// template<> struct traits<Type> : public VectorSpace<Type> { };
+/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
 template<class Class>
-struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
+struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
 
   typedef vector_space_tag structure_category;
 
@@ -178,9 +178,12 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
   enum { dimension = Class::dimension};
   typedef Class ManifoldType;
   /// @}
-
 };
 
+/// Both VectorSpaceTRaits and Testable
+template<class Class>
+struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
+
 /// A helper that implements the traits interface for GTSAM lie groups.
 /// To use this for your gtsam type, define:
 /// template<> struct traits<Type> : public ScalarTraits<Type> { };

gtsam/base/tests/testGroup.cpp

@@ -0,0 +1,143 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testGroup.cpp
+ * @brief  Unit tests for groups
+ * @author Frank Dellaert
+ **/
+
+#include <gtsam/base/Group.h>
+#include <gtsam/base/Testable.h>
+#include <Eigen/Core>
+#include <iostream>
+#include <boost/foreach.hpp>
+
+namespace gtsam {
+
+/// Symmetric group
+template<int N>
+class Symmetric: private Eigen::PermutationMatrix<N> {
+  Symmetric(const Eigen::PermutationMatrix<N>& P) :
+      Eigen::PermutationMatrix<N>(P) {
+  }
+public:
+  static Symmetric identity() { return Symmetric(); }
+  Symmetric() {
+    Eigen::PermutationMatrix<N>::setIdentity();
+  }
+  static Symmetric Transposition(int i, int j) {
+    Symmetric g;
+    return g.applyTranspositionOnTheRight(i, j);
+  }
+  Symmetric operator*(const Symmetric& other) const {
+    return Eigen::PermutationMatrix<N>::operator*(other);
+  }
+  bool operator==(const Symmetric& other) const {
+    for (size_t i = 0; i < N; i++)
+      if (this->indices()[i] != other.indices()[i])
+        return false;
+    return true;
+  }
+  Symmetric inverse() const {
+    return Eigen::PermutationMatrix<N>(Eigen::PermutationMatrix<N>::inverse());
+  }
+  friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) {
+    for (size_t i = 0; i < N; i++)
+      os << m.indices()[i] << " ";
+    return os;
+  }
+  void print(const std::string& s = "") const {
+    std::cout << s << *this << std::endl;
+  }
+  bool equals(const Symmetric<N>& other, double tol = 0) const {
+    return this->indices() == other.indices();
+  }
+};
+
+/// Define permutation group traits to be a model of the Multiplicative Group concept
+template<int N>
+struct traits<Symmetric<N> > : internal::MultiplicativeGroupTraits<Symmetric<N> >,
+    Testable<Symmetric<N> > {
+};
+
+} // namespace gtsam
+
+#include <gtsam/base/Testable.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+typedef Symmetric<2> S2;
+TEST(Group, S2) {
+  S2 e, s1 = S2::Transposition(0, 1);
+  BOOST_CONCEPT_ASSERT((IsGroup<S2>));
+  EXPECT(check_group_invariants(e, s1));
+}
+
+//******************************************************************************
+typedef Symmetric<3> S3;
+TEST(Group, S3) {
+  S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
+  BOOST_CONCEPT_ASSERT((IsGroup<S3>));
+  EXPECT(check_group_invariants(e, s1));
+  EXPECT(assert_equal(s1, s1 * e));
+  EXPECT(assert_equal(s1, e * s1));
+  EXPECT(assert_equal(e, s1 * s1));
+  S3 g = s1 * s2; // 1 2 0
+  EXPECT(assert_equal(s1, g * s2));
+  EXPECT(assert_equal(e, compose_pow(g, 0)));
+  EXPECT(assert_equal(g, compose_pow(g, 1)));
+  EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup
+}
+
+//******************************************************************************
+// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon,
+// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon)
+typedef DirectProduct<S2, S3> Dih6;
+
+std::ostream &operator<<(std::ostream &os, const Dih6& m) {
+  os << "( " << m.first << ", " << m.second << ")";
+  return os;
+}
+
+// Provide traits with Testable
+namespace gtsam {
+template<>
+struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
+  static void Print(const Dih6& m, const string& s = "") {
+    cout << s << m << endl;
+  }
+  static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) {
+    return m1 == m2;
+  }
+};
+} // namespace gtsam
+
+TEST(Group, Dih6) {
+  Dih6 e, g(S2::Transposition(0, 1),
+      S3::Transposition(0, 1) * S3::Transposition(1, 2));
+  BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
+  EXPECT(check_group_invariants(e, g));
+  EXPECT(assert_equal(e, compose_pow(g, 0)));
+  EXPECT(assert_equal(g, compose_pow(g, 1)));
+  EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+

gtsam/base/tests/testLieScalar.cpp

@@ -37,8 +37,8 @@ TEST(LieScalar , Concept) {
 //******************************************************************************
 TEST(LieScalar , Invariants) {
   LieScalar lie1(2), lie2(3);
-  check_group_invariants(lie1, lie2);
+  CHECK(check_group_invariants(lie1, lie2));
-  check_manifold_invariants(lie1, lie2);
+  CHECK(check_manifold_invariants(lie1, lie2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/Cyclic.h

@@ -16,10 +16,8 @@
  **/
 
 #include <gtsam/base/Group.h>
-#include <cstddef>
+#include <gtsam/base/Testable.h>
-#include <string>
+#include <iostream> // for cout :-(
-#include <iostream>
-#include <assert.h>
 
 namespace gtsam {
 
@@ -33,10 +31,11 @@ public:
       i_(i) {
     assert(i < N);
   }
-  /// Idenity element
+  /// Default constructor yields identity
-  static Cyclic Identity() {
+  Cyclic():i_(0) {
-    return Cyclic(0);
   }
+  static Cyclic identity() { return Cyclic();}
+
   /// Cast to size_t
   operator size_t() const {
     return i_;
@@ -63,20 +62,10 @@ public:
   }
 };
 
-/// Define cyclic group traits to be a model of the Group concept
+/// Define cyclic group to be a model of the Additive Group concept
 template<size_t N>
-struct traits<Cyclic<N> > {
+struct traits<Cyclic<N> > : internal::AdditiveGroupTraits<Cyclic<N> >, //
-  typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic<N>)
+    Testable<Cyclic<N> > {
-  static Cyclic<N> Identity() {
-    return Cyclic<N>::Identity();
-  }
-  static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b,
-      double tol = 1e-9) {
-    return a.equals(b, tol);
-  }
-  static void Print(const Cyclic<N>& c, const std::string &s = "") {
-    c.print(s);
-  }
 };
 
 } // \namespace gtsam

gtsam/geometry/EssentialMatrix.cpp

@@ -13,64 +13,46 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
+EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
     OptionalJacobian<5, 6> H) {
-  const Rot3& _1R2_ = _1P2_.rotation();
+  const Rot3& aRb = aPb.rotation();
-  const Point3& _1T2_ = _1P2_.translation();
+  const Point3& aTb = aPb.translation();
   if (!H) {
     // just make a direction out of translation and create E
-    Unit3 direction(_1T2_);
+    Unit3 direction(aTb);
-    return EssentialMatrix(_1R2_, direction);
+    return EssentialMatrix(aRb, direction);
   } else {
     // Calculate the 5*6 Jacobian H = D_E_1P2
     // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
     // First get 2*3 derivative from Unit3::FromPoint3
     Matrix23 D_direction_1T2;
-    Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
+    Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
     *H << I_3x3, Z_3x3, //
-    Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
+    Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
-    return EssentialMatrix(_1R2_, direction);
+    return EssentialMatrix(aRb, direction);
   }
 }
 
 /* ************************************************************************* */
 void EssentialMatrix::print(const string& s) const {
   cout << s;
-  aRb_.print("R:\n");
+  rotation().print("R:\n");
-  aTb_.print("d: ");
+  direction().print("d: ");
-}
-
-/* ************************************************************************* */
-EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
-  assert(xi.size() == 5);
-  Vector3 omega(sub(xi, 0, 3));
-  Vector2 z(sub(xi, 3, 5));
-  Rot3 R = aRb_.retract(omega);
-  Unit3 t = aTb_.retract(z);
-  return EssentialMatrix(R, t);
-}
-
-/* ************************************************************************* */
-Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
-  Vector5 v;
-  v << aRb_.localCoordinates(other.aRb_),
-      aTb_.localCoordinates(other.aTb_);
-  return v;
 }
 
 /* ************************************************************************* */
 Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
     OptionalJacobian<3, 3> Dpoint) const {
-  Pose3 pose(aRb_, aTb_.point3());
+  Pose3 pose(rotation(), direction().point3());
   Matrix36 DE_;
   Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
   if (DE) {
     // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
     // The last 3 columns are derivative with respect to change in translation
-    // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
+    // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
     // Duy made an educated guess that this needs to be rotated to the local frame
     Matrix35 H;
-    H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
+    H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
     *DE = H;
   }
   return q;
@@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
     OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
 
   // The rotation must be conjugated to act in the camera frame
-  Rot3 c1Rc2 = aRb_.conjugate(cRb);
+  Rot3 c1Rc2 = rotation().conjugate(cRb);
 
   if (!HE && !HR) {
     // Rotate translation direction and return
-    Unit3 c1Tc2 = cRb * aTb_;
+    Unit3 c1Tc2 = cRb * direction();
     return EssentialMatrix(c1Rc2, c1Tc2);
   } else {
     // Calculate derivatives
     Matrix23 D_c1Tc2_cRb; // 2*3
     Matrix2 D_c1Tc2_aTb; // 2*2
-    Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
+    Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
     if (HE)
       *HE << cRb.matrix(), Matrix32::Zero(), //
       Matrix23::Zero(), D_c1Tc2_aTb;
@@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
   if (H) {
     // See math.lyx
     Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+    Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
-        * aTb_.basis();
+        * direction().basis();
     *H << HR, HD;
   }
   return dot(vA, E_ * vB);

gtsam/geometry/EssentialMatrix.h

@@ -10,6 +10,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
+#include <gtsam/base/Manifold.h>
 #include <iosfwd>
 
 namespace gtsam {
@@ -20,17 +21,18 @@ namespace gtsam {
  * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
  * We can then non-linearly optimize immediately on this 5-dimensional manifold.
  */
-class GTSAM_EXPORT EssentialMatrix {
+class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {
 
 private:
-
+  typedef ProductManifold<Rot3, Unit3> Base;
-  Rot3 aRb_; ///< Rotation between a and b
-  Unit3 aTb_; ///< translation direction from a to b
   Matrix3 E_; ///< Essential matrix
 
-public:
+  /// Construct from Base
+  EssentialMatrix(const Base& base) :
+      Base(base), E_(direction().skew() * rotation().matrix()) {
+  }
 
-  enum { dimension = 5 };
+public:
 
   /// Static function to convert Point2 to homogeneous coordinates
   static Vector3 Homogeneous(const Point2& p) {
@@ -42,12 +44,12 @@ public:
 
   /// Default constructor
   EssentialMatrix() :
-      aTb_(1, 0, 0), E_(aTb_.skew()) {
+      Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
   }
 
   /// Construct from rotation and translation
   EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
-      aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
+      Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
   }
 
   /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
@@ -72,7 +74,8 @@ public:
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
-    return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
+    return rotation().equals(other.rotation(), tol)
+        && direction().equals(other.direction(), tol);
   }
 
   /// @}
@@ -80,22 +83,19 @@ public:
   /// @name Manifold
   /// @{
 
-  /// Dimensionality of tangent space = 5 DOF
+  using Base::dimension;
-  inline static size_t Dim() {
+  using Base::dim;
-    return 5;
+  using Base::Dim;
-  }
-
-  /// Return the dimensionality of the tangent space
-  size_t dim() const {
-    return 5;
-  }
 
   /// Retract delta to manifold
-  EssentialMatrix retract(const Vector& xi) const;
+  EssentialMatrix retract(const TangentVector& v) const {
+    return Base::retract(v);
+  }
 
   /// Compute the coordinates in the tangent space
-  Vector5 localCoordinates(const EssentialMatrix& other) const;
+  TangentVector localCoordinates(const EssentialMatrix& other) const {
-
+    return Base::localCoordinates(other);
+  }
   /// @}
 
   /// @name Essential matrix methods
@@ -103,12 +103,12 @@ public:
 
   /// Rotation
   inline const Rot3& rotation() const {
-    return aRb_;
+    return this->first;
   }
 
   /// Direction
   inline const Unit3& direction() const {
-    return aTb_;
+    return this->second;
   }
 
   /// Return 3*3 matrix representation
@@ -118,12 +118,12 @@ public:
 
   /// Return epipole in image_a , as Unit3 to allow for infinity
   inline const Unit3& epipole_a() const {
-    return aTb_; // == direction()
+    return direction();
   }
 
   /// Return epipole in image_b, as Unit3 to allow for infinity
   inline Unit3 epipole_b() const {
-    return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
+    return rotation().unrotate(direction());
   }
 
   /**
@@ -180,8 +180,8 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(aRb_);
+      ar & BOOST_SERIALIZATION_NVP(first);
-      ar & BOOST_SERIALIZATION_NVP(aTb_);
+      ar & BOOST_SERIALIZATION_NVP(second);
 
       ar & boost::serialization::make_nvp("E11", E_(0,0));
       ar & boost::serialization::make_nvp("E12", E_(0,1));
@@ -195,7 +195,6 @@ private:
     }
 
   /// @}
-
 };
 
 template<>

gtsam/geometry/Pose2.h

@@ -290,10 +290,10 @@ typedef std::pair<Point2,Point2> Point2Pair;
 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
 template<>
-struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
+struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 
 template<>
-struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
+struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
 
 } // namespace gtsam
 

gtsam/geometry/Pose3.h

@@ -324,10 +324,10 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 typedef std::vector<Pose3> Pose3Vector;
 
 template<>
-struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
+struct traits<Pose3> : public internal::LieGroup<Pose3> {};
 
 template<>
-struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
+struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
 
 
 } // namespace gtsam

gtsam/geometry/Rot2.h

@@ -208,9 +208,9 @@ namespace gtsam {
   };
 
   template<>
-  struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
+  struct traits<Rot2> : public internal::LieGroup<Rot2> {};
 
   template<>
-  struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
+  struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
 
 } // gtsam

gtsam/geometry/Rot3.h

@@ -463,9 +463,9 @@ namespace gtsam {
   GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
 
   template<>
-  struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
+  struct traits<Rot3> : public internal::LieGroup<Rot3> {};
   
   template<>
-  struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
+  struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
 }
 

gtsam/geometry/SO3.h

@@ -129,11 +129,11 @@ public:
 };
 
 template<>
-struct traits<SO3> : public internal::LieGroupTraits<SO3> {
+struct traits<SO3> : public internal::LieGroup<SO3> {
 };
 
 template<>
-struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
+struct traits<const SO3> : public internal::LieGroup<SO3> {
 };
 } // end namespace gtsam
 

gtsam/geometry/tests/testCyclic.cpp

@@ -22,52 +22,114 @@
 using namespace std;
 using namespace gtsam;
 
-typedef Cyclic<3> G; // Let's use the cyclic group of order 3
+typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3
+typedef Cyclic<2> Z2;
 
 //******************************************************************************
 TEST(Cyclic, Concept) {
-  BOOST_CONCEPT_ASSERT((IsGroup<G>));
+  BOOST_CONCEPT_ASSERT((IsGroup<Z3>));
-  EXPECT_LONGS_EQUAL(0, traits<G>::Identity());
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Identity());
 }
 
 //******************************************************************************
 TEST(Cyclic, Constructor) {
-  G g(0);
+  Z3 g(0);
 }
 
 //******************************************************************************
 TEST(Cyclic, Compose) {
-  EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(0),G(0)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Compose(Z3(0),Z3(0)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(0),G(1)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Compose(Z3(0),Z3(1)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(0),G(2)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Compose(Z3(0),Z3(2)));
 
-  EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(2),G(0)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Compose(Z3(2),Z3(0)));
-  EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(2),G(1)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Compose(Z3(2),Z3(1)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(2),G(2)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Compose(Z3(2),Z3(2)));
 }
 
 //******************************************************************************
 TEST(Cyclic, Between) {
-  EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(0),G(0)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Between(Z3(0),Z3(0)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(0),G(1)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Between(Z3(0),Z3(1)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(0),G(2)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Between(Z3(0),Z3(2)));
 
-  EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(2),G(0)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Between(Z3(2),Z3(0)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(2),G(1)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Between(Z3(2),Z3(1)));
-  EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(2),G(2)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Between(Z3(2),Z3(2)));
 }
 
 //******************************************************************************
-TEST(Cyclic, Ivnverse) {
+TEST(Cyclic, Inverse) {
-  EXPECT_LONGS_EQUAL(0, traits<G>::Inverse(G(0)));
+  EXPECT_LONGS_EQUAL(0, traits<Z3>::Inverse(Z3(0)));
-  EXPECT_LONGS_EQUAL(2, traits<G>::Inverse(G(1)));
+  EXPECT_LONGS_EQUAL(2, traits<Z3>::Inverse(Z3(1)));
-  EXPECT_LONGS_EQUAL(1, traits<G>::Inverse(G(2)));
+  EXPECT_LONGS_EQUAL(1, traits<Z3>::Inverse(Z3(2)));
+}
+
+//******************************************************************************
+TEST(Cyclic, Negation) {
+  EXPECT_LONGS_EQUAL(0, -Z3(0));
+  EXPECT_LONGS_EQUAL(2, -Z3(1));
+  EXPECT_LONGS_EQUAL(1, -Z3(2));
+}
+
+//******************************************************************************
+TEST(Cyclic, Negation2) {
+  EXPECT_LONGS_EQUAL(0, -Z2(0));
+  EXPECT_LONGS_EQUAL(1, -Z2(1));
 }
 
 //******************************************************************************
 TEST(Cyclic , Invariants) {
-  G g(2), h(1);
+  Z3 g(2), h(1);
- check_group_invariants(g,h);
+  EXPECT(check_group_invariants(g,h));
+}
+
+//******************************************************************************
+// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
+// smallest non-cyclic group called the Klein four-group:
+typedef DirectSum<Z2, Z2> K4;
+
+namespace gtsam {
+
+/// Define K4 to be a model of the Additive Group concept, and provide Testable
+template<>
+struct traits<K4> : internal::AdditiveGroupTraits<K4> {
+  static void Print(const K4& m, const string& s = "") {
+    cout << s << "(" << m.first << "," << m.second << ")" << endl;
+  }
+  static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) {
+    return m1 == m2;
+  }
+};
+
+}  // namespace gtsam
+
+TEST(Cyclic , DirectSum) {
+  // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
+  // smallest non-cyclic group called the Klein four-group:
+  BOOST_CONCEPT_ASSERT((IsGroup<K4>));
+  BOOST_CONCEPT_ASSERT((IsTestable<K4>));
+
+  // Refer to http://en.wikipedia.org/wiki/Klein_four-group
+  K4 e(0,0), a(0, 1), b(1, 0), c(1, 1);
+  EXPECT(assert_equal(a, - a));
+  EXPECT(assert_equal(b, - b));
+  EXPECT(assert_equal(c, - c));
+  EXPECT(assert_equal(a, a + e));
+  EXPECT(assert_equal(b, b + e));
+  EXPECT(assert_equal(c, c + e));
+  EXPECT(assert_equal(e, a + a));
+  EXPECT(assert_equal(e, b + b));
+  EXPECT(assert_equal(e, c + c));
+  EXPECT(assert_equal(c, a + b));
+  EXPECT(assert_equal(b, a + c));
+  EXPECT(assert_equal(a, b + c));
+  EXPECT(assert_equal(c, a - b));
+  EXPECT(assert_equal(a, b - c));
+  EXPECT(assert_equal(b, c - a));
+  EXPECT(check_group_invariants(a, b));
+  EXPECT(check_group_invariants(b, c));
+  EXPECT(check_group_invariants(c, a));
 }
 
 //******************************************************************************

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3) {
+  EssentialMatrix expected(c1Rc2, Unit3(c1Tc2));
+  Pose3 pose(c1Rc2, c1Tc2);
+  EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
+  EXPECT(assert_equal(expected, actual));
+}
+
 //*******************************************************************************
 TEST(EssentialMatrix, localCoordinates0) {
   EssentialMatrix E;
@@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) {
   Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
   EXPECT(assert_equal(zero(5), actual, 1e-8));
 
-  Vector d = zero(6);
+  Vector6 d;
-  d(4) += 1e-5;
+  d << 0.1, 0.2, 0.3, 0, 0, 0;
   Vector actual2 = hx.localCoordinates(
       EssentialMatrix::FromPose3(pose.retract(d)));
-  EXPECT(assert_equal(zero(5), actual2, 1e-8));
+  EXPECT(assert_equal(d.head(5), actual2, 1e-8));
 }
 
 //*************************************************************************
@@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, RoundTrip) {
+  Vector5 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5;
+  EssentialMatrix e, hx = e.retract(d);
+  Vector actual = e.localCoordinates(hx);
+  EXPECT(assert_equal(d, actual, 1e-8));
+}
+
 //*************************************************************************
 Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
   return E.transform_to(point);

gtsam/geometry/tests/testPoint2.cpp

@@ -37,8 +37,8 @@ TEST(Double , Concept) {
 //******************************************************************************
 TEST(Double , Invariants) {
   double p1(2), p2(5);
-  check_group_invariants(p1, p2);
+  EXPECT(check_group_invariants(p1, p2));
-  check_manifold_invariants(p1, p2);
+  EXPECT(check_manifold_invariants(p1, p2));
 }
 
 //******************************************************************************
@@ -51,8 +51,8 @@ TEST(Point2 , Concept) {
 //******************************************************************************
 TEST(Point2 , Invariants) {
   Point2 p1(1, 2), p2(4, 5);
-  check_group_invariants(p1, p2);
+  EXPECT(check_group_invariants(p1, p2));
-  check_manifold_invariants(p1, p2);
+  EXPECT(check_manifold_invariants(p1, p2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPoint3.cpp

@@ -36,8 +36,8 @@ TEST(Point3 , Concept) {
 //******************************************************************************
 TEST(Point3 , Invariants) {
   Point3 p1(1, 2, 3), p2(4, 5, 6);
-  check_group_invariants(p1, p2);
+  EXPECT(check_group_invariants(p1, p2));
-  check_manifold_invariants(p1, p2);
+  EXPECT(check_manifold_invariants(p1, p2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose2.cpp

@@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
 TEST(Pose2 , Invariants) {
   Pose2 id;
 
-  check_group_invariants(id,id);
+  EXPECT(check_group_invariants(id,id));
-  check_group_invariants(id,T1);
+  EXPECT(check_group_invariants(id,T1));
-  check_group_invariants(T2,id);
+  EXPECT(check_group_invariants(T2,id));
-  check_group_invariants(T2,T1);
+  EXPECT(check_group_invariants(T2,T1));
 
-  check_manifold_invariants(id,id);
+  EXPECT(check_manifold_invariants(id,id));
-  check_manifold_invariants(id,T1);
+  EXPECT(check_manifold_invariants(id,T1));
-  check_manifold_invariants(T2,id);
+  EXPECT(check_manifold_invariants(T2,id));
-  check_manifold_invariants(T2,T1);
+  EXPECT(check_manifold_invariants(T2,T1));
 
 }
 

gtsam/geometry/tests/testPose3.cpp

@@ -760,15 +760,15 @@ TEST( Pose3, stream)
 TEST(Pose3 , Invariants) {
   Pose3 id;
 
-  check_group_invariants(id,id);
+  EXPECT(check_group_invariants(id,id));
-  check_group_invariants(id,T3);
+  EXPECT(check_group_invariants(id,T3));
-  check_group_invariants(T2,id);
+  EXPECT(check_group_invariants(T2,id));
-  check_group_invariants(T2,T3);
+  EXPECT(check_group_invariants(T2,T3));
 
-  check_manifold_invariants(id,id);
+  EXPECT(check_manifold_invariants(id,id));
-  check_manifold_invariants(id,T3);
+  EXPECT(check_manifold_invariants(id,T3));
-  check_manifold_invariants(T2,id);
+  EXPECT(check_manifold_invariants(T2,id));
-  check_manifold_invariants(T2,T3);
+  EXPECT(check_manifold_invariants(T2,T3));
 }
 
 //******************************************************************************

gtsam/geometry/tests/testQuaternion.cpp

@@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) {
 
 //******************************************************************************
 TEST(Quaternion , Invariants) {
-  check_group_invariants(id, id);
+  EXPECT(check_group_invariants(id, id));
-  check_group_invariants(id, R1);
+  EXPECT(check_group_invariants(id, R1));
-  check_group_invariants(R2, id);
+  EXPECT(check_group_invariants(R2, id));
-  check_group_invariants(R2, R1);
+  EXPECT(check_group_invariants(R2, R1));
 
-  check_manifold_invariants(id, id);
+  EXPECT(check_manifold_invariants(id, id));
-  check_manifold_invariants(id, R1);
+  EXPECT(check_manifold_invariants(id, R1));
-  check_manifold_invariants(R2, id);
+  EXPECT(check_manifold_invariants(R2, id));
-  check_manifold_invariants(R2, R1);
+  EXPECT(check_manifold_invariants(R2, R1));
 }
 
 //******************************************************************************

gtsam/geometry/tests/testRot2.cpp

@@ -163,15 +163,15 @@ Rot2 T2(0.2);
 TEST(Rot2 , Invariants) {
   Rot2 id;
 
-  check_group_invariants(id,id);
+  EXPECT(check_group_invariants(id,id));
-  check_group_invariants(id,T1);
+  EXPECT(check_group_invariants(id,T1));
-  check_group_invariants(T2,id);
+  EXPECT(check_group_invariants(T2,id));
-  check_group_invariants(T2,T1);
+  EXPECT(check_group_invariants(T2,T1));
 
-  check_manifold_invariants(id,id);
+  EXPECT(check_manifold_invariants(id,id));
-  check_manifold_invariants(id,T1);
+  EXPECT(check_manifold_invariants(id,T1));
-  check_manifold_invariants(T2,id);
+  EXPECT(check_manifold_invariants(T2,id));
-  check_manifold_invariants(T2,T1);
+  EXPECT(check_manifold_invariants(T2,T1));
 
 }
 

gtsam/geometry/tests/testRot3.cpp

@@ -659,17 +659,17 @@ Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
 TEST(Rot3 , Invariants) {
   Rot3 id;
 
-  check_group_invariants(id,id);
+  EXPECT(check_group_invariants(id,id));
-  check_group_invariants(id,T1);
+  EXPECT(check_group_invariants(id,T1));
-  check_group_invariants(T2,id);
+  EXPECT(check_group_invariants(T2,id));
-  check_group_invariants(T2,T1);
+  EXPECT(check_group_invariants(T2,T1));
-  check_group_invariants(T1,T2);
+  EXPECT(check_group_invariants(T1,T2));
 
-  check_manifold_invariants(id,id);
+  EXPECT(check_manifold_invariants(id,id));
-  check_manifold_invariants(id,T1);
+  EXPECT(check_manifold_invariants(id,T1));
-  check_manifold_invariants(T2,id);
+  EXPECT(check_manifold_invariants(T2,id));
-  check_manifold_invariants(T2,T1);
+  EXPECT(check_manifold_invariants(T2,T1));
-  check_manifold_invariants(T1,T2);
+  EXPECT(check_manifold_invariants(T1,T2));
 }
 
 //******************************************************************************

gtsam/geometry/tests/testSO3.cpp

@@ -57,15 +57,15 @@ TEST(SO3 , Retract) {
 
 //******************************************************************************
 TEST(SO3 , Invariants) {
-  check_group_invariants(id,id);
+  EXPECT(check_group_invariants(id,id));
-  check_group_invariants(id,R1);
+  EXPECT(check_group_invariants(id,R1));
-  check_group_invariants(R2,id);
+  EXPECT(check_group_invariants(R2,id));
-  check_group_invariants(R2,R1);
+  EXPECT(check_group_invariants(R2,R1));
 
-  check_manifold_invariants(id,id);
+  EXPECT(check_manifold_invariants(id,id));
-  check_manifold_invariants(id,R1);
+  EXPECT(check_manifold_invariants(id,R1));
-  check_manifold_invariants(R2,id);
+  EXPECT(check_manifold_invariants(R2,id));
-  check_manifold_invariants(R2,R1);
+  EXPECT(check_manifold_invariants(R2,R1));
 }
 
 //******************************************************************************

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -3,18 +3,15 @@
  * @author Alex Cunningham
  */
 
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/Vector.h>
-
-#include <gtsam/geometry/Pose2.h>
-
 #include <gtsam_unstable/dynamics/PoseRTV.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Vector.h>
 
 namespace gtsam {
 
 using namespace std;
 
-static const Vector g = delta(3, 2, 9.81);
+static const Vector kGravity = delta(3, 2, 9.81);
 
 /* ************************************************************************* */
 double bound(double a, double min, double max) {
@@ -24,28 +21,30 @@ double bound(double a, double min, double max) {
 }
 
 /* ************************************************************************* */
-PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
+PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
-    double vx, double vy, double vz)
+    double z, double vx, double vy, double vz) :
-: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {}
+    Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
+        Velocity3(vx, vy, vz)) {
+}
 
 /* ************************************************************************* */
-PoseRTV::PoseRTV(const Vector& rtv)
+PoseRTV::PoseRTV(const Vector& rtv) :
-: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3))
+    Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
-{
+        Velocity3(rtv.tail(3))) {
 }
 
 /* ************************************************************************* */
 Vector PoseRTV::vector() const {
   Vector rtv(9);
-  rtv.head(3) = Rt_.rotation().xyz();
+  rtv.head(3) = rotation().xyz();
-  rtv.segment(3,3) = Rt_.translation().vector();
+  rtv.segment(3,3) = translation().vector();
-  rtv.tail(3) = v_.vector();
+  rtv.tail(3) = velocity().vector();
   return rtv;
 }
 
 /* ************************************************************************* */
 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
-  return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol);
+  return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
 }
 
 /* ************************************************************************* */
@@ -53,73 +52,7 @@ void PoseRTV::print(const string& s) const {
   cout << s << ":" << endl;
   gtsam::print((Vector)R().xyz(), "  R:rpy");
   t().print("  T");
-  v_.print("  V");
+  velocity().print("  V");
-}
-
-/* ************************************************************************* */
-PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) {
-  if (H) CONCEPT_NOT_IMPLEMENTED;
-  Pose3 newPose = Pose3::Expmap(v.head<6>());
-  Velocity3 newVel = Velocity3(v.tail<3>());
-  return PoseRTV(newPose, newVel);
-}
-
-/* ************************************************************************* */
-Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) {
-  if (H) CONCEPT_NOT_IMPLEMENTED;
-  Vector6 Lx = Pose3::Logmap(p.Rt_);
-  Vector3 Lv = p.v_.vector();
-  return (Vector9() << Lx, Lv).finished();
-}
-
-/* ************************************************************************* */
-PoseRTV PoseRTV::retract(const Vector& v,
-                         ChartJacobian Horigin,
-                         ChartJacobian Hv) const {
-  if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
-  assert(v.size() == 9);
-  // First order approximation
-  Pose3 newPose = Rt_.retract(sub(v, 0, 6));
-  Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9));
-  return PoseRTV(newPose, newVel);
-}
-
-/* ************************************************************************* */
-Vector PoseRTV::localCoordinates(const PoseRTV& p1,
-                                 ChartJacobian Horigin,
-                                 ChartJacobian Hp) const {
-  if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED;
-  const Pose3& x0 = pose(), &x1 = p1.pose();
-  // First order approximation
-  Vector6 poseLogmap = x0.localCoordinates(x1);
-  Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
-  return (Vector(9) << poseLogmap, lv).finished();
-}
-
-/* ************************************************************************* */
-PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
-PoseRTV PoseRTV::inverse(ChartJacobian H1) const {
-  if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
-  return PoseRTV(Rt_.inverse(), - v_);
-}
-
-/* ************************************************************************* */
-PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
-PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1,
-    ChartJacobian H2) const {
-  if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
-  if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
-  return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_);
-}
-
-/* ************************************************************************* */
-PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
-PoseRTV PoseRTV::between(const PoseRTV& p,
-    ChartJacobian H1,
-    ChartJacobian H2) const {
-  if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5);
-  if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5);
-  return inverse().compose(p);
 }
 
 /* ************************************************************************* */
@@ -187,7 +120,7 @@ PoseRTV PoseRTV::generalDynamics(
   Rot3 r2 = rotation().retract(gyro * dt);
 
   //  Integrate Velocity Equations
-  Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g)));
+  Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
 
   //  Integrate Position Equations
   Point3 t2 = translationIntegration(r2, v2, dt);
@@ -205,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
 
   // acceleration
   Vector3 accel = (v2-v1).vector() / dt;
-  imu.head<3>() = r2.transpose() * (accel - g);
+  imu.head<3>() = r2.transpose() * (accel - kGravity);
 
   // rotation rates
   // just using euler angles based on matlab code
@@ -232,54 +165,40 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub
 }
 
 /* ************************************************************************* */
-double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
 double PoseRTV::range(const PoseRTV& other,
     OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const {
-  if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
+  Matrix36 D_t1_pose, D_t2_other;
-  if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
+  const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
-  return t().distance(other.t());
+  const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
+  Matrix13 D_d_t1, D_d_t2;
+  double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
+  if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
+  if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
+  return d;
 }
 
 /* ************************************************************************* */
-PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
-  return global.transformed_from(transform);
-}
-
 PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
     OptionalJacobian<9, 6> Dtrans) const {
-  // Note that we rotate the velocity
-  Matrix DVr, DTt;
-  Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);
-  if (!Dglobal && !Dtrans)
-    return PoseRTV(trans.compose(pose()), newvel);
 
   // Pose3 transform is just compose
-  Matrix DTc, DGc;
+  Matrix6 D_newpose_trans, D_newpose_pose;
-  Pose3 newpose = trans.compose(pose(), DTc, DGc);
+  Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
+
+  // Note that we rotate the velocity
+  Matrix3 D_newvel_R, D_newvel_v;
+  Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v);
 
   if (Dglobal) {
-    *Dglobal = zeros(9,9);
+    Dglobal->setZero();
-    insertSub(*Dglobal, DGc, 0, 0);
+    Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
-
+    Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
-    // Rotate velocity
-    insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix?
   }
 
   if (Dtrans) {
-    *Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8);
+    Dtrans->setZero();
-    //
+    Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
-    //    *Dtrans = zeros(9,6);
+    Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
-    //    // directly affecting the pose
-    //    insertSub(*Dtrans, DTc, 0, 0); // correct in tests
-    //
-    //    // rotating the velocity
-    //    Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z());
-    //    trans.rotation().print("Transform rotation");
-    //    gtsam::print(vRhat, "vRhat");
-    //    gtsam::print(DVr, "DVr");
-    //    // FIXME: find analytic derivative
-    ////    insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I
-    ////    insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail
   }
   return PoseRTV(newpose, newvel);
 }

gtsam_unstable/dynamics/PoseRTV.h

@@ -7,8 +7,8 @@
 #pragma once
 
 #include <gtsam_unstable/base/dllexport.h>
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/ProductLieGroup.h>
 
 namespace gtsam {
 
@@ -19,29 +19,30 @@ typedef Point3 Velocity3;
  * Robot state for use with IMU measurements
  * - contains translation, translational velocity and rotation
  */
-class GTSAM_UNSTABLE_EXPORT PoseRTV {
+class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
 protected:
 
-  Pose3 Rt_;
+  typedef ProductLieGroup<Pose3,Velocity3> Base;
-  Velocity3 v_;
-
   typedef OptionalJacobian<9, 9> ChartJacobian;
 
 public:
-  enum { dimension = 9 };
 
   // constructors - with partial versions
   PoseRTV() {}
-  PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel)
+  PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel)
-  : Rt_(rot, pt), v_(vel) {}
+  : Base(Pose3(rot, t), vel) {}
-  PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel)
+  PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
-  : Rt_(rot, pt), v_(vel) {}
+  : Base(Pose3(rot, t), vel) {}
-  explicit PoseRTV(const Point3& pt)
+  explicit PoseRTV(const Point3& t)
-  : Rt_(Rot3::identity(), pt) {}
+  : Base(Pose3(Rot3(), t),Velocity3()) {}
   PoseRTV(const Pose3& pose, const Velocity3& vel)
-  : Rt_(pose), v_(vel) {}
+  : Base(pose, vel) {}
   explicit PoseRTV(const Pose3& pose)
-  : Rt_(pose) {}
+  : Base(pose,Velocity3()) {}
+
+  // Construct from Base
+  PoseRTV(const Base& base)
+  : Base(base) {}
 
   /** build from components - useful for data files */
   PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
@@ -51,72 +52,46 @@ public:
   explicit PoseRTV(const Vector& v);
 
   // access
-  const Point3& t() const { return Rt_.translation(); }
+  const Pose3& pose() const { return first; }
-  const Rot3& R() const { return Rt_.rotation(); }
+  const Velocity3& v() const { return second; }
-  const Velocity3& v() const { return v_; }
+  const Point3& t() const { return pose().translation(); }
-  const Pose3& pose() const { return Rt_; }
+  const Rot3& R() const { return pose().rotation(); }
 
   // longer function names
-  const Point3& translation() const { return Rt_.translation(); }
+  const Point3& translation() const { return pose().translation(); }
-  const Rot3& rotation() const { return Rt_.rotation(); }
+  const Rot3& rotation() const { return pose().rotation(); }
-  const Velocity3& velocity() const { return v_; }
+  const Velocity3& velocity() const { return second; }
 
   // Access to vector for ease of use with Matlab
   // and avoidance of Point3
   Vector vector() const;
-  Vector translationVec() const { return Rt_.translation().vector(); }
+  Vector translationVec() const { return pose().translation().vector(); }
-  Vector velocityVec() const { return v_.vector(); }
+  Vector velocityVec() const { return velocity().vector(); }
 
   // testable
   bool equals(const PoseRTV& other, double tol=1e-6) const;
   void print(const std::string& s="") const;
 
-  // Manifold
+  /// @name Manifold
-  static size_t Dim() { return 9; }
+  /// @{
-  size_t dim() const { return Dim(); }
+  using Base::dimension;
+  using Base::dim;
+  using Base::Dim;
+  using Base::retract;
+  using Base::localCoordinates;
+  /// @}
 
-  /**
+  /// @name measurement functions
-   * retract/unretract assume independence of components
+  /// @{
-   * Tangent space parameterization:
-   *    - v(0-2): Rot3 (roll, pitch, yaw)
-   *    - v(3-5): Point3
-   *    - v(6-8): Translational velocity
-   */
-  PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const;
-  Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const;
 
-  // Lie TODO IS this a Lie group or just a Manifold????
+  /** range between translations */
-  /**
-   * expmap/logmap are poor approximations that assume independence of components
-   * Currently implemented using the poor retract/unretract approximations
-   */
-  static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none);
-  static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none);
-
-  static PoseRTV identity() { return PoseRTV(); }
-
-  /** Derivatives calculated numerically */
-  PoseRTV inverse(ChartJacobian H1=boost::none) const;
-
-  /** Derivatives calculated numerically */
-  PoseRTV compose(const PoseRTV& p,
-      ChartJacobian H1=boost::none,
-      ChartJacobian H2=boost::none) const;
-
-  PoseRTV operator*(const PoseRTV& p) const { return compose(p); }
-
-  /** Derivatives calculated numerically */
-  PoseRTV between(const PoseRTV& p,
-                  ChartJacobian H1=boost::none,
-                  ChartJacobian H2=boost::none) const;
-
-  // measurement functions
-  /** Derivatives calculated numerically */
   double range(const PoseRTV& other,
                OptionalJacobian<1,9> H1=boost::none,
                OptionalJacobian<1,9> H2=boost::none) const;
+  /// @}
 
-  // IMU-specific
+  /// @name IMU-specific
+  /// @{
 
   /// Dynamics integrator for ground robots
   /// Always move from time 1 to time 2
@@ -164,7 +139,9 @@ public:
       ChartJacobian Dglobal = boost::none,
       OptionalJacobian<9, 6> Dtrans = boost::none) const;
 
-  // Utility functions
+  /// @}
+  /// @name Utility functions
+  /// @{
 
   /// RRTMbn - Function computes the rotation rate transformation matrix from
   /// body axis rates to euler angle (global) rates
@@ -177,19 +154,20 @@ public:
   static Matrix RRTMnb(const Vector& euler);
 
   static Matrix RRTMnb(const Rot3& att);
+  /// @}
 
 private:
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int /*version*/) {
-    ar & BOOST_SERIALIZATION_NVP(Rt_);
+    ar & BOOST_SERIALIZATION_NVP(first);
-    ar & BOOST_SERIALIZATION_NVP(v_);
+    ar & BOOST_SERIALIZATION_NVP(second);
   }
 };
 
 
 template<>
-struct traits<PoseRTV> : public internal::LieGroupTraits<PoseRTV> {};
+struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
 
 } // \namespace gtsam

gtsam_unstable/dynamics/tests/testPoseRTV.cpp

@@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) {
 /* ************************************************************************* */
 TEST( testPoseRTV, Lie ) {
   // origin and zero deltas
-  EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol));
+  PoseRTV identity;
-  EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol));
+  EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol));
+  EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol));
 
   PoseRTV state1(pt, rot, vel);
-  EXPECT(assert_equal(state1, state1.retract(zero(9)), tol));
+  EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol));
   EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
 
   Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished();
@@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) {
   Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
   Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
   PoseRTV state2(pt2, rot2, vel2);
-  EXPECT(assert_equal(state2, state1.retract(delta), 1e-1));
+  EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
   EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
-  EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation
+  EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
 }
 
 /* ************************************************************************* */
@@ -119,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) {
 }
 
 
+/* ************************************************************************* */
+PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); }
+TEST( testPoseRTV, compose ) {
+  PoseRTV state1(pt, rot, vel), state2 = state1;
+
+  Matrix actH1, actH2;
+  state1.compose(state2, actH1, actH2);
+  Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
+  Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); }
+TEST( testPoseRTV, between ) {
+  PoseRTV state1(pt, rot, vel), state2 = state1;
+
+  Matrix actH1, actH2;
+  state1.between(state2, actH1, actH2);
+  Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
+  Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); }
+TEST( testPoseRTV, inverse ) {
+  PoseRTV state1(pt, rot, vel);
+
+  Matrix actH1;
+  state1.inverse(actH1);
+  Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+}
+
 /* ************************************************************************* */
 double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
 TEST( testPoseRTV, range ) {

gtsam_unstable/geometry/Similarity3.h

@@ -148,5 +148,5 @@ public:
 };
 
 template<>
-struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
+struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
 }

tests/testLie.cpp

@@ -0,0 +1,111 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------1------------------------------------------- */
+
+/**
+ * @file testLie.cpp
+ * @date May, 2015
+ * @author Frank Dellaert
+ * @brief unit tests for Lie group type machinery
+ */
+
+#include <gtsam/base/ProductLieGroup.h>
+
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/base/testLie.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+static const double tol = 1e-9;
+
+//******************************************************************************
+typedef ProductLieGroup<Point2, Pose2> Product;
+
+// Define any direct product group to be a model of the multiplicative Group concept
+namespace gtsam {
+template<> struct traits<Product> : internal::LieGroupTraits<Product> {
+  static void Print(const Product& m, const string& s = "") {
+    cout << s << "(" << m.first << "," << m.second.translation() << "/"
+        << m.second.theta() << ")" << endl;
+  }
+  static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) {
+    return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol);
+  }
+};
+}
+
+//******************************************************************************
+TEST(Lie, ProductLieGroup) {
+  BOOST_CONCEPT_ASSERT((IsGroup<Product>));
+  BOOST_CONCEPT_ASSERT((IsManifold<Product>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<Product>));
+  Product pair1;
+  Vector5 d;
+  d << 1, 2, 0.1, 0.2, 0.3;
+  Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
+  Product pair2 = pair1.retract(d);
+  EXPECT(assert_equal(expected, pair2, 1e-9));
+  EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
+}
+
+/* ************************************************************************* */
+Product compose_proxy(const Product& A, const Product& B) {
+  return A.compose(B);
+}
+TEST( testProduct, compose ) {
+  Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
+
+  Matrix actH1, actH2;
+  state1.compose(state2, actH1, actH2);
+  Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
+  Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+Product between_proxy(const Product& A, const Product& B) {
+  return A.between(B);
+}
+TEST( testProduct, between ) {
+  Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
+
+  Matrix actH1, actH2;
+  state1.between(state2, actH1, actH2);
+  Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
+  Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+  EXPECT(assert_equal(numericH2, actH2, tol));
+}
+
+/* ************************************************************************* */
+Product inverse_proxy(const Product& A) {
+  return A.inverse();
+}
+TEST( testProduct, inverse ) {
+  Product state1(Point2(1, 2), Pose2(3, 4, 5));
+
+  Matrix actH1;
+  state1.inverse(actH1);
+  Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
+  EXPECT(assert_equal(numericH1, actH1, tol));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+

tests/testManifold.cpp

@@ -10,13 +10,14 @@
  * -------------------------------1------------------------------------------- */
 
 /**
- * @file testExpression.cpp
+ * @file testManifold.cpp
  * @date September 18, 2014
  * @author Frank Dellaert
  * @author Paul Furgale
- * @brief unit tests for Block Automatic Differentiation
+ * @brief unit tests for Manifold type machinery
  */
 
+#include <gtsam/base/Manifold.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
@@ -148,6 +149,32 @@ TEST(Manifold, DefaultChart) {
   EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
 }
 
+//******************************************************************************
+typedef ProductManifold<Point2,Point2> MyPoint2Pair;
+
+// Define any direct product group to be a model of the multiplicative Group concept
+namespace gtsam {
+template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair> {
+  static void Print(const MyPoint2Pair& m, const string& s = "") {
+    cout << s << "(" << m.first << "," << m.second << ")" << endl;
+  }
+  static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) {
+    return m1 == m2;
+  }
+};
+}
+
+TEST(Manifold, ProductManifold) {
+  BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
+  MyPoint2Pair pair1;
+  Vector4 d;
+  d << 1,2,3,4;
+  MyPoint2Pair expected(Point2(1,2),Point2(3,4));
+  MyPoint2Pair pair2 = pair1.retract(d);
+  EXPECT(assert_equal(expected,pair2,1e-9));
+  EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9));
+}
+
 //******************************************************************************
 int main() {
   TestResult tr;