Faster linearize now in class

gtsam/slam/GeneralSFMFactor.h

@@ -116,7 +116,6 @@ namespace gtsam {
     /** h(x)-z */
     Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
         boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
-
       try {
         Point2 reprojError(camera.project2(point,H1,H2) - measured_);
         return reprojError.vector();
@@ -124,12 +123,50 @@ namespace gtsam {
       catch( CheiralityException& e) {
         if (H1) *H1 = zeros(2, DimC);
         if (H2) *H2 = zeros(2, DimL);
-        std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
+        // TODO warn if verbose output asked for
-                              << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
         return zero(2);
       }
     }
 
+    /// Linearize using fixed-size matrices
+    boost::shared_ptr<GaussianFactor> linearize(const Values& values) {
+      // Only linearize if the factor is active
+      if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
+
+      const Key key1 = this->key1(), key2 = this->key2();
+      Eigen::Matrix<double, 2, DimC> H1;
+      Eigen::Matrix<double, 2, DimL> H2;
+      Vector2 b;
+      try {
+        const CAMERA& camera = values.at<CAMERA>(key1);
+        const LANDMARK& point = values.at<LANDMARK>(key2);
+        Point2 reprojError(camera.project2(point, H1, H2) - measured());
+        b = -reprojError.vector();
+      } catch (CheiralityException& e) {
+        // TODO warn if verbose output asked for
+        return boost::make_shared<JacobianFactor>();
+      }
+
+      // Whiten the system if needed
+      const SharedNoiseModel& noiseModel = this->get_noiseModel();
+      if (noiseModel && !noiseModel->isUnit()) {
+        // TODO: implement WhitenSystem for fixed size matrices and include
+        // above
+        H1 = noiseModel->Whiten(H1);
+        H2 = noiseModel->Whiten(H2);
+        b = noiseModel->Whiten(b);
+      }
+
+      if (noiseModel && noiseModel->isConstrained()) {
+        using noiseModel::Constrained;
+        return boost::make_shared<JacobianFactor>(
+            key1, H1, key2, H2, b,
+            boost::static_pointer_cast<Constrained>(noiseModel)->unit());
+      } else {
+        return boost::make_shared<JacobianFactor>(key1, H1, key2, H2, b);
+      }
+    }
+
     /** return the measured */
     inline const Point2 measured() const {
       return measured_;

gtsam/slam/tests/testGeneralSFMFactor.cpp

@@ -430,49 +430,6 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
   EXPECT(assert_equal(expected, actual, 1e-4));
 }
 
-/* ************************************************************************* */
-
-static const int DimC = 11, DimL = 3;
-
-/// Linearize using fixed-size matrices
-boost::shared_ptr<GaussianFactor> linearize(const Projection& factor,
-                                            const Values& values) {
-  // Only linearize if the factor is active
-  if (!factor.active(values)) return boost::shared_ptr<JacobianFactor>();
-
-  const Key key1 = factor.key1(), key2 = factor.key2();
-  Eigen::Matrix<double, 2, DimC> H1;
-  Eigen::Matrix<double, 2, DimL> H2;
-  Vector2 b;
-  try {
-    const GeneralCamera& camera = values.at<GeneralCamera>(key1);
-    const Point3& point = values.at<Point3>(key2);
-    Point2 reprojError(camera.project2(point, H1, H2) - factor.measured());
-    b = -reprojError.vector();
-  } catch (CheiralityException& e) {
-    // TODO warn if verbose output asked for
-    return boost::make_shared<JacobianFactor>();
-  }
-
-  // Whiten the system if needed
-  const SharedNoiseModel& noiseModel = factor.get_noiseModel();
-  if (noiseModel && !noiseModel->isUnit()) {
-    // TODO: implement WhitenSystem for fixed size matrices and include above
-    H1 = noiseModel->Whiten(H1);
-    H2 = noiseModel->Whiten(H2);
-    b = noiseModel->Whiten(b);
-  }
-
-  if (noiseModel && noiseModel->isConstrained()) {
-    using noiseModel::Constrained;
-    return boost::make_shared<JacobianFactor>(
-        key1, H1, key2, H2, b,
-        boost::static_pointer_cast<Constrained>(noiseModel)->unit());
-  } else {
-    return boost::make_shared<JacobianFactor>(key1, H1, key2, H2, b);
-  }
-}
-
 /* ************************************************************************* */
 TEST(GeneralSFMFactor, Linearize) {
   Point2 z(3.,0.);
@@ -490,31 +447,31 @@ TEST(GeneralSFMFactor, Linearize) {
   const SharedNoiseModel model;
   Projection factor(z, model, X(1), L(1));
   GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
-  GaussianFactor::shared_ptr actual = linearize(factor, values);
+  GaussianFactor::shared_ptr actual = factor.linearize(values);
   EXPECT(assert_equal(*expected,*actual,1e-9));
   }
   // Test with Unit Model
   {
   const SharedNoiseModel model(noiseModel::Unit::Create(2));
   Projection factor(z, model, X(1), L(1));
-  GaussianFactor::shared_ptr expected = factor.linearize(values);
+  GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
-  GaussianFactor::shared_ptr actual = linearize(factor, values);
+  GaussianFactor::shared_ptr actual = factor.linearize(values);
   EXPECT(assert_equal(*expected,*actual,1e-9));
   }
   // Test with Isotropic noise
   {
   const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5));
   Projection factor(z, model, X(1), L(1));
-  GaussianFactor::shared_ptr expected = factor.linearize(values);
+  GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
-  GaussianFactor::shared_ptr actual = linearize(factor, values);
+  GaussianFactor::shared_ptr actual = factor.linearize(values);
   EXPECT(assert_equal(*expected,*actual,1e-9));
   }
   // Test with Constrained Model
   {
   const SharedNoiseModel model(noiseModel::Constrained::All(2));
   Projection factor(z, model, X(1), L(1));
-  GaussianFactor::shared_ptr expected = factor.linearize(values);
+  GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values);
-  GaussianFactor::shared_ptr actual = linearize(factor, values);
+  GaussianFactor::shared_ptr actual = factor.linearize(values);
   EXPECT(assert_equal(*expected,*actual,1e-9));
   }
 }