Python example and necessary wrapper gymnastics

cython/gtsam_unstable/examples/TimeOfArrivalExample.py

@@ -0,0 +1,128 @@
+"""
+GTSAM Copyright 2010-2018, 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 defineMicrophones():
+    """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 createTrajectory(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 simulateOneTOA(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 simulateTOA(microphones, trajectory):
+    """Simulate time-of-arrival measurements for an entire trajectory."""
+    return [simulateOneTOA(microphones, event)
+            for event in trajectory]
+
+
+def createGraph(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 createInitialEstimate(n):
+    """Create initial estimate for n events."""
+    initial = Values()
+    zero = Event()
+    for key in range(n):
+        TOAFactor.InsertEvent(key, zero, initial)
+    return initial
+
+
+def TimeOfArrivalExample():
+    """Run example with 4 microphones and 5 events in a straight line."""
+    # Create microphones
+    microphones = defineMicrophones()
+    K = len(microphones)
+    for i in range(K):
+        print("mic {} = {}".format(i, microphones[i]))
+
+    # Create a ground truth trajectory
+    n = 5
+    groundTruth = createTrajectory(n)
+    for event in groundTruth:
+        print(event)
+
+    # Simulate time-of-arrival measurements
+    simulatedTOA = simulateTOA(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 = createGraph(microphones, simulatedTOA)
+    print(graph.at(0))
+
+    # Create initial estimate
+    initial_estimate = createInitialEstimate(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__':
+    TimeOfArrivalExample()
+    print("Example complete")

gtsam_unstable/geometry/Event.h

@@ -86,6 +86,7 @@ struct traits<Event> : internal::Manifold<Event> {};
 /// Time of arrival to given sensor
 class TimeOfArrival {
   const double speed_;  ///< signal speed
+
  public:
   typedef double result_type;
 
@@ -93,6 +94,12 @@ class TimeOfArrival {
   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 {

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

@@ -57,6 +57,11 @@ class TOAFactor : public ExpressionFactor<double> {
             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