From 625ebca72a1759f12410f854a92d6aa916441b3c Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 26 Dec 2014 00:40:08 +0100
Subject: [PATCH] Lucky guess fix of decoupled derivatives

---
 gtsam/geometry/Pose2.cpp | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 9416ada8b..0b0172857 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -96,7 +96,10 @@ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
 #ifdef SLOW_BUT_CORRECT_EXPMAP
   return Expmap(v, H);
 #else
-  if (H) *H = Matrix3::Identity();
+  if (H) {
+    *H = I_3x3;
+    H->topLeftCorner<2,2>() = Rot2(-v[2]).matrix();
+  }
   return Pose2(v[0], v[1], v[2]);
 #endif
 }
@@ -105,7 +108,10 @@ Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
 #ifdef SLOW_BUT_CORRECT_EXPMAP
   return Logmap(r, H);
 #else
-  if (H) *H = Matrix3::Identity();
+  if (H) {
+    *H = I_3x3;
+    H->topLeftCorner<2,2>() = r.rotation().matrix();
+  }
   return Vector3(r.x(), r.y(), r.theta());
 #endif
 }