Named constructor FromPose3, with Jacobians

gtsam/geometry/EssentialMatrix.cpp

@@ -0,0 +1,127 @@
+/*
+ * @file EssentialMatrix.cpp
+ * @brief EssentialMatrix class
+ * @author Frank Dellaert
+ * @date December 5, 2014
+ */
+
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <iostream>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
+    boost::optional<Matrix&> H) {
+  const Rot3& _1R2_ = _1P2_.rotation();
+  const Point3& _1T2_ = _1P2_.translation();
+  if (!H) {
+    // just make a direction out of translation and create E
+    Sphere2 direction(_1T2_);
+    return EssentialMatrix(_1R2_, direction);
+  } else {
+    // Calculate the 5*6 Jacobian H = D_E_1P2
+    // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
+    // First get 2*3 derivative from Sphere2::FromPoint3
+    Matrix D_direction_1T2;
+    Sphere2 direction = Sphere2::FromPoint3(_1T2_, D_direction_1T2);
+    H->resize(5, 6);
+    H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
+    H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
+    H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
+    H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
+    return EssentialMatrix(_1R2_, direction);
+  }
+}
+
+/* ************************************************************************* */
+void EssentialMatrix::print(const std::string& s) const {
+  std::cout << s;
+  aRb_.print("R:\n");
+  aTb_.print("d: ");
+}
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
+  assert(xi.size() == 5);
+  Vector3 omega(sub(xi, 0, 3));
+  Vector2 z(sub(xi, 3, 5));
+  Rot3 R = aRb_.retract(omega);
+  Sphere2 t = aTb_.retract(z);
+  return EssentialMatrix(R, t);
+}
+
+/* ************************************************************************* */
+Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
+  return Vector(5) << //
+      aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
+}
+
+/* ************************************************************************* */
+Point3 EssentialMatrix::transform_to(const Point3& p,
+    boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
+  Pose3 pose(aRb_, aTb_.point3());
+  Point3 q = pose.transform_to(p, DE, Dpoint);
+  if (DE) {
+    // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
+    // The last 3 columns are derivative with respect to change in translation
+    // 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();
+    *DE = H;
+  }
+  return q;
+}
+
+/* ************************************************************************* */
+EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
+    boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) 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);
+  }
+}
+
+/* ************************************************************************* */
+double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
+    boost::optional<Matrix&> H) const {
+  if (H) {
+    H->resize(1, 5);
+    // See math.lyx
+    Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+        * aTb_.basis();
+    *H << HR, HD;
+  }
+  return dot(vA, E_ * vB);
+}
+
+/* ************************************************************************* */
+
+} // gtsam
+

gtsam/geometry/EssentialMatrix.h

@@ -10,7 +10,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Sphere2.h>
 #include <gtsam/geometry/Point2.h>
-#include <iostream>
 
 namespace gtsam {
 
@@ -26,7 +25,7 @@ private:
 
   Rot3 aRb_; ///< Rotation between a and b
   Sphere2 aTb_; ///< translation direction from a to b
-  Matrix E_; ///< Essential matrix
+  Matrix3 E_; ///< Essential matrix
 
 public:
 
@@ -48,6 +47,10 @@ public:
       aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
   }
 
+  /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
+  static EssentialMatrix FromPose3(const Pose3& _1P2_,
+      boost::optional<Matrix&> H = boost::none);
+
   /// Random, using Rot3::Random and Sphere2::Random
   template<typename Engine>
   static EssentialMatrix Random(Engine & rng) {
@@ -60,11 +63,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const {
+  void print(const std::string& s = "") const;
-    std::cout << s;
-    aRb_.print("R:\n");
-    aTb_.print("d: ");
-  }
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
@@ -87,20 +86,10 @@ public:
   }
 
   /// Retract delta to manifold
-  virtual EssentialMatrix retract(const Vector& xi) const {
+  virtual EssentialMatrix retract(const Vector& xi) const;
-    assert(xi.size() == 5);
-    Vector3 omega(sub(xi, 0, 3));
-    Vector2 z(sub(xi, 3, 5));
-    Rot3 R = aRb_.retract(omega);
-    Sphere2 t = aTb_.retract(z);
-    return EssentialMatrix(R, t);
-  }
 
   /// Compute the coordinates in the tangent space
-  virtual Vector localCoordinates(const EssentialMatrix& other) const {
+  virtual Vector localCoordinates(const EssentialMatrix& other) const;
-    return Vector(5) << //
-        aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
-  }
 
   /// @}
 
@@ -108,17 +97,17 @@ public:
   /// @{
 
   /// Rotation
-  const Rot3& rotation() const {
+  inline const Rot3& rotation() const {
     return aRb_;
   }
 
   /// Direction
-  const Sphere2& direction() const {
+  inline const Sphere2& direction() const {
     return aTb_;
   }
 
   /// Return 3*3 matrix representation
-  const Matrix& matrix() const {
+  inline const Matrix3& matrix() const {
     return E_;
   }
 
@@ -131,20 +120,7 @@ public:
    */
   Point3 transform_to(const Point3& p,
       boost::optional<Matrix&> DE = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const {
+      boost::optional<Matrix&> Dpoint = boost::none) const;
-    Pose3 pose(aRb_, aTb_.point3());
-    Point3 q = pose.transform_to(p, DE, Dpoint);
-    if (DE) {
-      // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
-      // The last 3 columns are derivative with respect to change in translation
-      // 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();
-      *DE = H;
-    }
-    return q;
-  }
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -152,36 +128,7 @@ public:
    * @param E essential matrix E in camera frame C
    */
   EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
-      boost::none, boost::optional<Matrix&> HR = boost::none) const {
+      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
@@ -194,17 +141,7 @@ public:
 
   /// epipolar error, algebraic
   double error(const Vector& vA, const Vector& vB, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const;
-    if (H) {
-      H->resize(1, 5);
-      // See math.lyx
-      Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
-      Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
-          * aTb_.basis();
-      *H << HR, HD;
-    }
-    return dot(vA, E_ * vB);
-  }
 
   /// @}
 

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -88,13 +88,39 @@ TEST (EssentialMatrix, rotate) {
 
   // Derivatives
   Matrix actH1, actH2;
-  try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
+  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));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3_a) {
+  Matrix actualH;
+  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
+  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
+      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3_b) {
+  Matrix actualH;
+  Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
+  Point3 c1Tc2(0.4, 0.5, 0.6);
+  EssentialMatrix trueE(c1Rc2, c1Tc2);
+  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
+  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
+  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
+      boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose);
+  EXPECT(assert_equal(expectedH, actualH, 1e-8));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;