Development of templated factors

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,14 +67,16 @@ 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;
   }
 
 };
@@ -67,26 +89,50 @@ 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);
+    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,7 +140,7 @@ 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;
   }
 
@@ -112,40 +158,38 @@ public:
     // 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);
-      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();
   }
 
 };

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -18,7 +18,11 @@
 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);
 
 namespace example1 {
 
@@ -42,8 +46,6 @@ Vector vB(size_t i) {
   return EssentialMatrix::Homogeneous(pB(i));
 }
 
-Cal3_S2 K;
-
 //*************************************************************************
 TEST (EssentialMatrixFactor, testData) {
   CHECK(readOK);
@@ -76,10 +78,9 @@ TEST (EssentialMatrixFactor, testData) {
 //*************************************************************************
 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 +101,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,9 +110,8 @@ 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;
@@ -147,15 +147,13 @@ TEST (EssentialMatrixFactor, fromConstraints) {
 //*************************************************************************
 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);
-    const Point2 pn = SimpleCamera::project_to_camera(P2);
+    const Point2 pi = SimpleCamera::project_to_camera(P2);
-    const Point2 pi = K.uncalibrate(pn);
     Point2 reprojectionError(pi - pB(i));
     Vector expected = reprojectionError.vector();
 
@@ -185,9 +183,8 @@ 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;
@@ -235,8 +232,56 @@ 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);
+
+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, extraTest) {
+  // 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;
+  EssentialMatrix trueE(aRb, aTb);
+  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) {
@@ -245,9 +290,8 @@ TEST (EssentialMatrixFactor2, extraTest) {
   // 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;