Now dynamically sized matrices live in manifolds, too.

gtsam/base/Manifold.h

@@ -73,7 +73,6 @@ template<typename T>
 struct dimension: public Dynamic {
 };
 
-
 /**
  * zero<T>::value is intended to be the origin of a canonical coordinate system
  * with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
@@ -111,12 +110,14 @@ struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : publi
 };
 
 template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type{
+struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
 };
 
 template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<int,
+struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
-    M == Eigen::Dynamic ? Eigen::Dynamic : (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
+    int,
+    M == Eigen::Dynamic ? Eigen::Dynamic :
+        (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
   //TODO after switch to c++11 : the above should should be extracted to a constexpr function
   // for readability and to reduce code duplication
 };
@@ -131,10 +132,10 @@ struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
 };
 
 template<int M, int N, int Options>
-struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<Eigen::Matrix<double, M, N, Options> > {
+struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
+    Eigen::Matrix<double, M, N, Options> > {
 };
 
-
 template<typename T> struct is_chart: public boost::false_type {
 };
 
@@ -250,10 +251,14 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
    */
   typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
   typedef type T;
-  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
+  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
-      "DefaultChart has not been implemented yet for dynamically sized matrices");
+
+  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
+      "Internal error: DefaultChart for Dynamic should be handled by template below");
+
   static vector local(const T& origin, const T& other) {
-    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
+    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
+        - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
   }
   static T retract(const T& origin, const vector& d) {
     return origin + reshape<M, N, Options>(d);
@@ -266,20 +271,36 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
 // Dynamically sized Vector
 template<>
 struct DefaultChart<Vector> {
-  typedef Vector T;
+  typedef Vector type;
-  typedef T type;
+  typedef Vector vector;
-  typedef T vector;
+  static vector local(const Vector& origin, const Vector& other) {
-  static vector local(const T& origin, const T& other) {
     return other - origin;
   }
-  static T retract(const T& origin, const vector& d) {
+  static Vector retract(const Vector& origin, const vector& d) {
     return origin + d;
   }
-  static int getDimension(const T& origin) {
+  static int getDimension(const Vector& origin) {
     return origin.size();
   }
 };
 
+// Dynamically sized Matrix
+template<>
+struct DefaultChart<Matrix> {
+  typedef Matrix type;
+  typedef Vector vector;
+  static vector local(const Matrix& origin, const Matrix& other) {
+    Matrix d = other - origin;
+    return Eigen::Map<Vector>(d.data(),getDimension(d));
+  }
+  static Matrix retract(const Matrix& origin, const vector& d) {
+    return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
+  }
+  static int getDimension(const Matrix& m) {
+    return m.size();
+  }
+};
+
 /**
  * Old Concept check class for Manifold types
  * Requires a mapping between a linear tangent space and the underlying

gtsam/nonlinear/tests/testValues.cpp

@@ -12,6 +12,8 @@
 /**
  * @file testValues.cpp
  * @author Richard Roberts
+ * @author Frank Dellaert
+ * @author Mike Bosse
  */
 
 #include <gtsam/nonlinear/Values.h>
@@ -168,9 +170,9 @@ TEST(Values, basic_functions)
   Values values;
   const Values& values_c = values;
   values.insert(2, Vector3());
-  values.insert(4, Vector3());
+  values.insert(4, Vector(3));
-  values.insert(6, Vector3());
+  values.insert(6, Matrix23());
-  values.insert(8, Vector3());
+  values.insert(8, Matrix(2,3));
 
   // find
   EXPECT_LONGS_EQUAL(4, values.find(4)->key);