From 505c8765885a1952536034124dd3415f2ed2a20d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 21 Dec 2014 15:57:21 +0100
Subject: [PATCH] Small changes, fixed matrices

---
 gtsam/geometry/Point2.h             | 26 +++++++++++---------------
 gtsam/geometry/tests/testPoint2.cpp |  4 ++++
 2 files changed, 15 insertions(+), 15 deletions(-)

diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 8c60aa4d6..4a7809cad 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -53,9 +53,7 @@ public:
   /// @{
 
   /// construct from 2D vector
-  Point2(const Vector& v) {
-    if(v.size() != 2)
-      throw std::invalid_argument("Point2 constructor from Vector requires that the Vector have dimension 2");
+  Point2(const Vector2& v) {
     x_ = v(0);
     y_ = v(1);
   }
@@ -178,30 +176,28 @@ public:
   /// @{
 
   /// Exponential map around identity - just create a Point2 from a vector
-  static inline Point2 Expmap(const Vector& v) { return Point2(v); }
-  static Point2 Expmap(const Vector& v, OptionalJacobian<2,2> H) {
+  static inline Point2 Expmap(const Vector2& v) { return Point2(v); }
+  static Point2 Expmap(const Vector2& v, OptionalJacobian<2,2> H) {
     CONCEPT_NOT_IMPLEMENTED;
   }
 
-  /// Log map around identity - just return the Point2 as a vector
-  static inline Vector Logmap(const Point2& dp) {
-  	return (Vector(2) << dp.x(), dp.y()).finished();
+  /// Logmap around identity
+  static inline Vector2 Logmap(const Point2& dp) {
+    return Vector2(dp.x(), dp.y());
   }
 
-  /// Version with Jacobian
-  static Vector Logmap(const Point2& dp, OptionalJacobian<2,2> H) {
+  static inline Vector2 Logmap(const Point2& dp, OptionalJacobian<2,2> H) {
     CONCEPT_NOT_IMPLEMENTED;
-    return (Vector(2) << dp.x(), dp.y()).finished();
   }
 
   /// Left-trivialized derivative of the exponential map
-  static Matrix dexpL(const Vector2& v) {
-    return eye(2);
+  static Matrix2 dexpL(const Vector2& v) {
+    return I_2x2;
   }
 
   /// Left-trivialized derivative inverse of the exponential map
-  static Matrix dexpInvL(const Vector2& v) {
-    return eye(2);
+  static Matrix2 dexpInvL(const Vector2& v) {
+    return I_2x2;
   }
 
   /// @}
diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp
index ab40928ab..ef72ce988 100644
--- a/gtsam/geometry/tests/testPoint2.cpp
+++ b/gtsam/geometry/tests/testPoint2.cpp
@@ -108,6 +108,10 @@ TEST( Point2, norm ) {
   EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
   expectedH = numericalDerivative11(norm_proxy, x2);
   EXPECT(assert_equal(expectedH,actualH));
+
+  // analytical
+  expectedH = (Matrix(1, 2) << x2.x()/actual, x2.y()/actual).finished();
+  EXPECT(assert_equal(expectedH,actualH));
 }
 
 /* ************************************************************************* */