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