Point3 and Rot3 dexpInvL

gtsam/geometry/Point3.h

@@ -148,6 +148,11 @@ namespace gtsam {
       return eye(3);
     }
 
+    /// Left-trivialized derivative inverse of the exponential map
+    static Matrix dexpInvL(const Vector& v) {
+      return eye(3);
+    }
+
     /// @}
     /// @name Vector Space
     /// @{

gtsam/geometry/Rot3.h

@@ -271,6 +271,9 @@ namespace gtsam {
     /// 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 Point3
     /// @{

gtsam/geometry/Rot3M.cpp

@@ -316,6 +316,19 @@ Matrix3 Rot3::dexpL(const Vector3& v) {
   return res;
 }
 
+/* ************************************************************************* */
+/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
+Matrix3 Rot3::dexpInvL(const Vector3& v) {
+  if(zero(v)) return eye(3);
+  Matrix x = skewSymmetric(v);
+  Matrix x2 = x*x;
+  double theta = v.norm(), vi = theta/2.0;
+  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
+  Matrix res = eye(3) + 0.5*x - s2*x2;
+  return res;
+}
+
+
 /* ************************************************************************* */
 Matrix3 Rot3::matrix() const {
   return rot_;

gtsam/geometry/tests/testRot3M.cpp

@@ -419,6 +419,9 @@ TEST( Rot3, dexpL) {
       boost::function<Vector(const LieVector&)>(
           boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
   EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
+
+  Matrix actualDexpInvL = Rot3::dexpInvL(w);
+  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
 }
 
 /* ************************************************************************* */