BadFactor is now a functioning NoiseModelFactor

gtsam_unstable/nonlinear/BADFactor.h

@@ -19,6 +19,7 @@
 
 #include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Testable.h>
 
 namespace gtsam {
 
@@ -26,58 +27,46 @@ namespace gtsam {
  * BAD Factor that supports arbitrary expressions via AD
  */
 template<class T>
-class BADFactor: NonlinearFactor {
+class BADFactor: public NoiseModelFactor {
 
   const T measurement_;
   const Expression<T> expression_;
 
-  /// get value from expression and calculate error with respect to measurement
-  Vector unwhitenedError(const Values& values) const {
-    const T& value = expression_.value(values);
-    return value.localCoordinates(measurement_);
-  }
-
 public:
 
   /// Constructor
-  BADFactor(const T& measurement, const Expression<T>& expression) :
+  BADFactor(const SharedNoiseModel& noiseModel, //
+      const T& measurement, const Expression<T>& expression) :
+      NoiseModelFactor(noiseModel, expression.keys()), //
       measurement_(measurement), expression_(expression) {
   }
-  /// Constructor
-  BADFactor(const T& measurement, const ExpressionNode<T>& expression) :
-      measurement_(measurement), expression_(expression) {
-  }
-  /**
-   * Calculate the error of the factor.
-   * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
-   * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
-   * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
-   */
-  virtual double error(const Values& values) const {
-    if (this->active(values)) {
-      const Vector e = unwhitenedError(values);
-      return 0.5 * e.squaredNorm();
-    } else {
-      return 0.0;
-    }
-  }
 
   /// get the dimension of the factor (number of rows on linearization)
   size_t dim() const {
     return measurement_.dim();
   }
 
-  /// linearize to a GaussianFactor
-  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
-    // We will construct an n-ary factor below, where  terms is a container whose
-    // value type is std::pair<Key, Matrix>, specifying the
-    // collection of keys and matrices making up the factor.
-    std::map<Key, Matrix> terms;
-    expression_.value(values, terms);
-    Vector b = unwhitenedError(values);
-    SharedDiagonal model = SharedDiagonal();
-    return boost::shared_ptr<JacobianFactor>(
-        new JacobianFactor(terms, b, model));
+  /**
+   * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
+   * We override this method to provide
+   * both the function evaluation and its derivative(s) in H.
+   */
+  virtual Vector unwhitenedError(const Values& x,
+      boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    if (H) {
+      typedef std::map<Key, Matrix> MapType;
+      MapType terms;
+      const T& value = expression_.value(x, terms);
+      // copy terms to H
+      H->clear();
+      H->reserve(terms.size());
+      for (MapType::iterator it = terms.begin(); it != terms.end(); ++it)
+        H->push_back(it->second);
+      return measurement_.localCoordinates(value);
+    } else {
+      const T& value = expression_.value(x);
+      return measurement_.localCoordinates(value);
+    }
   }
 
 };

gtsam_unstable/nonlinear/tests/testBADFactor.cpp

@@ -59,15 +59,21 @@ TEST(BADFactor, test) {
   Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam);
   Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat);
 
-  // Create factor and check value, dimension, linearization
-  BADFactor<Point2> f(measured, uv_hat);
+  // Create factor and check unwhitenedError
+  BADFactor<Point2> f(model, measured, uv_hat);
+  std::vector<Matrix> H;
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(3, H.size());
+
+  // Check value, dimension, linearization
   EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
   EXPECT( assert_equal(*expected, *gf, 1e-9));
 
   // Try concise version
-  BADFactor<Point2> f2(measured, uncalibrate(K, project(transform_to(x, p))));
+  BADFactor<Point2> f2(model, measured,
+      uncalibrate(K, project(transform_to(x, p))));
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);
@@ -83,13 +89,20 @@ TEST(BADFactor, compose) {
   Rot3_ R3 = R1 * R2;
 
   // Create factor
-  BADFactor<Rot3> f(Rot3(), R3);
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 
   // Create some values
   Values values;
   values.insert(1, Rot3());
   values.insert(2, Rot3());
 
+  // Check unwhitenedError
+  std::vector<Matrix> H;
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(2, H.size());
+  EXPECT( assert_equal(eye(3), H[0],1e-9));
+  EXPECT( assert_equal(eye(3), H[1],1e-9));
+
   // Check linearization
   JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
@@ -107,12 +120,18 @@ TEST(BADFactor, compose2) {
   Rot3_ R3 = R1 * R2;
 
   // Create factor
-  BADFactor<Rot3> f(Rot3(), R3);
+  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 
   // Create some values
   Values values;
   values.insert(1, Rot3());
 
+  // Check unwhitenedError
+  std::vector<Matrix> H;
+  Vector actual = f.unwhitenedError(values, H);
+  EXPECT_LONGS_EQUAL(1, H.size());
+  EXPECT( assert_equal(2*eye(3), H[0],1e-9));
+
   // Check linearization
   JacobianFactor expected(1, 2 * eye(3), zero(3));
   boost::shared_ptr<GaussianFactor> gf = f.linearize(values);