From 2087075ee787e561e825c84084b0c0e4964a36ca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 17 Mar 2020 14:34:11 -0400 Subject: [PATCH 1/5] Transitioned toa method to a functor --- gtsam_unstable/geometry/Event.h | 91 ++++++++++++--------- gtsam_unstable/geometry/tests/testEvent.cpp | 42 +++++----- gtsam_unstable/slam/TOAFactor.h | 44 ++++++---- gtsam_unstable/slam/tests/testTOAFactor.cpp | 49 +++++------ wrap/python/pybind11 | 1 + 5 files changed, 125 insertions(+), 102 deletions(-) create mode 160000 wrap/python/pybind11 diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index fc186857f..b2a746595 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -20,42 +20,43 @@ #pragma once #include +#include + #include #include -#include +#include 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,37 @@ 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 : internal::Manifold {}; -} //\ 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 diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index ec8ca1f4b..0349f3293 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -17,10 +17,12 @@ * @date December 2014 */ -#include #include #include +#include + #include + #include 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 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( - 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( - 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_(key); - Expression knownMicrophone_(microphoneAt0); // constant expression - Expression expression(&Event::toa, event_, knownMicrophone_); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression 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); } //***************************************************************************** - diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 1df14a8de..af9dce51b 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,33 +17,47 @@ * @date December 2014 */ +#pragma once + #include #include namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) -class TOAFactor: public ExpressionFactor { - +class TOAFactor : public ExpressionFactor { typedef Expression 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 genral 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 */ TOAFactor(const Expression& eventExpression, - const Expression& microphone_, double toaMeasurement, - const SharedNoiseModel& model) : - ExpressionFactor(model, toaMeasurement, - Double_(&Event::toa, eventExpression, microphone_)) { - } + const Expression& sensorExpression, double toaMeasurement, + const SharedNoiseModel& model, + const TimeOfArrival& toa = TimeOfArrival()) + : ExpressionFactor( + 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& eventExpression, const Point3& sensor, + double toaMeasurement, const SharedNoiseModel& model, + const TimeOfArrival& toa = TimeOfArrival()) + : TOAFactor(eventExpression, Expression(sensor), toaMeasurement, + model, toa) {} }; -} //\ namespace gtsam - +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 4b10d5c0b..0e7a19095 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,15 +17,16 @@ * @date December 2014 */ -#include -#include -#include -#include #include +#include +#include +#include +#include +#include #include -#include #include +#include using namespace std; using namespace gtsam; @@ -43,21 +44,18 @@ 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); //***************************************************************************** -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 factor(model, measurement, expression); + TOAFactor factor(eventExpression, microphoneAt0, measurement, model); } //***************************************************************************** -TEST( TOAFactor, WholeEnchilada ) { - +TEST(TOAFactor, WholeEnchilada) { static const bool verbose = false; // Create microphones @@ -68,7 +66,7 @@ TEST( TOAFactor, WholeEnchilada ) { 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)); + // microphones.push_back(Point3(200 * cm, 200 * cm, height)); // Create a ground truth point const double timeOfEvent = 0; @@ -77,8 +75,9 @@ TEST( TOAFactor, WholeEnchilada ) { // Simulate simulatedTOA size_t K = microphones.size(); vector 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,40 +85,35 @@ 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 - Double_ predictTOA(&Event::toa, eventExpression, microphone_i); - graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); + graph.emplace_shared(eventExpression, microphones[i], + simulatedTOA[i], model); } /// Print the graph - if (verbose) - GTSAM_PRINT(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"); + if (verbose) initialEstimate.print("Initial Estimate:\n"); // Optimize using Levenberg-Marquardt optimization. LevenbergMarquardtParams params; params.setAbsoluteErrorTol(1e-10); - if (verbose) - params.setVerbosity("ERROR"); + if (verbose) params.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); - if (verbose) - result.print("Final Result:\n"); + if (verbose) result.print("Final Result:\n"); EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } @@ -129,4 +123,3 @@ int main() { return TestRegistry::runAllTests(tr); } //***************************************************************************** - diff --git a/wrap/python/pybind11 b/wrap/python/pybind11 new file mode 160000 index 000000000..b3bf248ee --- /dev/null +++ b/wrap/python/pybind11 @@ -0,0 +1 @@ +Subproject commit b3bf248eec9cad8260753c982e1ae6cb72fff470 From f3865539c66b495b3684e0ec6bf3d065b8a64374 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 18 Mar 2020 15:44:33 -0400 Subject: [PATCH 2/5] 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 { 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& eventExpression, const Expression& sensorExpression, double toaMeasurement, - const SharedNoiseModel& model, - const TimeOfArrival& toa = TimeOfArrival()) + const SharedNoiseModel& model, double speed = 330) : ExpressionFactor( model, toaMeasurement, - Double_(toa, eventExpression, sensorExpression)) {} + Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {} /** * Constructor with fixed sensor @@ -55,9 +54,9 @@ class TOAFactor : public ExpressionFactor { */ TOAFactor(const Expression& eventExpression, const Point3& sensor, double toaMeasurement, const SharedNoiseModel& model, - const TimeOfArrival& toa = TimeOfArrival()) + double speed = 330) : TOAFactor(eventExpression, Expression(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 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 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 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(eventExpression, microphones[i], - simulatedTOA[i], model); + graph.emplace_shared(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(key), 1e-6)); } From 2e75ffd01da7f65af78751271a01fccc5ce3d813 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 18 Mar 2020 15:44:45 -0400 Subject: [PATCH 3/5] C++ example --- .../examples/TimeOfArrivalExample.cpp | 160 ++++++++++++++++++ gtsam_unstable/geometry/Event.cpp | 9 +- 2 files changed, 165 insertions(+), 4 deletions(-) create mode 100644 gtsam_unstable/examples/TimeOfArrivalExample.cpp diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp new file mode 100644 index 000000000..f49d47fb7 --- /dev/null +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, 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 +#include +#include +#include +#include + +#include +#include + +#include + +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 defineMicrophones() { + const double height = 0.5; + vector 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 createTrajectory(int n) { + vector 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 simulateTOA(const vector& microphones, + const Event& event) { + size_t K = microphones.size(); + vector 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> simulateTOA(const vector& microphones, + const vector& trajectory) { + vector> simulatedTOA; + for (auto event : trajectory) { + simulatedTOA.push_back(simulateTOA(microphones, event)); + } + return simulatedTOA; +} + +/* ************************************************************************* */ +// create factor graph +NonlinearFactorGraph createGraph(const vector& microphones, + const vector>& 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(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"); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index c503514a6..45a24c101 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/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::Equals(location_, other.location_, tol); + return std::abs(time_ - other.time_) < tol && + traits::Equals(location_, other.location_, tol); } /* ************************************************************************* */ -} //\ namespace gtsam +} // namespace gtsam From cd318b2295dd0d23c0119d4a55d7a5a7c810e90a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 18 Mar 2020 17:28:40 -0400 Subject: [PATCH 4/5] Python example and necessary wrapper gymnastics --- .../examples/TimeOfArrivalExample.py | 128 ++++++++++++++++++ gtsam_unstable/geometry/Event.h | 7 + gtsam_unstable/gtsam_unstable.h | 24 ++++ gtsam_unstable/slam/TOAFactor.h | 5 + 4 files changed, 164 insertions(+) create mode 100644 cython/gtsam_unstable/examples/TimeOfArrivalExample.py 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 From 1cfcd2075a6beb8e6c289029feaa9caa6dd9fcb0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Mar 2020 08:59:58 -0400 Subject: [PATCH 5/5] Style and dates --- .../examples/TimeOfArrivalExample.py | 30 +++++++++---------- .../examples/TimeOfArrivalExample.cpp | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py index ac3af8a38..6ba06f0f2 100644 --- a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py +++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -1,5 +1,5 @@ """ -GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +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) @@ -23,7 +23,7 @@ CM = 1e-2 TIME_OF_ARRIVAL = TimeOfArrival(330) -def defineMicrophones(): +def define_microphones(): """Create microphones.""" height = 0.5 microphones = [] @@ -34,7 +34,7 @@ def defineMicrophones(): return microphones -def createTrajectory(n): +def create_trajectory(n): """Create ground truth trajectory.""" trajectory = [] timeOfEvent = 10 @@ -47,19 +47,19 @@ def createTrajectory(n): return trajectory -def simulateOneTOA(microphones, event): +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 simulateTOA(microphones, trajectory): +def simulate_toa(microphones, trajectory): """Simulate time-of-arrival measurements for an entire trajectory.""" - return [simulateOneTOA(microphones, event) + return [simulate_one_toa(microphones, event) for event in trajectory] -def createGraph(microphones, simulatedTOA): +def create_graph(microphones, simulatedTOA): """Create factor graph.""" graph = NonlinearFactorGraph() @@ -77,7 +77,7 @@ def createGraph(microphones, simulatedTOA): return graph -def createInitialEstimate(n): +def create_initial_estimate(n): """Create initial estimate for n events.""" initial = Values() zero = Event() @@ -86,32 +86,32 @@ def createInitialEstimate(n): return initial -def TimeOfArrivalExample(): +def toa_example(): """Run example with 4 microphones and 5 events in a straight line.""" # Create microphones - microphones = defineMicrophones() + 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 = createTrajectory(n) + groundTruth = create_trajectory(n) for event in groundTruth: print(event) # Simulate time-of-arrival measurements - simulatedTOA = simulateTOA(microphones, groundTruth) + 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 = createGraph(microphones, simulatedTOA) + graph = create_graph(microphones, simulatedTOA) print(graph.at(0)) # Create initial estimate - initial_estimate = createInitialEstimate(n) + initial_estimate = create_initial_estimate(n) print(initial_estimate) # Optimize using Levenberg-Marquardt optimization. @@ -124,5 +124,5 @@ def TimeOfArrivalExample(): if __name__ == '__main__': - TimeOfArrivalExample() + toa_example() print("Example complete") diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index f49d47fb7..9991e04b6 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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)