Renamed all dexpL/dexpInvL, merged Luca/Duy versions in Rot3

gtsam/base/LieScalar.h

@@ -107,12 +107,12 @@ namespace gtsam {
     static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); }
 
     /// Left-trivialized derivative of the exponential map
-    static Matrix dexpL(const Vector& v) {
+    static Matrix ExpmapDerivative(const Vector& v) {
       return eye(1);
     }
 
     /// Left-trivialized derivative inverse of the exponential map
-    static Matrix dexpInvL(const Vector& v) {
+    static Matrix LogmapDerivative(const Vector& v) {
       return eye(1);
     }
 

gtsam/geometry/Point2.h

@@ -174,14 +174,10 @@ public:
   static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); }
 
   /// Left-trivialized derivative of the exponential map
-  static Matrix dexpL(const Vector2& v) {
+  static Matrix ExpmapDerivative(const Vector2& v) {return I_2x2;}
-    return eye(2);
-  }
 
   /// Left-trivialized derivative inverse of the exponential map
-  static Matrix dexpInvL(const Vector2& v) {
+  static Matrix LogmapDerivative(const Vector2& v) { return I_2x2;}
-    return eye(2);
-  }
 
   /// @}
   /// @name Vector Space

gtsam/geometry/Point3.h

@@ -142,12 +142,12 @@ namespace gtsam {
     static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
 
     /// Left-trivialized derivative of the exponential map
-    static Matrix3 dexpL(const Vector& v) {
+    static Matrix3 ExpmapDerivative(const Vector& v) {
       return I_3x3;
     }
 
     /// Left-trivialized derivative inverse of the exponential map
-    static Matrix3 dexpInvL(const Vector& v) {
+    static Matrix3 LogmapDerivative(const Vector& v) {
       return I_3x3;
     }
 

gtsam/geometry/Pose2.cpp

@@ -135,7 +135,7 @@ Matrix3 Pose2::adjointMap(const Vector& v) {
 }
 
 /* ************************************************************************* */
-Matrix3 Pose2::dexpL(const Vector3& v) {
+Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
   double alpha = v[2];
   if (fabs(alpha) > 1e-5) {
     // Chirikjian11book2, pg. 36
@@ -145,7 +145,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) {
      *    \dot{g} g^{-1} = dexpR_{q}\dot{q}
      * where q = A, and g = exp(A)
      * and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26.
-     * Hence, to compute dexpL, we have to use the formula of J_r Chirikjian11book2, pg.36
+     * Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36
      */
     double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
     double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
@@ -164,7 +164,7 @@ Matrix3 Pose2::dexpL(const Vector3& v) {
 }
 
 /* ************************************************************************* */
-Matrix3 Pose2::dexpInvL(const Vector3& v) {
+Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
   double alpha = v[2];
   if (fabs(alpha) > 1e-5) {
     double alphaInv = 1/alpha;

gtsam/geometry/Pose2.h

@@ -182,10 +182,10 @@ public:
   }
 
   /// Left-trivialized derivative of the exponential map
-  static Matrix3 dexpL(const Vector3& v);
+  static Matrix3 ExpmapDerivative(const Vector3& v);
 
   /// Left-trivialized derivative inverse of the exponential map
-  static Matrix3 dexpInvL(const Vector3& v);
+  static Matrix3 LogmapDerivative(const Vector3& v);
 
 
   /// @}

gtsam/geometry/Rot2.h

@@ -176,12 +176,12 @@ namespace gtsam {
     }
 
     /// Left-trivialized derivative of the exponential map
-    static Matrix dexpL(const Vector& v) {
+    static Matrix ExpmapDerivative(const Vector& v) {
       return ones(1);
     }
 
     /// Left-trivialized derivative inverse of the exponential map
-    static Matrix dexpInvL(const Vector& v) {
+    static Matrix LogmapDerivative(const Vector& v) {
       return ones(1);
     }
 

gtsam/geometry/Rot3.cpp

@@ -107,32 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
   return q;
 }
 
-/* ************************************************************************* */
-/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
-Matrix3 Rot3::dexpL(const Vector3& v) {
-  if(zero(v)) return eye(3);
-  Matrix3 x = skewSymmetric(v);
-  Matrix3 x2 = x*x;
-  double theta = v.norm(), vi = theta/2.0;
-  double s1 = sin(vi)/vi;
-  double s2 = (theta - sin(theta))/(theta*theta*theta);
-  Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2;
-  return res;
-}
-
-/* ************************************************************************* */
-/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
-Matrix3 Rot3::dexpInvL(const Vector3& v) {
-  if(zero(v)) return eye(3);
-  Matrix3 x = skewSymmetric(v);
-  Matrix3 x2 = x*x;
-  double theta = v.norm(), vi = theta/2.0;
-  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
-  Matrix3 res = I_3x3 + 0.5*x - s2*x2;
-  return res;
-}
-
-
 /* ************************************************************************* */
 Point3 Rot3::column(int index) const{
   if(index == 3)
@@ -176,35 +150,56 @@ Vector Rot3::quaternion() const {
 
 /* ************************************************************************* */
 Matrix3 Rot3::ExpmapDerivative(const Vector3& x)    {
-  // x is the axis-angle representation (exponential coordinates) for a rotation
+  if(zero(x)) return I_3x3;
-  double normx = norm_2(x); // rotation angle
+  double theta = x.norm();  // rotation angle
-  Matrix3 Jr;
+#ifdef DUY_VERSION
-  if (normx < 10e-8){
+  /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
-    Jr = I_3x3;
+  Matrix3 X = skewSymmetric(x);
-  }
+  Matrix3 X2 = X*X;
-  else{
+  double vi = theta/2.0;
-    const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx
+  double s1 = sin(vi)/vi;
-    Jr = I_3x3 - ((1-cos(normx))/(normx)) * X +
+  double s2 = (theta - sin(theta))/(theta*theta*theta);
-        (1 -sin(normx)/normx) * X * X; // right Jacobian
+  return I_3x3 - 0.5*s1*s1*X + s2*X2;
-  }
+#else // Luca's version
-  return Jr;
+  /**
+   * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
+   * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+   * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
+   * where Jr = ExpmapDerivative(thetahat);
+   * This maps a perturbation in the tangent space (omega) to
+   * a perturbation on the manifold (expmap(Jr * omega))
+   */
+  // element of Lie algebra so(3): X = x^, normalized by normx
+  const Matrix3 Y = skewSymmetric(x) / theta;
+  return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+      + (1 - sin(theta) / theta) * Y * Y; // right Jacobian
+#endif
 }
 
 /* ************************************************************************* */
 Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
-  // x is the axis-angle representation (exponential coordinates) for a rotation
+  if(zero(x)) return I_3x3;
-  double normx = norm_2(x); // rotation angle
+  double theta = x.norm();
-  Matrix3 Jrinv;
+#ifdef DUY_VERSION
-
+  /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
-  if (normx < 10e-8){
+  Matrix3 X = skewSymmetric(x);
-    Jrinv = I_3x3;
+  Matrix3 X2 = X*X;
-  }
+  double vi = theta/2.0;
-  else{
+  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
-    const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
+  return I_3x3 + 0.5*X - s2*X2;
-    Jrinv = I_3x3 +
+#else // Luca's version
-        0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
+  /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
-  }
+   * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-  return Jrinv;
+   * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
+   * where Jrinv = LogmapDerivative(omega);
+   * This maps a perturbation on the manifold (expmap(omega))
+   * to a perturbation in the tangent space (Jrinv * omega)
+   */
+  const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
+  return I_3x3 + 0.5 * X
+      + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
+          * X;
+#endif
 }
 
 /* ************************************************************************* */

gtsam/geometry/Rot3.h

@@ -216,7 +216,7 @@ namespace gtsam {
     }
 
     /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
-    Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const;
+    Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const;
 
     /// Compose two rotations i.e., R= (*this) * R2
     Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
@@ -288,45 +288,21 @@ namespace gtsam {
      * Exponential map at identity - create a rotation from canonical coordinates
      * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
      */
-    static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
+    static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) {
-      if(H){
+      if(H) *H = Rot3::ExpmapDerivative(v);
-        H->resize(3,3);
+      if (zero(v)) return Rot3(); else return rodriguez(v);
-        *H = Rot3::ExpmapDerivative(v);
-      }
-      if(zero(v))
-        return Rot3();
-      else
-        return rodriguez(v);
     }
 
     /**
-     * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation
+     * Log map at identity - returns the canonical coordinates
+     * \f$ [R_x,R_y,R_z] \f$ of this rotation
      */
-    static Vector3 Logmap(const Rot3& R, boost::optional<Matrix3&> H = boost::none);
+    static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
 
-    /// Left-trivialized derivative of the exponential map
+    /// Derivative of Expmap
-    static Matrix3 dexpL(const Vector3& v);
-
-    /// Left-trivialized derivative inverse of the exponential map
-    static Matrix3 dexpInvL(const Vector3& v);
-
-    /**
-     * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
-     * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-     * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
-     * where Jr = ExpmapDerivative(thetahat);
-     * This maps a perturbation in the tangent space (omega) to
-     * a perturbation on the manifold (expmap(Jr * omega))
-     */
     static Matrix3 ExpmapDerivative(const Vector3& x);
 
-    /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
+    /// Derivative of Logmap
-     * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-     * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
-     * where Jrinv = LogmapDerivative(omega);
-     * This maps a perturbation on the manifold (expmap(omega))
-     * to a perturbation in the tangent space (Jrinv * omega)
-     */
     static Matrix3 LogmapDerivative(const Vector3& x);
 
     /// @}

gtsam/geometry/Rot3M.cpp

@@ -158,7 +158,7 @@ Matrix3 Rot3::transpose() const {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
+Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
   if (H1) *H1 = -rot_;
   return Rot3(Matrix3(transpose()));
 }
@@ -184,7 +184,7 @@ Point3 Rot3::rotate(const Point3& p,
 
 /* ************************************************************************* */
 // Log map at identity - return the canonical coordinates of this rotation
-Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
+Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
 
   static const double PI = boost::math::constants::pi<double>();
 
@@ -223,10 +223,7 @@ Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
         rot(1,0)-rot(0,1));
   }
 
-  if(H){
+  if(H) *H = Rot3::LogmapDerivative(thetaR);
-    H->resize(3,3);
-    *H = Rot3::LogmapDerivative(thetaR);
-  }
   return thetaR;
 }
 

gtsam/geometry/Rot3Q.cpp

@@ -101,7 +101,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Rot3 Rot3::inverse(boost::optional<Matrix3&> H1) const {
+  Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
     if (H1) *H1 = -matrix();
     return Rot3(quaternion_.inverse());
   }
@@ -133,7 +133,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
+  Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
     using std::acos;
     using std::sqrt;
     static const double twoPi = 2.0 * M_PI,
@@ -160,10 +160,7 @@ namespace gtsam {
       thetaR = (angle / s) * q.vec();
     }
 
-    if(H){
+    if(H) *H = Rot3::LogmapDerivative(thetaR);
-      H->resize(3,3);
-      *H = Rot3::LogmapDerivative(thetaR);
-    }
     return thetaR;
   }
 

gtsam/geometry/tests/testPose2.cpp

@@ -204,22 +204,22 @@ Vector3 testDexpL(const Vector3 w, const Vector3& dw) {
   return y;
 }
 
-TEST( Pose2, dexpL) {
+TEST( Pose2, ExpmapDerivative) {
-  Matrix actualDexpL = Pose2::dexpL(w);
+  Matrix actualDexpL = Pose2::ExpmapDerivative(w);
   Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(
           boost::bind(testDexpL, w, _1), zero(3), 1e-2);
   EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
 
-  Matrix actualDexpInvL = Pose2::dexpInvL(w);
+  Matrix actualDexpInvL = Pose2::LogmapDerivative(w);
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
 
   // test case where alpha = 0
-  Matrix actualDexpL0 = Pose2::dexpL(w0);
+  Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0);
   Matrix expectedDexpL0 = numericalDerivative11<Vector3, Vector3>(
           boost::bind(testDexpL, w0, _1), zero(3), 1e-2);
   EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5));
 
-  Matrix actualDexpInvL0 = Pose2::dexpInvL(w0);
+  Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0);
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
 }
 

gtsam/geometry/tests/testRot2.cpp

@@ -166,14 +166,14 @@ Vector1 testDexpL(const Vector& dw) {
   return y;
 }
 
-TEST( Rot2, dexpL) {
+TEST( Rot2, ExpmapDerivative) {
-  Matrix actualDexpL = Rot2::dexpL(w);
+  Matrix actualDexpL = Rot2::ExpmapDerivative(w);
   Matrix expectedDexpL = numericalDerivative11<Vector, Vector1>(
       boost::function<Vector(const Vector&)>(
           boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2);
   EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
 
-  Matrix actualDexpInvL = Rot2::dexpInvL(w);
+  Matrix actualDexpInvL = Rot2::LogmapDerivative(w);
   EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
 }
 

gtsam/geometry/tests/testRot3.cpp

@@ -214,14 +214,39 @@ TEST(Rot3, log)
   CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
 }
 
+/* ************************************************************************* */
+Vector w = Vector3(0.1, 0.27, -0.2);
+
+// Left trivialization Derivative of exp(w) wrpt w:
+// How does exp(w) change when w changes?
+// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
+// => y = log (exp(-w) * exp(w+dw))
+Vector3 testDexpL(const Vector3& dw) {
+  return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
+}
+
+TEST( Rot3, ExpmapDerivative) {
+  Matrix actualDexpL = Rot3::ExpmapDerivative(w);
+  Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
+      Vector3::Zero(), 1e-2);
+  EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
+
+  Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
+  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
+}
+
 /* ************************************************************************* */
 Vector3 thetahat(0.1, 0, 0.1);
-TEST( Rot3, ExpmapDerivative )
+TEST( Rot3, ExpmapDerivative2)
 {
   Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
       boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
+
   Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
   CHECK(assert_equal(Jexpected, Jactual));
+
+  Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
+  CHECK(assert_equal(Jexpected, Jactual2));
 }
 
 /* ************************************************************************* */
@@ -412,27 +437,6 @@ TEST( Rot3, between )
   CHECK(assert_equal(numericalH2,actualH2));
 }
 
-/* ************************************************************************* */
-Vector w = Vector3(0.1, 0.27, -0.2);
-
-// Left trivialization Derivative of exp(w) wrpt w:
-// How does exp(w) change when w changes?
-// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
-// => y = log (exp(-w) * exp(w+dw))
-Vector3 testDexpL(const Vector3& dw) {
-  return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
-}
-
-TEST( Rot3, dexpL) {
-  Matrix actualDexpL = Rot3::dexpL(w);
-  Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
-      Vector3::Zero(), 1e-2);
-  EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
-
-  Matrix actualDexpInvL = Rot3::dexpInvL(w);
-  EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
-}
-
 /* ************************************************************************* */
 TEST( Rot3, xyz )
 {