Moved new factor to header; Still fails!

gtsam/slam/EssentialMatrixFactor.h

@@ -9,6 +9,8 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/SimpleCamera.h>
+#include <gtsam/base/LieScalar.h>
 #include <iostream>
 
 namespace gtsam {
@@ -36,8 +38,9 @@ public:
 
   /// @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
   virtual void print(const std::string& s = "",
@@ -56,5 +59,100 @@ public:
 
 };
 
+/**
+ * Binary factor that optimizes for E and inverse depth: assumes measurement
+ * in image 2 is perfect, and returns re-projection error in image 1
+ */
+class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
+    LieScalar> {
+
+  Point2 pA_, pB_; ///< Measurements in image A and B
+  Cal3_S2 K_; ///< Calibration
+
+  typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
+  typedef EssentialMatrixFactor2 This;
+
+public:
+
+  /// Constructor
+  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
+      const Cal3_S2& K, const SharedNoiseModel& model) :
+      Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) {
+  }
+
+  /// @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 << "  EssentialMatrixFactor2 with measurements\n  ("
+        << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
+        << ")'" << std::endl;
+  }
+
+  /// vector of errors returns 1D vector
+  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)
+    // 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
+    Point3 dP1(pA_.x(), pA_.y(), 1);
+
+    // Project to normalized image coordinates, then uncalibrate
+    Point2 pi;
+    if (!DE && !Dd) {
+
+      Point3 _1T2 = E.direction().point3();
+      Point3 d1T2 = d * _1T2;
+      Point3 dP2 = E.rotation().unrotate(dP1 - d1T2);
+      Point2 pn = SimpleCamera::project_to_camera(dP2);
+      pi = K_.uncalibrate(pn);
+
+    } else {
+
+      // TODO, clean up this expensive mess w Mathematica
+
+      Matrix D_1T2_dir; // 3*2
+      Point3 _1T2 = E.direction().point3(D_1T2_dir);
+
+      Point3 d1T2 = d * _1T2;
+
+      Matrix DdP2_rot, DP2_point;
+      Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point);
+
+      Matrix Dpn_dP2; // 2*3
+      Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
+
+      Matrix Dpi_pn; // 2*2
+      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)
+      }
+
+      if (Dd)
+        // (2*2) * (2*3) * (3*3) * (3*1)
+        *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
+
+    }
+    Point2 reprojectionError = pi - pB_;
+    return reprojectionError.vector();
+  }
+
+};
+// EssentialMatrixFactor2
+
 }// gtsam
 

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -6,108 +6,6 @@
  */
 
 #include <gtsam/slam/EssentialMatrixFactor.h>
-#include <gtsam/geometry/SimpleCamera.h>
-#include <gtsam/base/LieScalar.h>
-
-namespace gtsam {
-
-/**
- * Binary factor that optimizes for E and inverse depth: assumes measurement
- * in image 2 is perfect, and returns re-projection error in image 1
- */
-class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
-    LieScalar> {
-
-  Point2 pA_, pB_; ///< Measurements in image A and B
-  Cal3_S2 K_; ///< Calibration
-
-  typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
-  typedef EssentialMatrixFactor2 This;
-
-public:
-
-  /// Constructor
-  EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
-      const Cal3_S2& K, const SharedNoiseModel& model) :
-      Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) {
-  }
-
-  /// @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 << "  EssentialMatrixFactor2 with measurements\n  ("
-        << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
-        << ")'" << std::endl;
-  }
-
-  /// vector of errors returns 1D vector
-  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)
-    // 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
-    Point3 dP1(pA_.x(), pA_.y(), 1);
-
-    // Project to normalized image coordinates, then uncalibrate
-    Point2 pi;
-    if (!DE && !Dd) {
-
-      Point3 _1T2 = E.direction().point3();
-      Point3 d1T2 = d * _1T2;
-      Point3 dP2 = E.rotation().unrotate(dP1 - d1T2);
-      Point2 pn = SimpleCamera::project_to_camera(dP2);
-      pi = K_.uncalibrate(pn);
-
-    } else {
-
-      // TODO, clean up this expensive mess w Mathematica
-
-      Matrix D_1T2_dir; // 3*2
-      Point3 _1T2 = E.direction().point3(D_1T2_dir);
-
-      Point3 d1T2 = d * _1T2;
-
-      Matrix DdP2_rot, DP2_point;
-      Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point);
-
-      Matrix Dpn_dP2; // 2*3
-      Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
-
-      Matrix Dpi_pn; // 2*2
-      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)
-      }
-
-      if (Dd)
-        // (2*2) * (2*3) * (3*3) * (3*1)
-        *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
-
-    }
-    Point2 reprojectionError = pi - pB_;
-    return reprojectionError.vector();
-  }
-
-};
-
-}
-// gtsam
-
 #include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>