Transitioned toa method to a functor

gtsam_unstable/geometry/Event.h

@@ -20,15 +20,21 @@
 #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
 
@@ -36,24 +42,19 @@ public:
   enum { dimension = 4 };
 
   /// Default Constructor
-  Event() :
+  Event() : time_(0), location_(0, 0, 0) {}
-      time_(0), location_(0,0,0) {
-  }
 
   /// Constructor from time and location
-  Event(double t, const Point3& p) :
+  Event(double t, const Point3& p) : time_(t), location_(p) {}
-      time_(t), location_(p) {
-  }
 
   /// Constructor with doubles
-  Event(double t, double x, double y, double z) :
+  Event(double t, double x, double y, double z)
-      time_(t), location_(x, y, z) {
+      : time_(t), location_(x, y, z) {}
-  }
 
   double time() const { return time_; }
   Point3 location() const { return location_; }
 
-  // TODO we really have to think of a better way to do linear arguments
+  // 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;
@@ -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,23 +75,7 @@ public:
 
   /// Returns inverse retraction
   inline Vector4 localCoordinates(const Event& q) const {
-    return Vector4::Zero(); // TODO
+    return Vector4::Zero();  // TODO(frank) implement!
-  }
-
-  /// 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;
   }
 };
 
@@ -97,4 +83,29 @@ public:
 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 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;
@@ -36,6 +38,9 @@ static const double timeOfEvent = 25;
 static const Event exampleEvent(timeOfEvent, 1, 0, 0);
 static const Point3 microphoneAt0(0, 0, 0);
 
+static const double kSpeedOfSound = 340;
+static const TimeOfArrival kToa(kSpeedOfSound);
+
 //*****************************************************************************
 TEST(Event, Constructor) {
   const double t = 0;
@@ -45,27 +50,27 @@ TEST( Event, Constructor ) {
 //*****************************************************************************
 TEST(Event, Toa1) {
   Event event(0, 1, 0, 0);
-  double expected = 1. / 330;
+  double expected = 1. / kSpeedOfSound;
-  EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(expected, kToa(event, microphoneAt0), 1e-9);
 }
 
 //*****************************************************************************
 TEST(Event, Toa2) {
-  double expectedTOA = timeOfEvent + 1. / 330;
+  double expectedTOA = timeOfEvent + 1. / kSpeedOfSound;
-  EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9);
+  EXPECT_DOUBLES_EQUAL(expectedTOA, kToa(exampleEvent, microphoneAt0), 1e-9);
 }
 
 //*************************************************************************
 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));
 }
@@ -75,11 +80,11 @@ TEST( Event, Expression ) {
   Key key = 12;
   Expression<Event> event_(key);
   Expression<Point3> knownMicrophone_(microphoneAt0);  // constant expression
-  Expression<double> expression(&Event::toa, event_, knownMicrophone_);
+  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/slam/TOAFactor.h

@@ -17,6 +17,8 @@
  *  @date December 2014
  */
 
+#pragma once
+
 #include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam_unstable/geometry/Event.h>
 
@@ -24,26 +26,38 @@ namespace gtsam {
 
 /// A "Time of Arrival" factor - so little code seems hardly worth it :-)
 class TOAFactor : public ExpressionFactor<double> {
-
   typedef Expression<double> Double_;
 
  public:
-
   /**
-   * Constructor
+   * Most genral constructor with two expressions
-   * @param some expression yielding an event
+   * @param eventExpression expression yielding an event
-   * @param microphone_ expression yielding a microphone location
+   * @param sensorExpression expression yielding a sensor location
-   * @param toaMeasurement time of arrival at microphone
+   * @param toaMeasurement time of arrival at sensor
    * @param model noise model
+   * @param toa optional time of arrival functor
    */
   TOAFactor(const Expression<Event>& eventExpression,
-      const Expression<Point3>& microphone_, double toaMeasurement,
+            const Expression<Point3>& sensorExpression, double toaMeasurement,
-      const SharedNoiseModel& model) :
+            const SharedNoiseModel& model,
-      ExpressionFactor<double>(model, toaMeasurement,
+            const TimeOfArrival& toa = TimeOfArrival())
-          Double_(&Event::toa, eventExpression, microphone_)) {
+      : ExpressionFactor<double>(
-  }
+            model, toaMeasurement,
+            Double_(toa, 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,
+            const TimeOfArrival& toa = TimeOfArrival())
+      : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
+                  model, toa) {}
 };
 
-} //\ 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;
@@ -49,15 +50,12 @@ static const Point3 microphoneAt0(0,0,0);
 TEST(TOAFactor, NewWay) {
   Key key = 12;
   Event_ eventExpression(key);
-  Point3_ microphoneConstant(microphoneAt0); // constant expression
   double measurement = 7;
-  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
+  TOAFactor factor(eventExpression, microphoneAt0, measurement, model);
-  ExpressionFactor<double> factor(model, measurement, expression);
 }
 
 //*****************************************************************************
 TEST(TOAFactor, WholeEnchilada) {
-
   static const bool verbose = false;
 
   // Create microphones
@@ -77,8 +75,9 @@ TEST( TOAFactor, WholeEnchilada ) {
   // Simulate simulatedTOA
   size_t K = microphones.size();
   vector<double> simulatedTOA(K);
+  TimeOfArrival toa;
   for (size_t i = 0; i < K; i++) {
-    simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
+    simulatedTOA[i] = toa(groundTruthEvent, microphones[i]);
     if (verbose) {
       cout << "mic" << i << " = " << microphones[i] << endl;
       cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
@@ -86,18 +85,16 @@ TEST( TOAFactor, WholeEnchilada ) {
   }
 
   // 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
+    graph.emplace_shared<TOAFactor>(eventExpression, microphones[i],
-    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
+                                    simulatedTOA[i], model);
-    graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
   }
 
   /// Print the graph
-  if (verbose)
+  if (verbose) GTSAM_PRINT(graph);
-    GTSAM_PRINT(graph);
 
   // Create initial estimate
   Values initialEstimate;
@@ -108,18 +105,15 @@ TEST( TOAFactor, WholeEnchilada ) {
   initialEstimate.insert(key, estimatedEvent);
 
   // Print
-  if (verbose)
+  if (verbose) initialEstimate.print("Initial Estimate:\n");
-    initialEstimate.print("Initial Estimate:\n");
 
   // Optimize using Levenberg-Marquardt optimization.
   LevenbergMarquardtParams params;
   params.setAbsoluteErrorTol(1e-10);
-  if (verbose)
+  if (verbose) params.setVerbosity("ERROR");
-    params.setVerbosity("ERROR");
   LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
   Values result = optimizer.optimize();
-  if (verbose)
+  if (verbose) result.print("Final Result:\n");
-    result.print("Final Result:\n");
 
   EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
 }
@@ -129,4 +123,3 @@ int main() {
   return TestRegistry::runAllTests(tr);
 }
 //*****************************************************************************
-

wrap/python/pybind11

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