diff --git a/cython/gtsam_unstable/examples/TimeOfArrivalExample.py b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py new file mode 100644 index 000000000..6ba06f0f2 --- /dev/null +++ b/cython/gtsam_unstable/examples/TimeOfArrivalExample.py @@ -0,0 +1,128 @@ +""" +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) + +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 define_microphones(): + """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 create_trajectory(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 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 simulate_toa(microphones, trajectory): + """Simulate time-of-arrival measurements for an entire trajectory.""" + return [simulate_one_toa(microphones, event) + for event in trajectory] + + +def create_graph(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 create_initial_estimate(n): + """Create initial estimate for n events.""" + initial = Values() + zero = Event() + for key in range(n): + TOAFactor.InsertEvent(key, zero, initial) + return initial + + +def toa_example(): + """Run example with 4 microphones and 5 events in a straight line.""" + # Create microphones + 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 = create_trajectory(n) + for event in groundTruth: + print(event) + + # Simulate time-of-arrival measurements + 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 = create_graph(microphones, simulatedTOA) + print(graph.at(0)) + + # Create initial estimate + initial_estimate = create_initial_estimate(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__': + toa_example() + print("Example complete") diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp new file mode 100644 index 000000000..9991e04b6 --- /dev/null +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * 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) + + * 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 diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index fc186857f..383c34915 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,44 @@ 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 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 { + 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/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 1df14a8de..8ad4a14a6 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,33 +17,51 @@ * @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 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 speed optional speed of signal, in m/sec */ 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, double speed = 330) + : ExpressionFactor( + model, toaMeasurement, + Double_(TimeOfArrival(speed), 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, + 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 - +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 4b10d5c0b..042130a24 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,83 +44,59 @@ 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 ) { +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(key, sensorAt0, measurement, model); } //***************************************************************************** -TEST( TOAFactor, WholeEnchilada ) { - - static const bool verbose = false; - - // Create microphones +TEST(TOAFactor, WholeEnchilada) { + // 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] = groundTruthEvent.toa(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 - 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(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); + // 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"); - // 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)); } @@ -129,4 +106,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