Derivatives of rotate work (at least, part that matters: not yet with respect to rotation)

gtsam/geometry/EssentialMatrix.h

@@ -88,7 +88,7 @@ public:
 
   /// Retract delta to manifold
   virtual EssentialMatrix retract(const Vector& xi) const {
-    assert(xi.size()==5);
+    assert(xi.size() == 5);
     Vector3 omega(sub(xi, 0, 3));
     Vector2 z(sub(xi, 3, 5));
     Rot3 R = aRb_.retract(omega);
@@ -97,8 +97,9 @@ public:
   }
 
   /// Compute the coordinates in the tangent space
-  virtual Vector localCoordinates(const EssentialMatrix& value) const {
-    return Vector(5) << 0, 0, 0, 0, 0;
+  virtual Vector localCoordinates(const EssentialMatrix& other) const {
+    return Vector(5) << //
+        aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
   }
 
   /// @}
@@ -139,7 +140,7 @@ public:
       // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
       // Duy made an educated guess that this needs to be rotated to the local frame
       Matrix H(3, 5);
-      H << DE->block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
+      H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
       *DE = H;
     }
     return q;
@@ -150,10 +151,45 @@ public:
    * @param cRb rotation from body frame to camera frame
    * @param E essential matrix E in camera frame C
    */
-  friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
-    Rot3  c1Rc2 = E.aRb_.conjugate(cRb);
-    Sphere2 c1Tc2 = cRb * E.aTb_;
+  EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
+      boost::none, boost::optional<Matrix&> HR = boost::none) const {
+
+    // The rotation must be conjugated to act in the camera frame
+    Rot3 c1Rc2 = aRb_.conjugate(cRb);
+
+    if (!HE && !HR) {
+      // Rotate translation direction and return
+      Sphere2 c1Tc2 = cRb * aTb_;
       return EssentialMatrix(c1Rc2, c1Tc2);
+    } else {
+      // Calculate derivatives
+      Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
+      Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
+      if (HE) {
+        *HE = zeros(5, 5);
+        HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
+        HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
+      }
+      if (HR) {
+        throw std::runtime_error(
+            "EssentialMatrix::rotate: derivative HR not implemented yet");
+        /*
+         HR->resize(5, 3);
+         HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
+         HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
+         */
+      }
+      return EssentialMatrix(c1Rc2, c1Tc2);
+    }
+  }
+
+  /**
+   * Given essential matrix E in camera frame B, convert to body frame C
+   * @param cRb rotation from body frame to camera frame
+   * @param E essential matrix E in camera frame C
+   */
+  friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
+    return E.rotate(cRb);
   }
 
   /// epipolar error, algebraic

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -58,14 +58,16 @@ TEST (EssentialMatrix, transform_to) {
   static Point3 P(0.2, 0.7, -2);
   Matrix actH1, actH2;
   E.transform_to(P, actH1, actH2);
-  Matrix expH1 = numericalDerivative21(transform_to_, E, P), expH2 =
-      numericalDerivative22(transform_to_, E, P);
+  Matrix expH1 = numericalDerivative21(transform_to_, E, P), //
+  expH2 = numericalDerivative22(transform_to_, E, P);
   EXPECT(assert_equal(expH1, actH1, 1e-8));
   EXPECT(assert_equal(expH2, actH2, 1e-8));
 }
 
 //*************************************************************************
-
+EssentialMatrix rotate_(const EssentialMatrix& E, const Rot3& cRb) {
+  return E.rotate(cRb);
+}
 TEST (EssentialMatrix, rotate) {
   // Suppose the essential matrix is specified in a body coordinate frame B
   // which is rotated with respect to the camera frame C, via rotation bRc.
@@ -78,9 +80,19 @@ TEST (EssentialMatrix, rotate) {
   Point3 b1Tb2 = bRc * c1Tc2;
   EssentialMatrix bodyE(b1Rb2, b1Tb2);
   EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
+  EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));
 
   // Let's go back to camera frame:
   EXPECT(assert_equal(trueE, cRb * bodyE, 1e-8));
+  EXPECT(assert_equal(trueE, bodyE.rotate(cRb), 1e-8));
+
+  // Derivatives
+  Matrix actH1, actH2;
+  try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
+  Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
+  expH2 = numericalDerivative22(rotate_, bodyE, cRb);
+  EXPECT(assert_equal(expH1, actH1, 1e-8));
+  // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
 }
 
 /* ************************************************************************* */