From 4be24f4f700c3c82efba71136cb064f3fae56e59 Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Fri, 17 Oct 2014 13:30:57 -0400
Subject: [PATCH] add adjointMap and expmap/logmap derivatives for Pose2

---
 gtsam/geometry/Pose2.cpp           | 55 ++++++++++++++++++++++++++++++
 gtsam/geometry/Pose2.h             | 12 +++++++
 gtsam/geometry/tests/testPose2.cpp | 24 ++++++++++++-
 3 files changed, 90 insertions(+), 1 deletion(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index 77a635453..352347d43 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -125,6 +125,61 @@ Vector Pose2::Adjoint(const Vector& xi) const {
   return AdjointMap()*xi;
 }
 
+/* ************************************************************************* */
+Matrix3 Pose2::adjointMap(const Vector& v) {
+  // See Chirikjian12book2, vol.2, pg. 36
+  Matrix3 ad = zeros(3,3);
+  ad(0,1) = -v[2];
+  ad(1,0) = v[2];
+  ad(0,2) = v[1];
+  ad(1,2) = -v[0];
+  return ad;
+}
+
+/* ************************************************************************* */
+Matrix3 Pose2::dexpL(const Vector3& v) {
+  // See Iserles05an, pg. 33.
+  // TODO: Duplicated code. Maybe unify them at higher Lie level?
+  static const int N = 10; // order of approximation
+  Matrix res = I3;
+  Matrix3 ad_i = I3;
+  Matrix3 ad = adjointMap(v);
+  double fac = 1.0;
+  for (int i = 1; i < N; ++i) {
+    ad_i = ad * ad_i;
+    fac = fac * (i+1);
+    // Since this is the left-trivialized version, we flip the signs of the odd terms
+    if (i%2 != 0)
+      res = res - 1.0 / fac * ad_i;
+    else
+      res = res + 1.0 / fac * ad_i;
+  }
+  return res;
+}
+
+/* ************************************************************************* */
+Matrix3 Pose2::dexpInvL(const Vector3& v) {
+  // TODO: Duplicated code with Pose3. Maybe unify them at higher Lie level?
+  // Bernoulli numbers, from Wikipedia
+  static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
+      0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
+  static const int N = 5; // order of approximation
+  Matrix res = I3;
+  Matrix3 ad_i = I3;
+  Matrix3 ad = adjointMap(v);
+  double fac = 1.0;
+  for (int i = 1; i < N; ++i) {
+    ad_i = ad * ad_i;
+    fac = fac * i;
+    // Since this is the left-trivialized version, we flip the signs of the odd terms
+    // Note that all Bernoulli odd numbers are 0, except 1.
+    if (i==1)
+      res = res - B(i) / fac * ad_i;
+    else
+      res = res + B(i) / fac * ad_i;
+  }
+  return res;
+}
 
 /* ************************************************************************* */
 Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 6cf3c91fc..d3d6d2695 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -161,6 +161,11 @@ public:
   Matrix AdjointMap() const;
   Vector Adjoint(const Vector& xi) const;
 
+  /**
+   * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
+   */
+  static Matrix3 adjointMap(const Vector& v);
+
   /**
    * wedge for SE(2):
    * @param xi 3-dim twist (v,omega) where
@@ -175,6 +180,13 @@ public:
         0., 0.,  0.);
   }
 
+  /// Left-trivialized derivative of the exponential map
+  static Matrix3 dexpL(const Vector3& v);
+
+  /// Left-trivialized derivative inverse of the exponential map
+  static Matrix3 dexpInvL(const Vector3& v);
+
+
   /// @}
   /// @name Group Action on Point2
   /// @{
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index 42b548f5a..77148f431 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -32,7 +32,7 @@ using namespace boost::assign;
 using namespace gtsam;
 using namespace std;
 
-// #define SLOW_BUT_CORRECT_EXPMAP
+#define SLOW_BUT_CORRECT_EXPMAP
 
 GTSAM_CONCEPT_TESTABLE_INST(Pose2)
 GTSAM_CONCEPT_LIE_INST(Pose2)
@@ -192,6 +192,28 @@ TEST(Pose2, logmap_full) {
   EXPECT(assert_equal(expected, actual, 1e-5));
 }
 
+/* ************************************************************************* */
+Vector w = (Vector(3) << 0.1, 0.27, -0.2);
+
+// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
+// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
+// => y = log (exp(-w) * exp(w+dw))
+Vector testDexpL(const LieVector& dw) {
+  Vector y = Pose2::Logmap(Pose2::Expmap(-w) * Pose2::Expmap(w + dw));
+  return y;
+}
+
+TEST( Pose2, dexpL) {
+  Matrix actualDexpL = Pose2::dexpL(w);
+  Matrix expectedDexpL = numericalDerivative11(
+      boost::function<Vector(const LieVector&)>(
+          boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
+  EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
+
+  Matrix actualDexpInvL = Pose2::dexpInvL(w);
+  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
+}
+
 /* ************************************************************************* */
 static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
   return pose.transform_to(point);