From ecb896ef03997ca09a464a0a2ba9681311664a6e Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Tue, 5 Aug 2014 21:56:36 -0400
Subject: [PATCH] move Adjoint to cpp and enable EXP_MAP

---
 gtsam/geometry/Pose2.cpp | 9 +++++++++
 gtsam/geometry/Pose2.h   | 5 +----
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 85307e322..77a635453 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -33,6 +33,8 @@ INSTANTIATE_LIE(Pose2);
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose2);
 
+#define SLOW_BUT_CORRECT_EXPMAP
+
 static const Matrix I3 = eye(3), Z12 = zeros(1,2);
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
@@ -117,6 +119,13 @@ Matrix Pose2::AdjointMap() const {
   );
 }
 
+/* ************************************************************************* */
+Vector Pose2::Adjoint(const Vector& xi) const {
+  assert(xi.size() == 3);
+  return AdjointMap()*xi;
+}
+
+
 /* ************************************************************************* */
 Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
   if (H1) *H1 = -AdjointMap();
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 26244877b..6cf3c91fc 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -159,10 +159,7 @@ public:
    * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
    */
   Matrix AdjointMap() const;
-  inline Vector Adjoint(const Vector& xi) const {
-    assert(xi.size() == 3);
-    return AdjointMap()*xi;
-  }
+  Vector Adjoint(const Vector& xi) const;
 
   /**
    * wedge for SE(2):