From f3865539c66b495b3684e0ec6bf3d065b8a64374 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 18 Mar 2020 15:44:33 -0400
Subject: [PATCH] Refactor TOAFactor and test

---
 gtsam_unstable/slam/TOAFactor.h             | 13 +++----
 gtsam_unstable/slam/tests/testTOAFactor.cpp | 43 +++++++--------------
 2 files changed, 19 insertions(+), 37 deletions(-)

diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h
index af9dce51b..c52f1b54c 100644
--- a/gtsam_unstable/slam/TOAFactor.h
+++ b/gtsam_unstable/slam/TOAFactor.h
@@ -30,20 +30,19 @@ class TOAFactor : public ExpressionFactor<double> {
 
  public:
   /**
-   * Most genral constructor with two expressions
+   * Most general constructor with two expressions
    * @param eventExpression expression yielding an event
    * @param sensorExpression expression yielding a sensor location
    * @param toaMeasurement time of arrival at sensor
    * @param model noise model
-   * @param toa optional time of arrival functor
+   * @param speed optional speed of signal, in m/sec
    */
   TOAFactor(const Expression<Event>& eventExpression,
             const Expression<Point3>& sensorExpression, double toaMeasurement,
-            const SharedNoiseModel& model,
-            const TimeOfArrival& toa = TimeOfArrival())
+            const SharedNoiseModel& model, double speed = 330)
       : ExpressionFactor<double>(
             model, toaMeasurement,
-            Double_(toa, eventExpression, sensorExpression)) {}
+            Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
 
   /**
    * Constructor with fixed sensor
@@ -55,9 +54,9 @@ class TOAFactor : public ExpressionFactor<double> {
    */
   TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
             double toaMeasurement, const SharedNoiseModel& model,
-            const TimeOfArrival& toa = TimeOfArrival())
+            double speed = 330)
       : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
-                  model, toa) {}
+                  model, speed) {}
 };
 
 }  // namespace gtsam
diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp
index 0e7a19095..042130a24 100644
--- a/gtsam_unstable/slam/tests/testTOAFactor.cpp
+++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp
@@ -44,58 +44,46 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));
 
 static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
-static const Point3 microphoneAt0(0, 0, 0);
+static const Point3 sensorAt0(0, 0, 0);
 
 //*****************************************************************************
 TEST(TOAFactor, NewWay) {
   Key key = 12;
-  Event_ eventExpression(key);
   double measurement = 7;
-  TOAFactor factor(eventExpression, microphoneAt0, measurement, model);
+  TOAFactor factor(key, sensorAt0, measurement, model);
 }
 
 //*****************************************************************************
 TEST(TOAFactor, WholeEnchilada) {
-  static const bool verbose = false;
-
-  // Create microphones
+  // Create sensors
   const double height = 0.5;
-  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));
-  microphones.push_back(Point3(0, 403 * cm, 2 * height));
-  EXPECT_LONGS_EQUAL(4, microphones.size());
-  //  microphones.push_back(Point3(200 * cm, 200 * cm, height));
+  vector<Point3> sensors;
+  sensors.push_back(Point3(0, 0, height));
+  sensors.push_back(Point3(403 * cm, 0, height));
+  sensors.push_back(Point3(403 * cm, 403 * cm, height));
+  sensors.push_back(Point3(0, 403 * cm, 2 * height));
+  EXPECT_LONGS_EQUAL(4, sensors.size());
+  //  sensors.push_back(Point3(200 * cm, 200 * cm, height));
 
   // Create a ground truth point
   const double timeOfEvent = 0;
   Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);
 
   // Simulate simulatedTOA
-  size_t K = microphones.size();
+  size_t K = sensors.size();
   vector<double> simulatedTOA(K);
   TimeOfArrival toa;
   for (size_t i = 0; i < K; i++) {
-    simulatedTOA[i] = toa(groundTruthEvent, microphones[i]);
-    if (verbose) {
-      cout << "mic" << i << " = " << microphones[i] << endl;
-      cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
-    }
+    simulatedTOA[i] = toa(groundTruthEvent, sensors[i]);
   }
 
   // Now, estimate using non-linear optimization
   NonlinearFactorGraph graph;
   Key key = 12;
-  Event_ eventExpression(key);
   for (size_t i = 0; i < K; i++) {
-    graph.emplace_shared<TOAFactor>(eventExpression, microphones[i],
-                                    simulatedTOA[i], model);
+    graph.emplace_shared<TOAFactor>(key, sensors[i], simulatedTOA[i], model);
   }
 
-  /// Print the graph
-  if (verbose) GTSAM_PRINT(graph);
-
   // Create initial estimate
   Values initialEstimate;
   // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
@@ -104,16 +92,11 @@ TEST(TOAFactor, WholeEnchilada) {
   Event estimatedEvent = groundTruthEvent.retract(delta);
   initialEstimate.insert(key, estimatedEvent);
 
-  // Print
-  if (verbose) initialEstimate.print("Initial Estimate:\n");
-
   // Optimize using Levenberg-Marquardt optimization.
   LevenbergMarquardtParams params;
   params.setAbsoluteErrorTol(1e-10);
-  if (verbose) params.setVerbosity("ERROR");
   LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
   Values result = optimizer.optimize();
-  if (verbose) result.print("Final Result:\n");
 
   EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
 }