dexp and dexpInv for Point2 and Rot2

gtsam/geometry/Point2.h

@@ -174,6 +174,16 @@ public:
   /// 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()); }
 
+  /// Left-trivialized derivative of the exponential map
+  static Matrix dexpL(const Vector2& v) {
+    return eye(2);
+  }
+
+  /// Left-trivialized derivative inverse of the exponential map
+  static Matrix dexpInvL(const Vector2& v) {
+    return eye(2);
+  }
+
   /// @}
   /// @name Vector Space
   /// @{

gtsam/geometry/Rot2.h

@@ -173,6 +173,16 @@ namespace gtsam {
       return (Vector(1) << r.theta());
     }
 
+    /// Left-trivialized derivative of the exponential map
+    static Matrix dexpL(const Vector& v) {
+      return ones(1);
+    }
+
+    /// Left-trivialized derivative inverse of the exponential map
+    static Matrix dexpInvL(const Vector& v) {
+      return ones(1);
+    }
+
     /// @}
     /// @name Group Action on Point2
     /// @{

gtsam/geometry/tests/testRot2.cpp

@@ -153,6 +153,28 @@ TEST( Rot2, relativeBearing )
   CHECK(assert_equal(expectedH,actualH));
 }
 
+/* ************************************************************************* */
+Vector w = (Vector(1) << 0.27);
+
+// 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 = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw));
+  return y;
+}
+
+TEST( Rot2, dexpL) {
+  Matrix actualDexpL = Rot2::dexpL(w);
+  Matrix expectedDexpL = numericalDerivative11(
+      boost::function<Vector(const LieVector&)>(
+          boost::bind(testDexpL, _1)), LieVector(zero(1)), 1e-2);
+  EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
+
+  Matrix actualDexpInvL = Rot2::dexpInvL(w);
+  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;