test Product

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 internal::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,11 @@ struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
   }
 };
 
+/// Implement both manifold and testable traits at the same time
+template<class Class>
+struct Manifold: Testable<Class>, ManifoldTraits<Class> {
+};
+
 } // \ namespace internal
 
 /// Check invariants for Manifold type
@@ -173,33 +178,37 @@ class ProductManifold: public std::pair<M1, M2> {
   BOOST_CONCEPT_ASSERT((IsManifold<M2>));
 
 private:
-  const M1& g() const {return this->first;}
+  enum { dimension1 = traits<M1>::dimension };
-  const M2& h() const {return this->second;}
+  enum { dimension2 = traits<M2>::dimension };
 
 public:
-  enum { dimension = M1::dimension + M2::dimension };
+  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 subgroup elements
+  // Construct from two original manifold values
-  ProductManifold(const M1& g, const M2& h):std::pair<M1,M2>(g,h) {}
+  ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
 
   /// Retract delta to manifold
   Derived retract(const TangentVector& xi) const {
-    return Derived(traits<M1>::Retract(g(),xi.head(M1::dimension)),
+    M1 m1 = traits<M1>::Retract(this->first,  xi.template head<dimension1>());
-        traits<M2>::Retract(h(),xi.tail(M2::dimension)));
+    M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
+    return Derived(m1,m2);
   }
 
   /// Compute the coordinates in the tangent space
   TangentVector localCoordinates(const Derived& other) const {
-    TangentVector xi;
+    typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first,  other.first);
-    xi << traits<M1>::Local(g(),other.g()), traits<M2>::Local(h(),other.h());
+    typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
-    return xi;
+    TangentVector v;
+    v << v1, v2;
+    return v;
   }
 };
 

tests/testManifold.cpp

@@ -148,6 +148,37 @@ TEST(Manifold, DefaultChart) {
   EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
 }
 
+//******************************************************************************
+struct MyPoint2Pair : public ProductManifold<MyPoint2Pair,Point2,Point2> {
+  typedef ProductManifold<MyPoint2Pair,Point2,Point2> Base;
+  MyPoint2Pair(const Point2& p1, const Point2& p2):Base(p1,p2) {}
+  MyPoint2Pair(const Base& base):Base(base) {}
+  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;