From fed2c8b6849205598ea97c4826260ac85ba8eabb Mon Sep 17 00:00:00 2001
From: HannesSommer <hannes.sommer@mavt.ethz.ch>
Date: Mon, 10 Nov 2014 16:35:23 +0100
Subject: [PATCH] added missing square matrix specialization - without it,
 square to square cases would be ambiguous.

---
 gtsam/base/Matrix.h    | 9 +++++++++
 tests/testManifold.cpp | 9 +++++++++
 2 files changed, 18 insertions(+)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index eaa57c810..00caed44a 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -303,6 +303,15 @@ struct Reshape {
   }
 };
 
+/// 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> {
diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp
index 6e720757a..32f04225f 100644
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@@ -96,6 +96,15 @@ TEST(Manifold, DefaultChart) {
     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;