included Jacobian for logmap and expmap, with unit tests (Note: only implemented for Rot3M, this will not work in quaternion mode)

gtsam/geometry/Rot3.h

@@ -294,15 +294,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)  {
-      if(zero(v)) return Rot3();
-      else return rodriguez(v);
+    static Rot3 Expmap(const Vector& v, boost::optional<Matrix3&> H = boost::none) {
+      if(H){
+        H->resize(3,3);
+        *H = Rot3::rightJacobianExpMapSO3(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);
@@ -313,11 +319,19 @@ 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 + thetatilde) \approx expmap(thetahat) * expmap(Jr * thetatilde)
+     * where Jr = rightJacobianExpMapSO3(thetahat);
+     * This maps a perturbation in the tangent space (thetatilde) to
+     * a perturbation on the manifold (expmap(Jr * thetatilde))
      */
     static Matrix3 rightJacobianExpMapSO3(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 * Rtilde) \approx logmap( Rhat ) + Jrinv * logmap( Rtilde )
+     * where Jrinv = rightJacobianExpMapSO3inverse(logmap( Rtilde ));
+     * This maps a perturbation on the manifold (Rtilde)
+     * to a perturbation in the tangent space (Jrinv * logmap( Rtilde ))
      */
     static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x);
 

gtsam/geometry/Rot3M.cpp

@@ -200,7 +200,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>();
 
@@ -208,6 +208,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) {
@@ -218,7 +220,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;
@@ -231,11 +233,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::rightJacobianExpMapSO3inverse(thetaR);
+  }
+  return thetaR;
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -215,33 +215,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, rightJacobianExpMapSO3 )
+{
+  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
+      boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
+  Matrix Jactual = Rot3::rightJacobianExpMapSO3(thetahat);
+  CHECK(assert_equal(Jexpected, Jactual));
 }
 
 /* ************************************************************************* */
-TEST( Rot3, rightJacobianExpMapSO3 )
+TEST( Rot3, jacobianExpmap )
 {
-  // Linearization point
-  Vector3 thetahat; thetahat << 0.1, 0, 0;
-
-  Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
-      boost::bind(&Rot3::Expmap, _1), thetahat);
-  Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
-  CHECK(assert_equal(expectedJacobian, actualJacobian));
+  Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
+      &Rot3::Expmap, _1, boost::none), thetahat);
+  Matrix3 Jactual;
+  const Rot3 R = Rot3::Expmap(thetahat, Jactual);
+  EXPECT(assert_equal(Jexpected, Jactual));
 }
 
 /* ************************************************************************* */
 TEST( Rot3, rightJacobianExpMapSO3inverse )
 {
-  // Linearization point
-  Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
-  Vector3 deltatheta; deltatheta << 0, 0, 0;
+  Rot3 R = Rot3::Expmap(thetahat); // some rotation
+  Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
+      &Rot3::Logmap, _1, boost::none), R);
+  Matrix3 Jactual = Rot3::rightJacobianExpMapSO3inverse(thetahat);
+  EXPECT(assert_equal(Jexpected, Jactual));
+}
 
-  Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
-      boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
-  Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
-  EXPECT(assert_equal(expectedJacobian, actualJacobian));
+/* ************************************************************************* */
+TEST( Rot3, jacobianLogmap )
+{
+  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));
 }
 
 /* ************************************************************************* */