Removed misguided Factory attempt in favor of direct expressions

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -111,88 +111,5 @@ public:
 };
 // ExpressionFactor
 
-/**
- *  ExpressionFactor Factories
- *  They obviate the need for making Factor classes that are almost empty.
- *  They also takes a default noise model.
- *  TODO: Ternary version
- */
-
-/**
- *  A functor that creates unary expression factors on demand
- *  Example usage:
- *    MakeUnaryFactor<double, Event> MakePrior(&Event::height, model);
- *    ExpressionFactor<double> factor = MakePrior(z, eventExpr);
- */
-template<typename T, typename A1>
-class MakeUnaryFactor {
-
-  typedef typename UnaryExpression<T, A1>::Method Method;
-  typedef typename UnaryExpression<T, A1>::Function Function;
-
-  Function function_;
-  SharedNoiseModel model_;
-
-public:
-
-  /// Constructor with a binary function
-  MakeUnaryFactor(Function function, const SharedNoiseModel& model) :
-      function_(function), model_(model) {
-  }
-
-  /// Constructor with a unary method pointer
-  MakeUnaryFactor(Method method, const SharedNoiseModel& model) :
-      function_(boost::bind(method, _1, _2)), model_(model) {
-
-  }
-
-  /// Operator just needs noise model, measurement, and two expressions
-  ExpressionFactor<T> operator()(double measurement,
-      const Expression<A1>& expression1) {
-    // Create expression and return factor
-    Expression<T> expression(function_, expression1);
-    return ExpressionFactor<T>(model_, measurement, expression);
-  }
-
-};
-
-/**
- *  A functor that creates binary expression factors on demand
- *  Example usage:
- *    MakeBinaryFactor<double, Event, Point3> MakeFactor(&Event::toa, model);
- *    ExpressionFactor<double> factor = MakeFactor(z, eventExpr, microphoneExpr);
- */
-template<typename T, typename A1, typename A2>
-class MakeBinaryFactor {
-
-  typedef typename BinaryExpression<T, A1, A2>::Method Method;
-  typedef typename BinaryExpression<T, A1, A2>::Function Function;
-
-  Function function_;
-  SharedNoiseModel model_;
-
-public:
-
-  /// Constructor with a binary function
-  MakeBinaryFactor(Function function, const SharedNoiseModel& model) :
-      function_(function), model_(model) {
-  }
-
-  /// Constructor with a unary method pointer
-  MakeBinaryFactor(Method method, const SharedNoiseModel& model) :
-      function_(boost::bind(method, _1, _2, _3, _4)), model_(model) {
-
-  }
-
-  /// Operator just needs noise model, measurement, and two expressions
-  ExpressionFactor<T> operator()(double measurement,
-      const Expression<A1>& expression1, const Expression<A2>& expression2) {
-    // Create expression and return factor
-    Expression<T> expression(function_, expression1, expression2);
-    return ExpressionFactor<T>(model_, measurement, expression);
-  }
-
-};
-
 } // \ namespace gtsam
 

gtsam_unstable/slam/tests/testTOAFactor.cpp

@@ -30,30 +30,57 @@
 using namespace std;
 using namespace gtsam;
 
-// Create a noise model for the TOA error
+// typedefs
+typedef Eigen::Matrix<double, 1, 1> Vector1;
+typedef Expression<double> Double_;
+typedef Expression<Point3> Point3_;
+typedef Expression<Event> Event_;
+
+// units
 static const double ms = 1e-3;
 static const double cm = 1e-2;
-typedef Eigen::Matrix<double, 1, 1> Vector1;
+
+// Create a noise model for the TOA error
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
 
+// And for height prior
+static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100 * cm));
+
 static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
 static const Point3 microphoneAt0;
 
-// A TOA factor factory
+/**
-static MakeBinaryFactor<double, Event, Point3> MakeFactor(&Event::toa, model);
+ * Factor graph that supports adding Expression Factors directly
+ */
+class ExpressionFactorGraph: public NonlinearFactorGraph {
 
-// A height prior factory
+public:
-static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100*cm));
+
-static MakeUnaryFactor<double, Event> MakePrior(&Event::height, heightModel);
+  /// @name Adding Factors
+  /// @{
+
+  /**
+   * Add an Expression factor directly
+   * Which implements |h(x)-z|^2_R
+   */
+  template<typename T>
+  void addExpressionFactor(const Expression<T>& h, const T& z,
+      const SharedNoiseModel& R) {
+    push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
+  }
+
+  /// @}
+};
 
 //*****************************************************************************
 TEST( TOAFactor, NewWay ) {
   Key key = 12;
-  Expression<Event> eventExpression(key);
+  Event_ eventExpression(key);
-  Expression<Point3> microphoneConstant(microphoneAt0); // constant expression
+  Point3_ microphoneConstant(microphoneAt0); // constant expression
-  double z = 7;
+  double measurement = 7;
-  ExpressionFactor<double> factor = MakeFactor(z, eventExpression, microphoneConstant);
+  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
+  ExpressionFactor<double> factor(model, measurement, expression);
 }
 
 //*****************************************************************************
@@ -67,7 +94,7 @@ TEST( TOAFactor, WholeEnchilada ) {
   microphones.push_back(Point3(0, 0, height));
   microphones.push_back(Point3(403 * cm, 0, height));
   microphones.push_back(Point3(403 * cm, 403 * cm, height));
-  microphones.push_back(Point3(0, 403 * cm, 2*height));
+  microphones.push_back(Point3(0, 403 * cm, 2 * height));
   EXPECT_LONGS_EQUAL(4, microphones.size());
 //  microphones.push_back(Point3(200 * cm, 200 * cm, height));
 
@@ -75,24 +102,25 @@ TEST( TOAFactor, WholeEnchilada ) {
   const double timeOfEvent = 0;
   Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
 
-  // Simulate measurements
+  // Simulate simulatedTOA
   size_t K = microphones.size();
-  vector<double> measurements(K);
+  vector<double> simulatedTOA(K);
   for (size_t i = 0; i < K; i++) {
-    measurements[i] = groundTruthEvent.toa(microphones[i]);
+    simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
     if (verbose) {
       cout << "mic" << i << " = " << microphones[i] << endl;
-      cout << "z" << i << " = " << measurements[i] / ms << endl;
+      cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
     }
   }
 
   // Now, estimate using non-linear optimization
-  NonlinearFactorGraph graph;
+  ExpressionFactorGraph graph;
   Key key = 12;
-  Expression<Event> eventExpression(key);
+  Event_ eventExpression(key);
   for (size_t i = 0; i < K; i++) {
-    Expression<Point3> microphoneConstant(microphones[i]); // constant expression
+    Point3_ microphone_i(microphones[i]); // constant expression
-    graph.add(MakeFactor(measurements[i], eventExpression, microphoneConstant));
+    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
+    graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
   }
 
   /// Print the graph
@@ -129,9 +157,9 @@ TEST( TOAFactor, RealExperiment1 ) {
 
   static const bool verbose = true;
 
-  // Create microphones
+  // Create constant expressions for microphones
   const double height = 0.5;
-  vector<Point3> microphones;
+  vector<Point3_> microphones;
   microphones.push_back(Point3(0, 0, height));
   microphones.push_back(Point3(403 * cm, 0, height));
   microphones.push_back(Point3(403 * cm, 403 * cm, height));
@@ -159,27 +187,31 @@ TEST( TOAFactor, RealExperiment1 ) {
   // Create unknowns and initial estimate
   Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm);
   Values initialEstimate;
-  vector<Expression<Event> > eventExpressions;
+  vector<Event_> unknownEvents;
   for (size_t j = 0; j < 15; j++) {
     initialEstimate.insert(j, nullEvent);
-    eventExpressions.push_back(Expression<Event>(j));
+    unknownEvents.push_back(Event_(j));
   }
 
   // Print
   if (verbose)
     initialEstimate.print("Initial Estimate:\n");
 
-  // Create factor graph and initial estimate
+  // Create factor graph with TOA factors
-  NonlinearFactorGraph graph;
+  ExpressionFactorGraph graph;
   for (size_t i = 0; i < 4; i++) {
-    Expression<Point3> mic_(microphones[i]); // constant expression
+    for (size_t j = 0; j < 15; j++) {
-    for (size_t j = 0; j < 15; j++)
+      Double_ predictTOA_ij(&Event::toa, unknownEvents[j], microphones[i]);
-      graph.add(MakeFactor(data[j][i], eventExpressions[j], mic_));
+      graph.addExpressionFactor(predictTOA_ij, data[j][i], model);
+    }
   }
 
   // Add height priors
-  for (size_t j = 0; j < 15; j++)
+  const double heightPrior = (212 - 45) * cm;
-    graph.add(MakePrior((212 - 45) * cm, eventExpressions[j]));
+  for (size_t j = 0; j < 15; j++) {
+    Double_ height_j(&Event::height, unknownEvents[j]);
+    graph.addExpressionFactor(height_j, heightPrior, heightModel);
+  }
 
   /// Print the graph
   if (verbose)