Merged in fix/BAD_cleanerEigenMatrixDefaultChart (pull request #33)

cleaned up and optimized a bit the Eigen matrices' DefaultChart

gtsam/base/Manifold.h

@@ -244,19 +244,19 @@ struct DefaultChart<double> {
 
 template<int M, int N, int Options>
 struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
+  /**
+   * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options).
+   * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
+   */
   typedef Eigen::Matrix<double, M, N, Options> type;
   typedef type T;
   typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
       "DefaultChart has not been implemented yet for dynamically sized matrices");
   static vector local(const T& origin, const T& other) {
-    T diff = other - origin;
-    Eigen::Map<vector> map(diff.data());
-    return vector(map);
-    // Why is this function not : return other - origin; ?? what is the Eigen::Map used for?
+    return reshape<vector::RowsAtCompileTime, 1>(other) - reshape<vector::RowsAtCompileTime, 1>(origin);
   }
   static T retract(const T& origin, const vector& d) {
-    Eigen::Map<const T> map(d.data());
-    return origin + map;
+    return origin + reshape<M, N>(d);
   }
   static int getDimension(const T&origin) {
     return origin.rows() * origin.cols();

gtsam/base/Matrix.h

@@ -293,6 +293,49 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
  */
 inline Matrix trans(const Matrix& A) { return A.transpose(); }
 
+/// Reshape functor
+template <int OutM, int OutN, int InM, int InN, int InOptions>
+struct Reshape {
+  //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
+  typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN> > ReshapedType;
+  static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
+    return in.data();
+  }
+};
+
+/// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output)
+template <int M, int InOptions>
+struct Reshape<M, M, M, M, InOptions> {
+  typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
+  static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
+    return in;
+  }
+};
+
+/// Reshape specialization that does nothing as shape stays the same
+template <int M, int N, int InOptions>
+struct Reshape<M, N, M, N, InOptions> {
+  typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
+  static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
+    return in;
+  }
+};
+
+/// Reshape specialization that does transpose
+template <int M, int N, int InOptions>
+struct Reshape<N, M, M, N, InOptions> {
+  typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
+  static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
+    return in.transpose();
+  }
+};
+
+template <int OutM, int OutN, int InM, int InN, int InOptions>
+inline typename Reshape<OutM, OutN, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
+  BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
+  return Reshape<OutM, OutN, InM, InN, InOptions>::reshape(m);
+}
+
 /**
  * solve AX=B via in-place Lu factorization and backsubstitution
  * After calling, A contains LU, B the solved RHS vectors

tests/testManifold.cpp

@@ -76,6 +76,35 @@ TEST(Manifold, DefaultChart) {
   EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0))));
   EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0));
 
+  {
+    typedef Matrix2 ManifoldPoint;
+    ManifoldPoint m;
+    DefaultChart<ManifoldPoint> chart;
+    m << 1, 3,
+         2, 4;
+    // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
+    EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m))));
+    EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
+  }
+
+  {
+    typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
+    ManifoldPoint m;
+    DefaultChart<ManifoldPoint> chart;
+    m << 1, 2;
+    EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m))));
+    EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m);
+  }
+
+  {
+    typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
+    ManifoldPoint m;
+    DefaultChart<ManifoldPoint> chart;
+    m << 1;
+    EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m))));
+    EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m);
+  }
+
   DefaultChart<double> chart3;
   Vector v1(1);
   v1 << 1;
@@ -109,7 +138,7 @@ TEST(Manifold, _zero) {
   EXPECT(assert_equal(cal, traits::zero<Cal3Bundler>::value()));
   EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero<Camera>::value()));
   EXPECT(assert_equal(Point2(), traits::zero<Point2>::value()));
-  EXPECT(assert_equal(Matrix(Matrix24::Zero().eval()), Matrix(traits::zero<Matrix24>::value())));
+  EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero<Matrix24>::value())));
   EXPECT_DOUBLES_EQUAL(0.0, traits::zero<double>::value(), 0.0);
 }