Merge pull request #252 from borglab/feature/TOA

Time of arrival measurements

cython/gtsam_unstable/examples/TimeOfArrivalExample.py

@@ -0,0 +1,128 @@
+"""
+GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Track a moving object "Time of Arrival" measurements at 4 microphones.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module
+
+from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, Point3, Values, noiseModel_Isotropic)
+from gtsam_unstable import Event, TimeOfArrival, TOAFactor
+
+# units
+MS = 1e-3
+CM = 1e-2
+
+# Instantiate functor with speed of sound value
+TIME_OF_ARRIVAL = TimeOfArrival(330)
+
+
+def define_microphones():
+    """Create microphones."""
+    height = 0.5
+    microphones = []
+    microphones.append(Point3(0, 0, height))
+    microphones.append(Point3(403 * CM, 0, height))
+    microphones.append(Point3(403 * CM, 403 * CM, height))
+    microphones.append(Point3(0, 403 * CM, 2 * height))
+    return microphones
+
+
+def create_trajectory(n):
+    """Create ground truth trajectory."""
+    trajectory = []
+    timeOfEvent = 10
+    # simulate emitting a sound every second while moving on straight line
+    for key in range(n):
+        trajectory.append(
+            Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
+        timeOfEvent += 1
+
+    return trajectory
+
+
+def simulate_one_toa(microphones, event):
+    """Simulate time-of-arrival measurements for a single event."""
+    return [TIME_OF_ARRIVAL.measure(event, microphones[i])
+            for i in range(len(microphones))]
+
+
+def simulate_toa(microphones, trajectory):
+    """Simulate time-of-arrival measurements for an entire trajectory."""
+    return [simulate_one_toa(microphones, event)
+            for event in trajectory]
+
+
+def create_graph(microphones, simulatedTOA):
+    """Create factor graph."""
+    graph = NonlinearFactorGraph()
+
+    # Create a noise model for the TOA error
+    model = noiseModel_Isotropic.Sigma(1, 0.5 * MS)
+
+    K = len(microphones)
+    key = 0
+    for toa in simulatedTOA:
+        for i in range(K):
+            factor = TOAFactor(key, microphones[i], toa[i], model)
+            graph.push_back(factor)
+        key += 1
+
+    return graph
+
+
+def create_initial_estimate(n):
+    """Create initial estimate for n events."""
+    initial = Values()
+    zero = Event()
+    for key in range(n):
+        TOAFactor.InsertEvent(key, zero, initial)
+    return initial
+
+
+def toa_example():
+    """Run example with 4 microphones and 5 events in a straight line."""
+    # Create microphones
+    microphones = define_microphones()
+    K = len(microphones)
+    for i in range(K):
+        print("mic {} = {}".format(i, microphones[i]))
+
+    # Create a ground truth trajectory
+    n = 5
+    groundTruth = create_trajectory(n)
+    for event in groundTruth:
+        print(event)
+
+    # Simulate time-of-arrival measurements
+    simulatedTOA = simulate_toa(microphones, groundTruth)
+    for key in range(n):
+        for i in range(K):
+            print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
+
+    # create factor graph
+    graph = create_graph(microphones, simulatedTOA)
+    print(graph.at(0))
+
+    # Create initial estimate
+    initial_estimate = create_initial_estimate(n)
+    print(initial_estimate)
+
+    # Optimize using Levenberg-Marquardt optimization.
+    params = LevenbergMarquardtParams()
+    params.setAbsoluteErrorTol(1e-10)
+    params.setVerbosityLM("SUMMARY")
+    optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
+    result = optimizer.optimize()
+    print("Final Result:\n", result)
+
+
+if __name__ == '__main__':
+    toa_example()
+    print("Example complete")

gtsam_unstable/examples/TimeOfArrivalExample.cpp

@@ -0,0 +1,160 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  TimeOfArrivalExample.cpp
+ *  @brief Track a moving object "Time of Arrival" measurements at 4
+ * microphones.
+ *  @author Frank Dellaert
+ *  @author Jay Chakravarty
+ *  @date March 2020
+ */
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam_unstable/slam/TOAFactor.h>
+
+#include <boost/bind.hpp>
+#include <boost/format.hpp>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+// units
+static const double ms = 1e-3;
+static const double cm = 1e-2;
+
+// Instantiate functor with speed of sound value
+static const TimeOfArrival kTimeOfArrival(330);
+
+/* ************************************************************************* */
+// Create microphones
+vector<Point3> defineMicrophones() {
+  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));
+  return microphones;
+}
+
+/* ************************************************************************* */
+// Create ground truth trajectory
+vector<Event> createTrajectory(int n) {
+  vector<Event> trajectory;
+  double timeOfEvent = 10;
+  // simulate emitting a sound every second while moving on straight line
+  for (size_t key = 0; key < n; key++) {
+    trajectory.push_back(
+        Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
+    timeOfEvent += 1;
+  }
+  return trajectory;
+}
+
+/* ************************************************************************* */
+// Simulate time-of-arrival measurements for a single event
+vector<double> simulateTOA(const vector<Point3>& microphones,
+                           const Event& event) {
+  size_t K = microphones.size();
+  vector<double> simulatedTOA(K);
+  for (size_t i = 0; i < K; i++) {
+    simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
+  }
+  return simulatedTOA;
+}
+
+/* ************************************************************************* */
+// Simulate time-of-arrival measurements for an entire trajectory
+vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
+                                   const vector<Event>& trajectory) {
+  vector<vector<double>> simulatedTOA;
+  for (auto event : trajectory) {
+    simulatedTOA.push_back(simulateTOA(microphones, event));
+  }
+  return simulatedTOA;
+}
+
+/* ************************************************************************* */
+// create factor graph
+NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
+                                 const vector<vector<double>>& simulatedTOA) {
+  NonlinearFactorGraph graph;
+
+  // Create a noise model for the TOA error
+  auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms);
+
+  size_t K = microphones.size();
+  size_t key = 0;
+  for (auto toa : simulatedTOA) {
+    for (size_t i = 0; i < K; i++) {
+      graph.emplace_shared<TOAFactor>(key, microphones[i], toa[i], model);
+    }
+    key += 1;
+  }
+  return graph;
+}
+
+/* ************************************************************************* */
+// create initial estimate for n events
+Values createInitialEstimate(int n) {
+  Values initial;
+
+  Event zero;
+  for (size_t key = 0; key < n; key++) {
+    initial.insert(key, zero);
+  }
+  return initial;
+}
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  // Create microphones
+  auto microphones = defineMicrophones();
+  size_t K = microphones.size();
+  for (size_t i = 0; i < K; i++) {
+    cout << "mic" << i << " = " << microphones[i] << endl;
+  }
+
+  // Create a ground truth trajectory
+  const size_t n = 5;
+  auto groundTruth = createTrajectory(n);
+
+  // Simulate time-of-arrival measurements
+  auto simulatedTOA = simulateTOA(microphones, groundTruth);
+  for (size_t key = 0; key < n; key++) {
+    for (size_t i = 0; i < K; i++) {
+      cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
+           << endl;
+    }
+  }
+
+  // Create factor graph
+  auto graph = createGraph(microphones, simulatedTOA);
+
+  // Create initial estimate
+  auto initialEstimate = createInitialEstimate(n);
+  initialEstimate.print("Initial Estimate:\n");
+
+  // Optimize using Levenberg-Marquardt optimization.
+  LevenbergMarquardtParams params;
+  params.setAbsoluteErrorTol(1e-10);
+  params.setVerbosityLM("SUMMARY");
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
+  Values result = optimizer.optimize();
+  result.print("Final Result:\n");
+}
+/* ************************************************************************* */

gtsam_unstable/geometry/Event.cpp

@@ -24,15 +24,16 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void Event::print(const std::string& s) const {
-  std::cout << s << "time = " << time_ << "location = " << location_.transpose();
+  std::cout << s << "{'time':" << time_
+            << ", 'location': " << location_.transpose() << "}";
 }
 
 /* ************************************************************************* */
 bool Event::equals(const Event& other, double tol) const {
-  return std::abs(time_ - other.time_) < tol
-      && traits<Point3>::Equals(location_, other.location_, tol);
+  return std::abs(time_ - other.time_) < tol &&
+         traits<Point3>::Equals(location_, other.location_, tol);
 }
 
 /* ************************************************************************* */
 
-} //\ namespace gtsam
+}  // namespace gtsam

gtsam_unstable/geometry/Event.h

@@ -20,42 +20,43 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
+#include <gtsam_unstable/dllexport.h>
+
 #include <cmath>
 #include <iosfwd>
-#include <gtsam_unstable/dllexport.h>
+#include <string>
 
 namespace gtsam {
 
-/// A space-time event
+/**
+ * A space-time event models an event that happens at a certain 3D location, at
+ * a certain time. One use for it is in sound-based or UWB-ranging tracking or
+ * SLAM, where we have "time of arrival" measurements at a set of sensors. The
+ * TOA functor below provides a measurement function for those applications.
+ */
 class Event {
+  double time_;      ///< Time event was generated
+  Point3 location_;  ///< Location at time event was generated
 
-  double time_; ///< Time event was generated
-  Point3 location_; ///< Location at time event was generated
-
-public:
+ public:
   enum { dimension = 4 };
 
   /// Default Constructor
-  Event() :
-      time_(0), location_(0,0,0) {
-  }
+  Event() : time_(0), location_(0, 0, 0) {}
 
   /// Constructor from time and location
-  Event(double t, const Point3& p) :
-      time_(t), location_(p) {
-  }
+  Event(double t, const Point3& p) : time_(t), location_(p) {}
 
   /// Constructor with doubles
-  Event(double t, double x, double y, double z) :
-      time_(t), location_(x, y, z) {
-  }
+  Event(double t, double x, double y, double z)
+      : time_(t), location_(x, y, z) {}
 
-  double time() const { return time_;}
-  Point3 location() const { return location_;}
+  double time() const { return time_; }
+  Point3 location() const { return location_; }
 
-  // TODO we really have to think of a better way to do linear arguments
-  double height(OptionalJacobian<1,4> H = boost::none) const {
-    static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished();
+  // TODO(frank) we really have to think of a better way to do linear arguments
+  double height(OptionalJacobian<1, 4> H = boost::none) const {
+    static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished();
     if (H) *H = JacobianZ;
     return location_.z();
   }
@@ -64,7 +65,8 @@ public:
   GTSAM_UNSTABLE_EXPORT void print(const std::string& s = "") const;
 
   /** equals with an tolerance */
-  GTSAM_UNSTABLE_EXPORT bool equals(const Event& other, double tol = 1e-9) const;
+  GTSAM_UNSTABLE_EXPORT bool equals(const Event& other,
+                                    double tol = 1e-9) const;
 
   /// Updates a with tangent space delta
   inline Event retract(const Vector4& v) const {
@@ -73,28 +75,44 @@ public:
 
   /// Returns inverse retraction
   inline Vector4 localCoordinates(const Event& q) const {
-    return Vector4::Zero(); // TODO
-  }
-
-  /// Time of arrival to given microphone
-  double toa(const Point3& microphone, //
-      OptionalJacobian<1, 4> H1 = boost::none, //
-      OptionalJacobian<1, 3> H2 = boost::none) const {
-    static const double Speed = 330;
-    Matrix13 D1, D2;
-    double distance = gtsam::distance3(location_, microphone, D1, D2);
-    if (H1)
-      // derivative of toa with respect to event
-      *H1 << 1.0, D1 / Speed;
-    if (H2)
-      // derivative of toa with respect to microphone location
-      *H2 << D2 / Speed;
-    return time_ + distance / Speed;
+    return Vector4::Zero();  // TODO(frank) implement!
   }
 };
 
 // Define GTSAM traits
-template<>
+template <>
 struct traits<Event> : internal::Manifold<Event> {};
 
-} //\ namespace gtsam
+/// Time of arrival to given sensor
+class TimeOfArrival {
+  const double speed_;  ///< signal speed
+
+ public:
+  typedef double result_type;
+
+  /// Constructor with optional speed of signal, in m/sec
+  explicit TimeOfArrival(double speed = 330) : speed_(speed) {}
+
+  /// Calculate time of arrival
+  double measure(const Event& event, const Point3& sensor) const {
+    double distance = gtsam::distance3(event.location(), sensor);
+    return event.time() + distance / speed_;
+  }
+
+  /// Calculate time of arrival, with derivatives
+  double operator()(const Event& event, const Point3& sensor,  //
+                    OptionalJacobian<1, 4> H1 = boost::none,   //
+                    OptionalJacobian<1, 3> H2 = boost::none) const {
+    Matrix13 D1, D2;
+    double distance = gtsam::distance3(event.location(), sensor, D1, D2);
+    if (H1)
+      // derivative of toa with respect to event
+      *H1 << 1.0, D1 / speed_;
+    if (H2)
+      // derivative of toa with respect to sensor location
+      *H2 << D2 / speed_;
+    return event.time() + distance / speed_;
+  }
+};
+
+}  // namespace gtsam

gtsam_unstable/geometry/tests/testEvent.cpp

@@ -17,10 +17,12 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/nonlinear/Expression.h>
+#include <gtsam_unstable/geometry/Event.h>
+
 #include <CppUnitLite/TestHarness.h>
+
 #include <boost/bind.hpp>
 
 using namespace std;
@@ -30,56 +32,59 @@ using namespace gtsam;
 static const double ms = 1e-3;
 static const double cm = 1e-2;
 typedef Eigen::Matrix<double, 1, 1> Vector1;
-static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms));
+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 microphoneAt0(0, 0, 0);
+
+static const double kSpeedOfSound = 340;
+static const TimeOfArrival kToa(kSpeedOfSound);
 
 //*****************************************************************************
-TEST( Event, Constructor ) {
+TEST(Event, Constructor) {
   const double t = 0;
   Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm);
 }
 
 //*****************************************************************************
-TEST( Event, Toa1 ) {
+TEST(Event, Toa1) {
   Event event(0, 1, 0, 0);
-  double expected = 1. / 330;
-  EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
+  double expected = 1. / kSpeedOfSound;
+  EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
 }
 
 //*****************************************************************************
-TEST( Event, Toa2 ) {
-  double expectedTOA = timeOfEvent + 1. / 330;
-  EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
+TEST(Event, Toa2) {
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
+  EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
 }
 
 //*************************************************************************
-TEST (Event, Derivatives) {
+TEST(Event, Derivatives) {
   Matrix14 actualH1;
   Matrix13 actualH2;
-  exampleEvent.toa(microphoneAt0, actualH1, actualH2);
+  kToa(exampleEvent, microphoneAt0, actualH1, actualH2);
   Matrix expectedH1 = numericalDerivative11<double, Event>(
-      boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none),
+      boost::bind(kToa, _1, microphoneAt0, boost::none, boost::none),
       exampleEvent);
   EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
   Matrix expectedH2 = numericalDerivative11<double, Point3>(
-      boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none),
+      boost::bind(kToa, exampleEvent, _1, boost::none, boost::none),
       microphoneAt0);
   EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
 }
 
 //*****************************************************************************
-TEST( Event, Expression ) {
+TEST(Event, Expression) {
   Key key = 12;
   Expression<Event> event_(key);
-  Expression<Point3> knownMicrophone_(microphoneAt0); // constant expression
-  Expression<double> expression(&Event::toa, event_, knownMicrophone_);
+  Expression<Point3> knownMicrophone_(microphoneAt0);  // constant expression
+  Expression<double> expression(kToa, event_, knownMicrophone_);
 
   Values values;
   values.insert(key, exampleEvent);
-  double expectedTOA = timeOfEvent + 1. / 330;
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
   EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9);
 }
 
@@ -97,4 +102,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-

gtsam_unstable/gtsam_unstable.h

@@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
 
 typedef gtsam::RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> RangeFactorRTV;
 
+#include <gtsam_unstable/geometry/Event.h>
+class Event {
+  Event();
+  Event(double t, const gtsam::Point3& p);
+  Event(double t, double x, double y, double z);
+  double time() const;
+  gtsam::Point3 location() const;
+  double height() const;
+  void print(string s) const;
+};
+
+class TimeOfArrival {
+  TimeOfArrival();
+  TimeOfArrival(double speed);
+  double measure(const gtsam::Event& event, const gtsam::Point3& sensor) const;
+};
+
+#include <gtsam_unstable/slam/TOAFactor.h>
+virtual class TOAFactor : gtsam::NonlinearFactor {
+  // For now, because of overload issues, we only expose constructor with known sensor coordinates:
+  TOAFactor(size_t key1, gtsam::Point3 sensor, double measured,
+            const gtsam::noiseModel::Base* noiseModel);
+  static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values);
+};
 
 #include <gtsam/nonlinear/NonlinearEquality.h>
 template<T = {gtsam::PoseRTV}>

gtsam_unstable/slam/TOAFactor.h

@@ -17,33 +17,51 @@
  *  @date December 2014
  */
 
+#pragma once
+
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam_unstable/geometry/Event.h>
 
 namespace gtsam {
 
 /// A "Time of Arrival" factor - so little code seems hardly worth it :-)
-class TOAFactor: public ExpressionFactor<double> {
-
+class TOAFactor : public ExpressionFactor<double> {
   typedef Expression<double> Double_;
 
-public:
-
+ public:
   /**
-   * Constructor
-   * @param some expression yielding an event
-   * @param microphone_ expression yielding a microphone location
-   * @param toaMeasurement time of arrival at microphone
+   * 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 speed optional speed of signal, in m/sec
    */
   TOAFactor(const Expression<Event>& eventExpression,
-      const Expression<Point3>& microphone_, double toaMeasurement,
-      const SharedNoiseModel& model) :
-      ExpressionFactor<double>(model, toaMeasurement,
-          Double_(&Event::toa, eventExpression, microphone_)) {
-  }
+            const Expression<Point3>& sensorExpression, double toaMeasurement,
+            const SharedNoiseModel& model, double speed = 330)
+      : ExpressionFactor<double>(
+            model, toaMeasurement,
+            Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
 
+  /**
+   * Constructor with fixed sensor
+   * @param eventExpression expression yielding an event
+   * @param sensor a known sensor location
+   * @param toaMeasurement time of arrival at sensor
+   * @param model noise model
+   * @param toa optional time of arrival functor
+   */
+  TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
+            double toaMeasurement, const SharedNoiseModel& model,
+            double speed = 330)
+      : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
+                  model, speed) {}
+
+  static void InsertEvent(Key key, const Event& event,
+                          boost::shared_ptr<Values> values) {
+    values->insert(key, event);
+  }
 };
 
-} //\ namespace gtsam
-
+}  // namespace gtsam

gtsam_unstable/slam/tests/testTOAFactor.cpp

@@ -17,15 +17,16 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/geometry/Event.h>
-#include <gtsam/nonlinear/ExpressionFactorGraph.h>
-#include <gtsam/nonlinear/expressions.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/base/numericalDerivative.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam_unstable/geometry/Event.h>
+#include <gtsam_unstable/slam/TOAFactor.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <boost/format.hpp>
 #include <boost/bind.hpp>
+#include <boost/format.hpp>
 
 using namespace std;
 using namespace gtsam;
@@ -43,83 +44,59 @@ 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 ) {
+TEST(TOAFactor, NewWay) {
   Key key = 12;
-  Event_ eventExpression(key);
-  Point3_ microphoneConstant(microphoneAt0); // constant expression
   double measurement = 7;
-  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
-  ExpressionFactor<double> factor(model, measurement, expression);
+  TOAFactor factor(key, sensorAt0, measurement, model);
 }
 
 //*****************************************************************************
-TEST( TOAFactor, WholeEnchilada ) {
-
-  static const bool verbose = false;
-
-  // Create microphones
+TEST(TOAFactor, WholeEnchilada) {
+  // 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] = groundTruthEvent.toa(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
-  ExpressionFactorGraph graph;
+  NonlinearFactorGraph graph;
   Key key = 12;
-  Event_ eventExpression(key);
   for (size_t i = 0; i < K; i++) {
-    Point3_ microphone_i(microphones[i]); // constant expression
-    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
-    graph.addExpressionFactor(predictTOA, 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);
+  // Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
   Vector4 delta;
   delta << 0.1, 0.1, -0.1, 0.1;
   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));
 }
@@ -129,4 +106,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-

wrap/python/pybind11

@@ -0,0 +1 @@
+Subproject commit b3bf248eec9cad8260753c982e1ae6cb72fff470