Merged in Luca's rename to ExpmapDerivative and LogMapDerivative from 'origin/feature/imuFixed'

gtsam/geometry/Rot3.cpp

@@ -175,7 +175,7 @@ Vector Rot3::quaternion() const {
 }
 
 /* ************************************************************************* */
-Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x)    {
+Matrix3 Rot3::ExpmapDerivative(const Vector3& x)    {
   // x is the axis-angle representation (exponential coordinates) for a rotation
   double normx = norm_2(x); // rotation angle
   Matrix3 Jr;
@@ -183,15 +183,15 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x)    {
     Jr = I_3x3;
   }
   else{
-    const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
+    const Matrix3 X = skewSymmetric(x) / normx; // element of Lie algebra so(3): X = x^, normalized by normx
-    Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X +
+    Jr = I_3x3 - ((1-cos(normx))/(normx)) * X +
-        ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
+        (1 -sin(normx)/normx) * X * X; // right Jacobian
   }
   return Jr;
 }
 
 /* ************************************************************************* */
-Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x)    {
+Matrix3 Rot3::LogmapDerivative(const Vector3& x)    {
   // x is the axis-angle representation (exponential coordinates) for a rotation
   double normx = norm_2(x); // rotation angle
   Matrix3 Jrinv;

gtsam/geometry/Rot3.h

@@ -16,6 +16,7 @@
  * @author  Christian Potthast
  * @author  Frank Dellaert
  * @author  Richard Roberts
+ * @author  Luca Carlone
  */
 // \callgraph
 
@@ -287,15 +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)  {
+    static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
-      if(zero(v)) return Rot3();
+      if(H){
-      else return rodriguez(v);
+        H->resize(3,3);
+        *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
      */
-    static Vector3 Logmap(const Rot3& R);
+    static Vector3 Logmap(const Rot3& R, boost::optional<Matrix3&> H = boost::none);
 
     /// Left-trivialized derivative of the exponential map
     static Matrix3 dexpL(const Vector3& v);
@@ -306,13 +313,21 @@ namespace gtsam {
     /**
      * 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 rightJacobianExpMapSO3(const Vector3& x);
+    static Matrix3 ExpmapDerivative(const Vector3& 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.
+     * 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 rightJacobianExpMapSO3inverse(const Vector3& x);
+    static Matrix3 LogmapDerivative(const Vector3& x);
 
     /// @}
     /// @name Group Action on Point3

gtsam/geometry/Rot3M.cpp

@@ -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) {
+Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
 
   static const double PI = boost::math::constants::pi<double>();
 
@@ -192,6 +192,8 @@ Vector3 Rot3::Logmap(const Rot3& R) {
   // Get trace(R)
   double tr = rot.trace();
 
+  Vector3 thetaR;
+
   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
   // we do something special
   if (std::abs(tr+1.0) < 1e-10) {
@@ -202,7 +204,7 @@ Vector3 Rot3::Logmap(const Rot3& R) {
       return (PI / sqrt(2.0+2.0*rot(1,1))) *
           Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
     else // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
-      return (PI / sqrt(2.0+2.0*rot(0,0))) *
+      thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) *
           Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
   } else {
     double magnitude;
@@ -215,11 +217,17 @@ Vector3 Rot3::Logmap(const Rot3& R) {
       // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
       magnitude = 0.5 - tr_3*tr_3/12.0;
     }
-    return magnitude*Vector3(
+    thetaR =  magnitude*Vector3(
         rot(2,1)-rot(1,2),
         rot(0,2)-rot(2,0),
         rot(1,0)-rot(0,1));
   }
+
+  if(H){
+    H->resize(3,3);
+    *H = Rot3::LogmapDerivative(thetaR);
+  }
+  return thetaR;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Rot3Q.cpp

@@ -133,21 +133,22 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Vector3 Rot3::Logmap(const Rot3& R) {
+  Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
     using std::acos;
     using std::sqrt;
     static const double twoPi = 2.0 * M_PI,
     // define these compile time constants to avoid std::abs:
     NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
 
+    Vector3 thetaR;
     const Quaternion& q = R.quaternion_;
     const double qw = q.w();
     if (qw > NearlyOne) {
       // Taylor expansion of (angle / s) at 1
-      return (2 - 2 * (qw - 1) / 3) * q.vec();
+      thetaR = (2 - 2 * (qw - 1) / 3) * q.vec();
     } else if (qw < NearlyNegativeOne) {
       // Angle is zero, return zero vector
-      return Vector3::Zero();
+      thetaR = Vector3::Zero();
     } else {
       // Normal, away from zero case
       double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
@@ -156,8 +157,14 @@ namespace gtsam {
         angle -= twoPi;
       else if (angle < -M_PI)
         angle += twoPi;
-      return (angle / s) * q.vec();
+      thetaR = (angle / s) * q.vec();
     }
+
+    if(H){
+      H->resize(3,3);
+      *H = Rot3::LogmapDerivative(thetaR);
+    }
+    return thetaR;
   }
 
   /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -214,33 +214,45 @@ TEST(Rot3, log)
   CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
 }
 
-Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
+/* ************************************************************************* */
-  return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
+Vector3 thetahat(0.1, 0, 0.1);
+TEST( Rot3, ExpmapDerivative )
+{
+  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
+  Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
+  CHECK(assert_equal(Jexpected, Jactual));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, rightJacobianExpMapSO3 )
+TEST( Rot3, jacobianExpmap )
 {
-  // Linearization point
+  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
-  Vector3 thetahat; thetahat << 0.1, 0, 0;
+      &Rot3::Expmap, _1, boost::none), thetahat);
-
+  Matrix3 Jactual;
-  Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
+  const Rot3 R = Rot3::Expmap(thetahat, Jactual);
-      boost::bind(&Rot3::Expmap, _1), thetahat);
+  EXPECT(assert_equal(Jexpected, Jactual));
-  Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
-  CHECK(assert_equal(expectedJacobian, actualJacobian));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, rightJacobianExpMapSO3inverse )
+TEST( Rot3, LogmapDerivative )
 {
-  // Linearization point
+  Rot3 R = Rot3::Expmap(thetahat); // some rotation
-  Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
+  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
-  Vector3 deltatheta; deltatheta << 0, 0, 0;
+      &Rot3::Logmap, _1, boost::none), R);
+  Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
+  EXPECT(assert_equal(Jexpected, Jactual));
+}
 
-  Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
+/* ************************************************************************* */
-      boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
+TEST( Rot3, jacobianLogmap )
-  Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
+{
-  EXPECT(assert_equal(expectedJacobian, actualJacobian));
+  Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
+      &Rot3::Logmap, _1, boost::none), R);
+  Matrix3 Jactual;
+  Rot3::Logmap(R, Jactual);
+  EXPECT(assert_equal(Jexpected, Jactual));
 }
 
 /* ************************************************************************* */