diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h
index 0b3f00f7f..67cdfe815 100644
--- a/gtsam/base/LieMatrix.h
+++ b/gtsam/base/LieMatrix.h
@@ -149,10 +149,14 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
     throw std::runtime_error("LieMatrix::Expmap(): Don't use this function");
     return LieMatrix(v); }
 
-  /** Logmap around identity - just returns with default cast back */
+  /** Logmap around identity */
   static inline Vector Logmap(const LieMatrix& p) {
-    return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
-
+    Vector result(p.size());
+    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
+        result.data(), p.rows(), p.cols()) = p;
+    return result;
+  }
+	
   /// @}
 
 private:
diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp
index 5ef867e85..7e5356fc0 100644
--- a/gtsam/base/tests/testLieMatrix.cpp
+++ b/gtsam/base/tests/testLieMatrix.cpp
@@ -57,10 +57,14 @@ TEST(LieMatrix, retract) {
 
   EXPECT(assert_equal(expected, actual));
 
-  LieMatrix expectedUpdate = update;
-  LieMatrix actualUpdate = init.localCoordinates(actual);
+  Vector expectedUpdate = update;
+  Vector actualUpdate = init.localCoordinates(actual);
 
   EXPECT(assert_equal(expectedUpdate, actualUpdate));
+
+  Vector expectedLogmap = (Vec() << 1, 2, 3, 4);
+  Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0));
+  EXPECT(assert_equal(expectedLogmap, actualLogmap));
 }
 
 /* ************************************************************************* */