diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index 901960001..9dadaf9e6 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -114,28 +114,15 @@ template<int M, int N, int Options>
 struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
 };
 
-// TODO: Could be more sophisticated using Eigen traits and SFINAE?
-
-template<int Options>
-struct dimension<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Options> > : public Dynamic {
-};
-
-template<int M, int Options>
-struct dimension<Eigen::Matrix<double, M, Eigen::Dynamic, Options> > : public Dynamic {
-};
-
-template<int N, int Options>
-struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dynamic {
+template<int M, int N, int Options>
+struct dimension<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<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
 };
 
 template<int M, int N, int Options>
-struct dimension<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<
-    int, M * N> {
-};
-
-template<int M, int N, int Options>
-struct zero<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<
-    int, M * N> {
+struct zero<Eigen::Matrix<double, M, N, Options> > {
   BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
       "traits::zero is only supported for fixed-size matrices");
   static Eigen::Matrix<double, M, N, Options> value() {
@@ -143,6 +130,11 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public boost::integral_cons
   }
 };
 
+template<int M, int N, int 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 {
 };
 
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index dd17c00ef..1469e265d 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -667,7 +667,7 @@ Matrix expm(const Matrix& A, size_t K) {
 
 /* ************************************************************************* */
 Matrix Cayley(const Matrix& A) {
-  size_t n = A.cols();
+  Matrix::Index n = A.cols();
   assert(A.rows() == n);
 
   // original
diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp
index 55a5f5af0..b7ebfc4c8 100644
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@@ -108,6 +108,20 @@ TEST(Manifold, _zero) {
   Cal3Bundler cal(0, 0, 0);
   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_DOUBLES_EQUAL(0.0, traits::zero<double>::value(), 0.0);
+}
+
+/* ************************************************************************* */
+// identity
+TEST(Manifold, _identity) {
+  EXPECT(assert_equal(Pose3(), traits::identity<Pose3>::value()));
+  EXPECT(assert_equal(Cal3Bundler(), traits::identity<Cal3Bundler>::value()));
+  EXPECT(assert_equal(Camera(), traits::identity<Camera>::value()));
+  EXPECT(assert_equal(Point2(), traits::identity<Point2>::value()));
+  EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity<Matrix24>::value())));
+  EXPECT_DOUBLES_EQUAL(0.0, traits::identity<double>::value(), 0.0);
 }
 
 /* ************************************************************************* */