diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py new file mode 100644 index 000000000..ac3af8a38 --- /dev/null +++ b/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") diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index b2a746595..383c34915 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -86,6 +86,7 @@ struct traits : internal::Manifold {}; /// 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 { diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index d77895d86..b46c5354b 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -377,6 +377,30 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { typedef gtsam::RangeFactor RangeFactorRTV; +#include +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 +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 template diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index c52f1b54c..8ad4a14a6 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -57,6 +57,11 @@ class TOAFactor : public ExpressionFactor { double speed = 330) : TOAFactor(eventExpression, Expression(sensor), toaMeasurement, model, speed) {} + + static void InsertEvent(Key key, const Event& event, + boost::shared_ptr values) { + values->insert(key, event); + } }; } // namespace gtsam