Merged in feature/2.4.0/extrinsicE (pull request #2)

A third factor for essential matrices, now with an extrinsic calibration (rotation only)

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 {
+  virtual Vector localCoordinates(const EssentialMatrix& other) const {
-    return Vector(5) << 0, 0, 0, 0, 0;
+    return Vector(5) << //
+        aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
   }
 
   /// @}
@@ -139,12 +140,58 @@ 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;
   }
 
+  /**
+   * 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
+   */
+  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
   double error(const Vector& vA, const Vector& vB, //
       boost::optional<Matrix&> H = boost::none) const {

gtsam/geometry/Rot3.h

@@ -225,6 +225,16 @@ namespace gtsam {
     /** compose two rotations */
     Rot3 operator*(const Rot3& R2) const;
 
+    /**
+     * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C
+     * @param cRb rotation from B frame to C frame
+     * @return c1Rc2 = cRb * b1Rb2 * cRb'
+     */
+    Rot3 conjugate(const Rot3& cRb) const {
+      // TODO: do more efficiently by using Eigen or quaternion properties
+      return cRb * (*this) * cRb.inverse();
+    }
+
     /**
      * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
      */

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -19,48 +19,82 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
 
 //*************************************************************************
 // Create two cameras and corresponding essential matrix E
-Rot3 aRb = Rot3::yaw(M_PI_2);
+Rot3 c1Rc2 = Rot3::yaw(M_PI_2);
-Point3 aTb(0.1, 0, 0);
+Point3 c1Tc2(0.1, 0, 0);
+EssentialMatrix trueE(c1Rc2, c1Tc2);
 
 //*************************************************************************
 TEST (EssentialMatrix, equality) {
-  EssentialMatrix actual(aRb, aTb), expected(aRb, aTb);
+  EssentialMatrix actual(c1Rc2, c1Tc2), expected(c1Rc2, c1Tc2);
   EXPECT(assert_equal(expected, actual));
 }
 
 //*************************************************************************
 TEST (EssentialMatrix, retract1) {
-  EssentialMatrix expected(aRb.retract((Vector(3) << 0.1, 0, 0)), aTb);
+  EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), c1Tc2);
-  EssentialMatrix trueE(aRb, aTb);
   EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0));
   EXPECT(assert_equal(expected, actual));
 }
 
 //*************************************************************************
 TEST (EssentialMatrix, retract2) {
-  EssentialMatrix expected(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
+  EssentialMatrix expected(c1Rc2,
-  EssentialMatrix trueE(aRb, aTb);
+      Sphere2(c1Tc2).retract((Vector(2) << 0.1, 0)));
   EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0));
   EXPECT(assert_equal(expected, actual));
 }
 
 //*************************************************************************
-Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); }
+Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
+  return E.transform_to(point);
+}
 TEST (EssentialMatrix, transform_to) {
   // test with a more complicated EssentialMatrix
-  Rot3 aRb2 = Rot3::yaw(M_PI/3.0)*Rot3::pitch(M_PI_4)*Rot3::roll(M_PI/6.0);
+  Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4)
+      * Rot3::roll(M_PI / 6.0);
   Point3 aTb2(19.2, 3.7, 5.9);
   EssentialMatrix E(aRb2, aTb2);
   //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
-  static Point3 P(0.2,0.7,-2);
+  static Point3 P(0.2, 0.7, -2);
   Matrix actH1, actH2;
-  E.transform_to(P,actH1,actH2);
+  E.transform_to(P, actH1, actH2);
-  Matrix expH1 = numericalDerivative21(transform_to_, E,P),
+  Matrix expH1 = numericalDerivative21(transform_to_, E, P), //
-       expH2 = numericalDerivative22(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.
+  // The rotation between body and camera is:
+  Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1);
+  Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse();
+
+  // Let's compute the ground truth E in body frame:
+  Rot3 b1Rb2 = bRc * c1Rc2 * cRb;
+  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));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/EssentialMatrixFactor.h

@@ -20,26 +20,46 @@ namespace gtsam {
  */
 class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
 
-  Point2 pA_, pB_; ///< Measurements in image A and B
+  Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates
-  Vector vA_, vB_; ///< Homogeneous versions
 
   typedef NoiseModelFactor1<EssentialMatrix> Base;
   typedef EssentialMatrixFactor This;
 
 public:
 
-  /// Constructor
+  /**
+   *  Constructor
+   *  @param pA point in first camera, in calibrated coordinates
+   *  @param pB point in second camera, in calibrated coordinates
+   *  @param model noise model is about dot product in ideal, homogeneous coordinates
+   */
   EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
       const SharedNoiseModel& model) :
-      Base(model, key), pA_(pA), pB_(pB), //
+      Base(model, key) {
-      vA_(EssentialMatrix::Homogeneous(pA)), //
+    vA_ = EssentialMatrix::Homogeneous(pA);
-      vB_(EssentialMatrix::Homogeneous(pB)) {
+    vB_ = EssentialMatrix::Homogeneous(pB);
+  }
+
+  /**
+   *  Constructor
+   *  @param pA point in first camera, in pixel coordinates
+   *  @param pB point in second camera, in pixel coordinates
+   *  @param model noise model is about dot product in ideal, homogeneous coordinates
+   *  @param K calibration object, will be used only in constructor
+   */
+  template<class CALIBRATION>
+  EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
+      const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
+      Base(model, key) {
+    assert(K);
+    vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
+    vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
   }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-    return boost::static_pointer_cast < gtsam::NonlinearFactor
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-        > (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
@@ -47,46 +67,72 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor with measurements\n  ("
-        << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
+        << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
-        << ")'" << std::endl;
+        << std::endl;
   }
 
   /// vector of errors returns 1D vector
   Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
       boost::none) const {
-    return (Vector(1) << E.error(vA_, vB_, H));
+    Vector error(1);
+    error << E.error(vA_, vB_, H);
+    return error;
   }
 
 };
 
 /**
- * Binary factor that optimizes for E and inverse depth: assumes measurement
+ * Binary factor that optimizes for E and inverse depth d: assumes measurement
  * in image 2 is perfect, and returns re-projection error in image 1
  */
 class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
     LieScalar> {
 
   Point3 dP1_; ///< 3D point corresponding to measurement in image 1
-  Point2 p1_, p2_; ///< Measurements in image 1 and image 2
+  Point2 pn_; ///< Measurement in image 2, in ideal coordinates
-  Cal3_S2 K_; ///< Calibration
+  double f_; ///< approximate conversion factor for error scaling
 
   typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
   typedef EssentialMatrixFactor2 This;
 
 public:
 
-  /// Constructor
+  /**
+   *  Constructor
+   *  @param pA point in first camera, in calibrated coordinates
+   *  @param pB point in second camera, in calibrated coordinates
+   *  @param model noise model should be in pixels, as well
+   */
   EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
-      const Cal3_S2& K, const SharedNoiseModel& model) :
+      const SharedNoiseModel& model) :
-      Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) {
+      Base(model, key1, key2) {
-    Point2 xy = K_.calibrate(p1_);
+    dP1_ = Point3(pA.x(), pA.y(), 1);
-    dP1_ = Point3(xy.x(), xy.y(), 1);
+    pn_ = pB;
+    f_ = 1.0;
+  }
+
+  /**
+   *  Constructor
+   *  @param pA point in first camera, in pixel coordinates
+   *  @param pB point in second camera, in pixel coordinates
+   *  @param K calibration object, will be used only in constructor
+   *  @param model noise model should be in pixels, as well
+   */
+  template<class CALIBRATION>
+  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
+      const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
+      Base(model, key1, key2) {
+    assert(K);
+    Point2 p1 = K->calibrate(pA);
+    dP1_ = Point3(p1.x(), p1.y(), 1); // d*P1 = (x,y,1)
+    pn_ = K->calibrate(pB);
+    f_ = 0.5 * (K->fx() + K->fy());
   }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-    return boost::static_pointer_cast < gtsam::NonlinearFactor
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-        > (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
@@ -94,62 +140,143 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
-        << p1_.vector().transpose() << ")' and (" << p2_.vector().transpose()
+        << dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose()
         << ")'" << std::endl;
   }
 
-  /// vector of errors returns 1D vector
+  /*
+   * Vector of errors returns 2D vector
+   * @param E essential matrix
+   * @param d inverse depth d
+   */
   Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
           boost::none) const {
 
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
-    // We then convert to first camera by 2P = 1R2Õ*(P1-1T2)
+    // We then convert to second camera by P2 = 1R2Õ*(P1-1T2)
     // The homogeneous coordinates of can be written as
     //   2R1*(P1-1T2) ==  2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
-    // Note that this is just a homography for d==0
+    // where we multiplied with d which yields equivalent homogeneous coordinates.
+    // Note that this is just the homography 2R1 for d==0
     // The point d*P1 = (x,y,1) is computed in constructor as dP1_
 
     // Project to normalized image coordinates, then uncalibrate
-    Point2 pi;
+    Point2 pn;
     if (!DE && !Dd) {
 
       Point3 _1T2 = E.direction().point3();
       Point3 d1T2 = d * _1T2;
-      Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);
+      Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
-      Point2 pn = SimpleCamera::project_to_camera(dP2);
+      pn = SimpleCamera::project_to_camera(dP2);
-      pi = K_.uncalibrate(pn);
 
     } else {
 
       // Calculate derivatives. TODO if slow: optimize with Mathematica
-      //     3*2        3*3       3*3        2*3      2*2
+      //     3*2        3*3       3*3        2*3
-      Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2, Dpi_pn;
+      Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2;
 
       Point3 _1T2 = E.direction().point3(D_1T2_dir);
       Point3 d1T2 = d * _1T2;
       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
-      Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
+      pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
-      pi = K_.uncalibrate(pn, boost::none, Dpi_pn);
 
       if (DE) {
         Matrix DdP2_E(3, 5);
         DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
-        *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5)
+        *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
       }
 
       if (Dd) // efficient backwards computation:
-        //     (2*2)   * (2*3)    * (3*3)      * (3*1)
+        //      (2*3)    * (3*3)      * (3*1)
-        *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
+        *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector()));
 
     }
-    Point2 reprojectionError = pi - p2_;
+    Point2 reprojectionError = pn - pn_;
-    return reprojectionError.vector();
+    return f_ * reprojectionError.vector();
   }
 
 };
 // EssentialMatrixFactor2
 
+/**
+ * Binary factor that optimizes for E and inverse depth d: assumes measurement
+ * in image 2 is perfect, and returns re-projection error in image 1
+ * This version takes an extrinsic rotation to allow for omni-directional rigs
+ */
+class EssentialMatrixFactor3: public EssentialMatrixFactor2 {
+
+  typedef EssentialMatrixFactor2 Base;
+  typedef EssentialMatrixFactor3 This;
+
+  Rot3 cRb_; ///< Rotation from body to camera frame
+
+public:
+
+  /**
+   *  Constructor
+   *  @param pA point in first camera, in calibrated coordinates
+   *  @param pB point in second camera, in calibrated coordinates
+   *  @param bRc extra rotation between "body" and "camera" frame
+   *  @param model noise model should be in calibrated coordinates, as well
+   */
+  EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
+      const Rot3& cRb, const SharedNoiseModel& model) :
+      EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {
+  }
+
+  /**
+   *  Constructor
+   *  @param pA point in first camera, in pixel coordinates
+   *  @param pB point in second camera, in pixel coordinates
+   *  @param K calibration object, will be used only in constructor
+   *  @param model noise model should be in pixels, as well
+   */
+  template<class CALIBRATION>
+  EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
+      const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
+      EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /// print
+  virtual void print(const std::string& s = "",
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    Base::print(s);
+    std::cout << "  EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
+  }
+
+  /*
+   * Vector of errors returns 2D vector
+   * @param E essential matrix
+   * @param d inverse depth d
+   */
+  Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
+      boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
+          boost::none) const {
+    if (!DE) {
+      // Convert E from body to camera frame
+      EssentialMatrix cameraE = cRb_ * E;
+      // Evaluate error
+      return Base::evaluateError(cameraE, d, boost::none, Dd);
+    } else {
+      // Version with derivatives
+      Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
+      EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
+      Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
+      *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
+      return e;
+    }
+  }
+
+};
+// EssentialMatrixFactor3
+
 }// gtsam
 

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -6,6 +6,7 @@
  */
 
 #include <gtsam/slam/EssentialMatrixFactor.h>
+
 #include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -18,15 +19,25 @@
 using namespace std;
 using namespace gtsam;
 
-typedef noiseModel::Isotropic::shared_ptr Model;
+// Noise model for first type of factor is evaluating algebraic error
+noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1,
+    0.01);
+// Noise model for second type of factor is evaluating pixel coordinates
+noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
+
+// The rotation between body and camera is:
+gtsam::Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1);
+gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse();
 
 namespace example1 {
 
 const string filename = findExampleDataFile("5pointExample1.txt");
 SfM_data data;
 bool readOK = readBAL(filename, data);
-Rot3 aRb = data.cameras[1].pose().rotation();
+Rot3 c1Rc2 = data.cameras[1].pose().rotation();
-Point3 aTb = data.cameras[1].pose().translation();
+Point3 c1Tc2 = data.cameras[1].pose().translation();
+PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2());
+EssentialMatrix trueE(c1Rc2, c1Tc2);
 double baseline = 0.1; // actual baseline of the camera
 
 Point2 pA(size_t i) {
@@ -42,8 +53,6 @@ Vector vB(size_t i) {
   return EssentialMatrix::Homogeneous(pB(i));
 }
 
-Cal3_S2 K;
-
 //*************************************************************************
 TEST (EssentialMatrixFactor, testData) {
   CHECK(readOK);
@@ -51,35 +60,32 @@ TEST (EssentialMatrixFactor, testData) {
   // Check E matrix
   Matrix expected(3, 3);
   expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
-  Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix();
+  Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
-  EXPECT(assert_equal(expected, aEb_matrix,1e-8));
+      * c1Rc2.matrix();
+  EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
 
   // Check some projections
-  EXPECT(assert_equal(Point2(0,0),pA(0),1e-8));
+  EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
-  EXPECT(assert_equal(Point2(0,0.1),pB(0),1e-8));
+  EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
-  EXPECT(assert_equal(Point2(0,-1),pA(4),1e-8));
+  EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
-  EXPECT(assert_equal(Point2(-1,0.2),pB(4),1e-8));
+  EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
 
   // Check homogeneous version
-  EXPECT(assert_equal((Vector(3) << -1,0.2,1),vB(4),1e-8));
+  EXPECT(assert_equal((Vector(3) << -1, 0.2, 1), vB(4), 1e-8));
 
   // Check epipolar constraint
   for (size_t i = 0; i < 5; i++)
     EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
 
   // Check epipolar constraint
-  EssentialMatrix trueE(aRb, aTb);
   for (size_t i = 0; i < 5; i++)
-    EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i),vB(i)), 1e-7);
+    EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
 }
 
 //*************************************************************************
 TEST (EssentialMatrixFactor, factor) {
-  EssentialMatrix trueE(aRb, aTb);
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
-
   for (size_t i = 0; i < 5; i++) {
-    EssentialMatrixFactor factor(1, pA(i), pB(i), model);
+    EssentialMatrixFactor factor(1, pA(i), pB(i), model1);
 
     // Check evaluation
     Vector expected(1);
@@ -100,7 +106,7 @@ TEST (EssentialMatrixFactor, factor) {
 }
 
 //*************************************************************************
-TEST (EssentialMatrixFactor, fromConstraints) {
+TEST (EssentialMatrixFactor, minimization) {
   // Here we want to optimize directly on essential matrix constraints
   // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
   // but GTSAM does the equivalent anyway, provided we give the right
@@ -109,13 +115,11 @@ TEST (EssentialMatrixFactor, fromConstraints) {
   // We start with a factor graph and add constraints to it
   // Noise sigma is 1cm, assuming metric measurements
   NonlinearFactorGraph graph;
-  Model model = noiseModel::Isotropic::Sigma(1, 0.01);
   for (size_t i = 0; i < 5; i++)
-    graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model));
+    graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));
 
   // Check error at ground truth
   Values truth;
-  EssentialMatrix trueE(aRb, aTb);
   truth.insert(1, trueE);
   EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
 
@@ -133,35 +137,31 @@ TEST (EssentialMatrixFactor, fromConstraints) {
 
   // Check result
   EssentialMatrix actual = result.at<EssentialMatrix>(1);
-  EXPECT(assert_equal(trueE, actual,1e-1));
+  EXPECT(assert_equal(trueE, actual, 1e-1));
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 
   // Check errors individually
   for (size_t i = 0; i < 5; i++)
-    EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i),vB(i)), 1e-6);
+    EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
 
 }
 
 //*************************************************************************
 TEST (EssentialMatrixFactor2, factor) {
-  EssentialMatrix E(aRb, aTb);
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
-
   for (size_t i = 0; i < 5; i++) {
-    EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model);
+    EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
 
     // Check evaluation
-    Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
+    Point3 P1 = data.tracks[i].p;
-    const Point2 pn = SimpleCamera::project_to_camera(P2);
+    const Point2 pi = camera2.project(P1);
-    const Point2 pi = K.uncalibrate(pn);
     Point2 reprojectionError(pi - pB(i));
     Vector expected = reprojectionError.vector();
 
     Matrix Hactual1, Hactual2;
     LieScalar d(baseline / P1.z());
-    Vector actual = factor.evaluateError(E, d, Hactual1, Hactual2);
+    Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     // Use numerical derivatives to calculate the expected Jacobian
@@ -169,8 +169,8 @@ TEST (EssentialMatrixFactor2, factor) {
     boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
         boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
             boost::none, boost::none);
-    Hexpected1 = numericalDerivative21<EssentialMatrix>(f, E, d);
+    Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
-    Hexpected2 = numericalDerivative22<EssentialMatrix>(f, E, d);
+    Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
 
     // Verify the Jacobian is correct
     EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@@ -185,13 +185,11 @@ TEST (EssentialMatrixFactor2, minimization) {
   // We start with a factor graph and add constraints to it
   // Noise sigma is 1cm, assuming metric measurements
   NonlinearFactorGraph graph;
-  Model model = noiseModel::Isotropic::Sigma(2, 0.01);
   for (size_t i = 0; i < 5; i++)
-    graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
+    graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));
 
   // Check error at ground truth
   Values truth;
-  EssentialMatrix trueE(aRb, aTb);
   truth.insert(100, trueE);
   for (size_t i = 0; i < 5; i++) {
     Point3 P1 = data.tracks[i].p;
@@ -207,14 +205,72 @@ TEST (EssentialMatrixFactor2, minimization) {
 
   // Check result
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
-  EXPECT(assert_equal(trueE, actual,1e-1));
+  EXPECT(assert_equal(trueE, actual, 1e-1));
   for (size_t i = 0; i < 5; i++)
-    EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
+    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 }
 
+//*************************************************************************
+// Below we want to optimize for an essential matrix specified in a
+// body coordinate frame B which is rotated with respect to the camera
+// frame C, via the rotation bRc.
+
+// The "true E" in the body frame is then
+EssentialMatrix bodyE = cRb.inverse() * trueE;
+
+//*************************************************************************
+TEST (EssentialMatrixFactor3, factor) {
+
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);
+
+    // Check evaluation
+    Point3 P1 = data.tracks[i].p;
+    const Point2 pi = camera2.project(P1);
+    Point2 reprojectionError(pi - pB(i));
+    Vector expected = reprojectionError.vector();
+
+    Matrix Hactual1, Hactual2;
+    LieScalar d(baseline / P1.z());
+    Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
+    EXPECT(assert_equal(expected, actual, 1e-7));
+
+    // Use numerical derivatives to calculate the expected Jacobian
+    Matrix Hexpected1, Hexpected2;
+    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
+        boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
+            boost::none, boost::none);
+    Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
+    Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
+
+    // Verify the Jacobian is correct
+    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
+    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
+  }
+}
+
+//*************************************************************************
+TEST (EssentialMatrixFactor3, minimization) {
+
+  // As before, we start with a factor graph and add constraints to it
+  NonlinearFactorGraph graph;
+  for (size_t i = 0; i < 5; i++)
+    // but now we specify the rotation bRc
+    graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2));
+
+  // Check error at ground truth
+  Values truth;
+  truth.insert(100, bodyE);
+  for (size_t i = 0; i < 5; i++) {
+    Point3 P1 = data.tracks[i].p;
+    truth.insert(i, LieScalar(baseline / P1.z()));
+  }
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+}
+
 } // namespace example1
 
 //*************************************************************************
@@ -226,6 +282,8 @@ SfM_data data;
 bool readOK = readBAL(filename, data);
 Rot3 aRb = data.cameras[1].pose().rotation();
 Point3 aTb = data.cameras[1].pose().translation();
+EssentialMatrix trueE(aRb, aTb);
+
 double baseline = 10; // actual baseline of the camera
 
 Point2 pA(size_t i) {
@@ -235,23 +293,99 @@ Point2 pB(size_t i) {
   return data.tracks[i].measurements[1].second;
 }
 
-// Matches Cal3Bundler K(500, 0, 0);
+boost::shared_ptr<Cal3Bundler> //
-Cal3_S2 K(500, 500, 0, 0, 0);
+K = boost::make_shared<Cal3Bundler>(500, 0, 0);
+PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(),*K);
+
+Vector vA(size_t i) {
+  Point2 xy = K->calibrate(pA(i));
+  return EssentialMatrix::Homogeneous(xy);
+}
+Vector vB(size_t i) {
+  Point2 xy = K->calibrate(pB(i));
+  return EssentialMatrix::Homogeneous(xy);
+}
+
+//*************************************************************************
+TEST (EssentialMatrixFactor, extraMinimization) {
+  // Additional test with camera moving in positive X direction
+
+  NonlinearFactorGraph graph;
+  for (size_t i = 0; i < 5; i++)
+    graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K));
+
+  // Check error at ground truth
+  Values truth;
+  truth.insert(1, trueE);
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Check error at initial estimate
+  Values initial;
+  EssentialMatrix initialE = trueE.retract(
+      (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1));
+  initial.insert(1, initialE);
+  EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2);
+
+  // Optimize
+  LevenbergMarquardtParams parameters;
+  LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actual = result.at<EssentialMatrix>(1);
+  EXPECT(assert_equal(trueE, actual, 1e-1));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+
+  // Check errors individually
+  for (size_t i = 0; i < 5; i++)
+    EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
+
+}
 
 //*************************************************************************
 TEST (EssentialMatrixFactor2, extraTest) {
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K);
+
+    // Check evaluation
+    Point3 P1 = data.tracks[i].p;
+    const Point2 pi = camera2.project(P1);
+    Point2 reprojectionError(pi - pB(i));
+    Vector expected = reprojectionError.vector();
+
+    Matrix Hactual1, Hactual2;
+    LieScalar d(baseline / P1.z());
+    Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
+    EXPECT(assert_equal(expected, actual, 1e-7));
+
+    // Use numerical derivatives to calculate the expected Jacobian
+    Matrix Hexpected1, Hexpected2;
+    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
+        boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
+            boost::none, boost::none);
+    Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
+    Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
+
+    // Verify the Jacobian is correct
+    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
+    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
+  }
+}
+
+//*************************************************************************
+TEST (EssentialMatrixFactor2, extraMinimization) {
   // Additional test with camera moving in positive X direction
 
   // We start with a factor graph and add constraints to it
   // Noise sigma is 1, assuming pixel measurements
   NonlinearFactorGraph graph;
-  Model model = noiseModel::Isotropic::Sigma(2, 1);
   for (size_t i = 0; i < data.number_tracks(); i++)
-    graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
+    graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K));
 
   // Check error at ground truth
   Values truth;
-  EssentialMatrix trueE(aRb, aTb);
   truth.insert(100, trueE);
   for (size_t i = 0; i < data.number_tracks(); i++) {
     Point3 P1 = data.tracks[i].p;
@@ -267,16 +401,50 @@ TEST (EssentialMatrixFactor2, extraTest) {
 
   // Check result
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
-  EXPECT(assert_equal(trueE, actual,1e-1));
+  EXPECT(assert_equal(trueE, actual, 1e-1));
   for (size_t i = 0; i < data.number_tracks(); i++)
-    EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
+    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 }
 
+//*************************************************************************
+TEST (EssentialMatrixFactor3, extraTest) {
+
+  // The "true E" in the body frame is
+  EssentialMatrix bodyE = cRb.inverse() * trueE;
+
+  for (size_t i = 0; i < 5; i++) {
+    EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K);
+
+    // Check evaluation
+    Point3 P1 = data.tracks[i].p;
+    const Point2 pi = camera2.project(P1);
+    Point2 reprojectionError(pi - pB(i));
+    Vector expected = reprojectionError.vector();
+
+    Matrix Hactual1, Hactual2;
+    LieScalar d(baseline / P1.z());
+    Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
+    EXPECT(assert_equal(expected, actual, 1e-7));
+
+    // Use numerical derivatives to calculate the expected Jacobian
+    Matrix Hexpected1, Hexpected2;
+    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
+        boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
+            boost::none, boost::none);
+    Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
+    Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
+
+    // Verify the Jacobian is correct
+    EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
+    EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
+  }
 }
 
+} // namespace example2
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;