Successful representation of E as product manifold

gtsam/geometry/EssentialMatrix.cpp

@@ -13,64 +13,46 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
+EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
     OptionalJacobian<5, 6> H) {
-  const Rot3& _1R2_ = _1P2_.rotation();
+  const Rot3& aRb = aPb.rotation();
-  const Point3& _1T2_ = _1P2_.translation();
+  const Point3& aTb = aPb.translation();
   if (!H) {
     // just make a direction out of translation and create E
-    Unit3 direction(_1T2_);
+    Unit3 direction(aTb);
-    return EssentialMatrix(_1R2_, direction);
+    return EssentialMatrix(aRb, 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 Unit3::FromPoint3
     Matrix23 D_direction_1T2;
-    Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
+    Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
     *H << I_3x3, Z_3x3, //
-    Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
+    Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
-    return EssentialMatrix(_1R2_, direction);
+    return EssentialMatrix(aRb, direction);
   }
 }
 
 /* ************************************************************************* */
 void EssentialMatrix::print(const string& s) const {
   cout << s;
-  aRb_.print("R:\n");
+  rotation().print("R:\n");
-  aTb_.print("d: ");
+  direction().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);
-  Unit3 t = aTb_.retract(z);
-  return EssentialMatrix(R, t);
-}
-
-/* ************************************************************************* */
-Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
-  Vector5 v;
-  v << aRb_.localCoordinates(other.aRb_),
-      aTb_.localCoordinates(other.aTb_);
-  return v;
 }
 
 /* ************************************************************************* */
 Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
     OptionalJacobian<3, 3> Dpoint) const {
-  Pose3 pose(aRb_, aTb_.point3());
+  Pose3 pose(rotation(), direction().point3());
   Matrix36 DE_;
   Point3 q = pose.transform_to(p, DE ? &DE_ : 0, 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()
+    // The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
     // Duy made an educated guess that this needs to be rotated to the local frame
     Matrix35 H;
-    H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
+    H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
     *DE = H;
   }
   return q;
@@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
     OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
 
   // The rotation must be conjugated to act in the camera frame
-  Rot3 c1Rc2 = aRb_.conjugate(cRb);
+  Rot3 c1Rc2 = rotation().conjugate(cRb);
 
   if (!HE && !HR) {
     // Rotate translation direction and return
-    Unit3 c1Tc2 = cRb * aTb_;
+    Unit3 c1Tc2 = cRb * direction();
     return EssentialMatrix(c1Rc2, c1Tc2);
   } else {
     // Calculate derivatives
     Matrix23 D_c1Tc2_cRb; // 2*3
     Matrix2 D_c1Tc2_aTb; // 2*2
-    Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
+    Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
     if (HE)
       *HE << cRb.matrix(), Matrix32::Zero(), //
       Matrix23::Zero(), D_c1Tc2_aTb;
@@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
   if (H) {
     // See math.lyx
     Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+    Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
-        * aTb_.basis();
+        * direction().basis();
     *H << HR, HD;
   }
   return dot(vA, E_ * vB);

gtsam/geometry/EssentialMatrix.h

@@ -7,6 +7,55 @@
 
 #pragma once
 
+#include <gtsam/base/Manifold.h>
+
+namespace gtsam {
+
+/// CRTP to construct the product manifold of two other manifolds, M1 and M2
+/// Assumes manifold structure from M1 and M2, and binary constructor
+template<class Derived, typename M1, typename M2>
+class ProductManifold: public std::pair<M1, M2> {
+  BOOST_CONCEPT_ASSERT((IsManifold<M1>));
+  BOOST_CONCEPT_ASSERT((IsManifold<M2>));
+
+private:
+  const M1& g() const {return this->first;}
+  const M2& h() const {return this->second;}
+
+public:
+  // Dimension of the manifold
+  enum { dimension = M1::dimension + M2::dimension };
+
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+
+  /// Default constructor yields identity
+  ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
+
+  // Construct from two subgroup elements
+  ProductManifold(const M1& g, const M2& h):std::pair<M1,M2>(g,h) {}
+
+  /// Retract delta to manifold
+  Derived retract(const TangentVector& xi) const {
+    return Derived(traits<M1>::Retract(g(),xi.head(M1::dimension)),
+        traits<M2>::Retract(h(),xi.tail(M2::dimension)));
+  }
+
+  /// Compute the coordinates in the tangent space
+  TangentVector localCoordinates(const Derived& other) const {
+    TangentVector xi;
+    xi << traits<M1>::Local(g(),other.g()), traits<M2>::Local(h(),other.h());
+    return xi;
+  }
+};
+
+// Define any direct product group to be a model of the multiplicative Group concept
+template<class Derived, typename M1, typename M2>
+struct traits<ProductManifold<Derived, M1, M2> > : internal::Manifold<
+    ProductManifold<Derived, M1, M2> > {
+};
+
+}  // namespace gtsam
+
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
@@ -20,18 +69,15 @@ namespace gtsam {
  * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
  * We can then non-linearly optimize immediately on this 5-dimensional manifold.
  */
-class GTSAM_EXPORT EssentialMatrix {
+class GTSAM_EXPORT EssentialMatrix : public ProductManifold<EssentialMatrix, Rot3, Unit3> {
 
 private:
 
-  Rot3 aRb_; ///< Rotation between a and b
+  typedef ProductManifold<EssentialMatrix, Rot3, Unit3> Base;
-  Unit3 aTb_; ///< translation direction from a to b
   Matrix3 E_; ///< Essential matrix
 
 public:
 
-  enum { dimension = 5 };
-
   /// Static function to convert Point2 to homogeneous coordinates
   static Vector3 Homogeneous(const Point2& p) {
     return Vector3(p.x(), p.y(), 1);
@@ -42,12 +88,12 @@ public:
 
   /// Default constructor
   EssentialMatrix() :
-      aTb_(1, 0, 0), E_(aTb_.skew()) {
+      Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
   }
 
   /// Construct from rotation and translation
   EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
-      aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
+      Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
   }
 
   /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
@@ -72,43 +118,23 @@ public:
 
   /// assert equality up to a tolerance
   bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
-    return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
+    return rotation().equals(other.rotation(), tol)
+        && direction().equals(other.direction(), tol);
   }
 
   /// @}
 
-  /// @name Manifold
-  /// @{
-
-  /// Dimensionality of tangent space = 5 DOF
-  inline static size_t Dim() {
-    return 5;
-  }
-
-  /// Return the dimensionality of the tangent space
-  size_t dim() const {
-    return 5;
-  }
-
-  /// Retract delta to manifold
-  EssentialMatrix retract(const Vector& xi) const;
-
-  /// Compute the coordinates in the tangent space
-  Vector5 localCoordinates(const EssentialMatrix& other) const;
-
-  /// @}
-
   /// @name Essential matrix methods
   /// @{
 
   /// Rotation
   inline const Rot3& rotation() const {
-    return aRb_;
+    return this->first;
   }
 
   /// Direction
   inline const Unit3& direction() const {
-    return aTb_;
+    return this->second;
   }
 
   /// Return 3*3 matrix representation
@@ -118,12 +144,12 @@ public:
 
   /// Return epipole in image_a , as Unit3 to allow for infinity
   inline const Unit3& epipole_a() const {
-    return aTb_; // == direction()
+    return direction();
   }
 
   /// Return epipole in image_b, as Unit3 to allow for infinity
   inline Unit3 epipole_b() const {
-    return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
+    return rotation().unrotate(direction());
   }
 
   /**
@@ -180,8 +206,8 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(aRb_);
+      ar & BOOST_SERIALIZATION_NVP(rotation());
-      ar & BOOST_SERIALIZATION_NVP(aTb_);
+      ar & BOOST_SERIALIZATION_NVP(direction());
 
       ar & boost::serialization::make_nvp("E11", E_(0,0));
       ar & boost::serialization::make_nvp("E12", E_(0,1));
@@ -195,7 +221,6 @@ private:
     }
 
   /// @}
-
 };
 
 template<>