From 777aa005b483c5f3f6dc2cacf2b74772b671389b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 14:24:10 +0100 Subject: [PATCH 01/39] Developing time of arrival factor with Jay at KU Leuven --- .cproject | 64 +++++++++++ gtsam_unstable/slam/tests/testTOAFactor.cpp | 112 ++++++++++++++++++++ 2 files changed, 176 insertions(+) create mode 100644 gtsam_unstable/slam/tests/testTOAFactor.cpp diff --git a/.cproject b/.cproject index 9c03c5b7d..bb38b29b1 100644 --- a/.cproject +++ b/.cproject @@ -848,6 +848,22 @@ true true + + make + -j4 + testSmartStereoProjectionPoseFactor.run + true + true + true + + + make + -j4 + testTOAFactor.run + true + true + true + make -j5 @@ -2393,6 +2409,46 @@ true true + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run + true + true + true + make -j5 @@ -2902,6 +2958,14 @@ true true + + make + -j4 + testRangeFactor.run + true + true + true + make -j2 diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp new file mode 100644 index 000000000..7f4bbbb38 --- /dev/null +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -0,0 +1,112 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTOAFactor.cpp + * @brief Unit tests for "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include + +namespace gtsam { + +/// A space-time event +class Event { + + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated + +public: + + /// Speed of sound + static const double Speed; + + /// Constructor + Event(double t, double x, double y, double z) : + time_(t), location_(x, y, z) { + } + + /// Time of arrival to given microphone + double toa(const Point3& microphone) { + return time_ + location_.distance(microphone) / Speed; + } +}; + +const double Event::Speed = 330; + +} //\ namespace gtsam + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the TOA error +static const double ms = 1e-3, cm = 1e-2; +typedef Eigen::Matrix Vector1; +static SharedNoiseModel model(noiseModel::Diagonal::Sigmas(Vector1(5. * ms))); + +//***************************************************************************** +TEST( Event, Constructor ) { + const double t = 0; + Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); +} + +//***************************************************************************** +TEST( TOA, Toa1 ) { + Point3 microphone; + Event event(0, 1, 0, 0); + double expected = 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expected, event.toa(microphone), 1e-9); +} + +//***************************************************************************** +TEST( TOA, Toa2 ) { + Point3 microphone; + double timeOfEvent = 25; + Event event(timeOfEvent, 1, 0, 0); + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, event.toa(microphone), 1e-9); +} + +//***************************************************************************** +TEST( TOAFactor, WholeEnchilada ) { + + // Create microphones + vector microphones; + microphones.push_back(Point3(0, 0, 0)); + microphones.push_back(Point3(403 * cm, 0, 0)); + microphones.push_back(Point3(403 * cm, 403 * cm, 0)); + microphones.push_back(Point3(0, 403 * cm, 0)); + EXPECT_LONGS_EQUAL(4, microphones.size()); + + // Create a ground truth point + const double timeOfEvent = 0; + Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); + + // Simulate measurements + vector measurements(4); + for (size_t i = 0; i < 4; i++) + measurements[i] = event.toa(microphones[i]); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + From ba1a2685bc832df640680167f27c1e051d671624 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 15:10:59 +0100 Subject: [PATCH 02/39] Not-working yet, but lots of progress towards TOAFactor --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 109 ++++++++++++++++---- 1 file changed, 89 insertions(+), 20 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 7f4bbbb38..0a734c446 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,6 +17,7 @@ * @date December 2014 */ +#include #include namespace gtsam { @@ -32,21 +33,71 @@ public: /// Speed of sound static const double Speed; + /// Default Constructor + Event() : + time_(0) { + } + /// Constructor Event(double t, double x, double y, double z) : time_(t), location_(x, y, z) { } /// Time of arrival to given microphone - double toa(const Point3& microphone) { + double toa(const Point3& microphone, OptionalJacobian<1, 4> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const { + if (H1) { + + } return time_ + location_.distance(microphone) / Speed; } }; const double Event::Speed = 330; -} //\ namespace gtsam +// Define GTSAM traits +namespace traits { +//template<> +//struct GTSAM_EXPORT is_group : public boost::true_type{ +//}; +// +//template<> +//struct GTSAM_EXPORT is_manifold : public boost::true_type{ +//}; + +template<> +struct GTSAM_EXPORT dimension : public boost::integral_constant { +}; + +} + +/// A "Time of Arrival" factor +class TOAFactor: public ExpressionFactor { + + typedef Expression double_; + +public: + + /** + * Constructor + * @param some expression yielding an event + * @param microphone_ expression yielding a microphone location + * @param toaMeasurement time of arrival at microphone + * @param model noise model + */ + TOAFactor(const Expression& event_, + const Expression& microphone_, double toaMeasurement, + const SharedNoiseModel& model) : + ExpressionFactor(model, toaMeasurement, + double_(&Event::toa, event_, microphone_)) { + } + +}; + +}//\ namespace gtsam + +#include #include #include #include @@ -83,26 +134,44 @@ TEST( TOA, Toa2 ) { } //***************************************************************************** -TEST( TOAFactor, WholeEnchilada ) { - - // Create microphones - vector microphones; - microphones.push_back(Point3(0, 0, 0)); - microphones.push_back(Point3(403 * cm, 0, 0)); - microphones.push_back(Point3(403 * cm, 403 * cm, 0)); - microphones.push_back(Point3(0, 403 * cm, 0)); - EXPECT_LONGS_EQUAL(4, microphones.size()); - - // Create a ground truth point - const double timeOfEvent = 0; - Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); - - // Simulate measurements - vector measurements(4); - for (size_t i = 0; i < 4; i++) - measurements[i] = event.toa(microphones[i]); +TEST( TOA, Expression ) { + Key key = 12; + Expression event_(key); + Point3 microphone; + Expression knownMicrophone_(microphone); // constant expression + Expression expression(&Event::toa, event_, knownMicrophone_); } +//***************************************************************************** +//TEST( TOAFactor, WholeEnchilada ) { +// +// // Create microphones +// vector microphones; +// microphones.push_back(Point3(0, 0, 0)); +// microphones.push_back(Point3(403 * cm, 0, 0)); +// microphones.push_back(Point3(403 * cm, 403 * cm, 0)); +// microphones.push_back(Point3(0, 403 * cm, 0)); +// EXPECT_LONGS_EQUAL(4, microphones.size()); +// +// // Create a ground truth point +// const double timeOfEvent = 0; +// Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); +// +// // Simulate measurements +// vector measurements(4); +// for (size_t i = 0; i < 4; i++) +// measurements[i] = event.toa(microphones[i]); +// +// // Now, estimate using non-linear optimization +// NonlinearFactorGraph graph; +// Key key = 12; +// Expression event_(key); +// for (size_t i = 0; i < 4; i++) { +// Expression knownMicrophone_(microphones[i]); // constant expression +// graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); +// } +//} + //***************************************************************************** int main() { TestResult tr; From d54c70202add8090b2003c417b2471912aaa2f62 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 15:43:31 +0100 Subject: [PATCH 03/39] TOAFactor compiles and can be put into a factor graph --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 81 ++++++++++++--------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 0a734c446..6ce248abe 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -58,14 +58,6 @@ const double Event::Speed = 330; // Define GTSAM traits namespace traits { -//template<> -//struct GTSAM_EXPORT is_group : public boost::true_type{ -//}; -// -//template<> -//struct GTSAM_EXPORT is_manifold : public boost::true_type{ -//}; - template<> struct GTSAM_EXPORT dimension : public boost::integral_constant { }; @@ -140,37 +132,54 @@ TEST( TOA, Expression ) { Point3 microphone; Expression knownMicrophone_(microphone); // constant expression Expression expression(&Event::toa, event_, knownMicrophone_); + +// double timeOfEvent = 25; +// Event event12(timeOfEvent, 1, 0, 0); +// Values values; +// values.insert(key,event12); +// double expectedTOA = timeOfEvent + 1 / Event::Speed; +// EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); } //***************************************************************************** -//TEST( TOAFactor, WholeEnchilada ) { -// -// // Create microphones -// vector microphones; -// microphones.push_back(Point3(0, 0, 0)); -// microphones.push_back(Point3(403 * cm, 0, 0)); -// microphones.push_back(Point3(403 * cm, 403 * cm, 0)); -// microphones.push_back(Point3(0, 403 * cm, 0)); -// EXPECT_LONGS_EQUAL(4, microphones.size()); -// -// // Create a ground truth point -// const double timeOfEvent = 0; -// Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); -// -// // Simulate measurements -// vector measurements(4); -// for (size_t i = 0; i < 4; i++) -// measurements[i] = event.toa(microphones[i]); -// -// // Now, estimate using non-linear optimization -// NonlinearFactorGraph graph; -// Key key = 12; -// Expression event_(key); -// for (size_t i = 0; i < 4; i++) { -// Expression knownMicrophone_(microphones[i]); // constant expression -// graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); -// } -//} +TEST( TOAFactor, Constract ) { + Key key = 12; + Expression event_(key); + Point3 microphone; + Expression knownMicrophone_(microphone); // constant expression + double measurement = 7; + TOAFactor factor(event_, knownMicrophone_, measurement, model); +} + +//***************************************************************************** +TEST( TOAFactor, WholeEnchilada ) { + + // Create microphones + vector microphones; + microphones.push_back(Point3(0, 0, 0)); + microphones.push_back(Point3(403 * cm, 0, 0)); + microphones.push_back(Point3(403 * cm, 403 * cm, 0)); + microphones.push_back(Point3(0, 403 * cm, 0)); + EXPECT_LONGS_EQUAL(4, microphones.size()); + + // Create a ground truth point + const double timeOfEvent = 0; + Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); + + // Simulate measurements + vector measurements(4); + for (size_t i = 0; i < 4; i++) + measurements[i] = event.toa(microphones[i]); + + // Now, estimate using non-linear optimization + NonlinearFactorGraph graph; + Key key = 12; + Expression event_(key); + for (size_t i = 0; i < 4; i++) { + Expression knownMicrophone_(microphones[i]); // constant expression + graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); + } +} //***************************************************************************** int main() { From cca1a54544895e9853220c13e0b868329c8e5df4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 16:02:13 +0100 Subject: [PATCH 04/39] Everything done, except derivatives !!! --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 86 ++++++++++++++++++--- 1 file changed, 76 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 6ce248abe..072e0bcb9 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,8 +17,10 @@ * @date December 2014 */ +#include #include #include +#include namespace gtsam { @@ -38,11 +40,47 @@ public: time_(0) { } - /// Constructor + /// Constructor from time and location + 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) { } + /** print with optional string */ + void print(const std::string& s = "") const { + std::cout << s << ", time = " << time_ << std::endl; + location_.print("location"); + } + + /** equals with an tolerance */ + bool equals(const Event& other, double tol = 1e-9) const { + return std::abs(time_-other.time_) < tol + && location_.equals(other.location_, tol); + } + + /// Manifold stuff: + + size_t dim() const { + return 4; + } + static size_t Dim() { + return 4; + } + + /// Updates a with tangent space delta + inline Event retract(const Vector4& v) const { + return Event(time_ + v[0], location_.retract(v.tail(3))); + } + + /// 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 { @@ -87,7 +125,7 @@ public: }; -}//\ namespace gtsam +} //\ namespace gtsam #include #include @@ -98,9 +136,10 @@ using namespace std; using namespace gtsam; // Create a noise model for the TOA error -static const double ms = 1e-3, cm = 1e-2; +//static const double ms = 1e-3; +static const double cm = 1e-2; typedef Eigen::Matrix Vector1; -static SharedNoiseModel model(noiseModel::Diagonal::Sigmas(Vector1(5. * ms))); +static SharedNoiseModel model(noiseModel::Unit::Create(1)); //***************************************************************************** TEST( Event, Constructor ) { @@ -109,7 +148,7 @@ TEST( Event, Constructor ) { } //***************************************************************************** -TEST( TOA, Toa1 ) { +TEST( Event, Toa1 ) { Point3 microphone; Event event(0, 1, 0, 0); double expected = 1 / Event::Speed; @@ -117,7 +156,7 @@ TEST( TOA, Toa1 ) { } //***************************************************************************** -TEST( TOA, Toa2 ) { +TEST( Event, Toa2 ) { Point3 microphone; double timeOfEvent = 25; Event event(timeOfEvent, 1, 0, 0); @@ -126,7 +165,7 @@ TEST( TOA, Toa2 ) { } //***************************************************************************** -TEST( TOA, Expression ) { +TEST( Event, Expression ) { Key key = 12; Expression event_(key); Point3 microphone; @@ -142,7 +181,15 @@ TEST( TOA, Expression ) { } //***************************************************************************** -TEST( TOAFactor, Constract ) { +TEST(Event, Retract) { + Event event, expected(1, 2, 3, 4); + Vector4 v; + v << 1, 2, 3, 4; + EXPECT(assert_equal(expected, event.retract(v))); +} + +//***************************************************************************** +TEST( TOAFactor, Construct ) { Key key = 12; Expression event_(key); Point3 microphone; @@ -164,12 +211,12 @@ TEST( TOAFactor, WholeEnchilada ) { // Create a ground truth point const double timeOfEvent = 0; - Event event(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); + Event groundTruthEvent(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); // Simulate measurements vector measurements(4); for (size_t i = 0; i < 4; i++) - measurements[i] = event.toa(microphones[i]); + measurements[i] = groundTruthEvent.toa(microphones[i]); // Now, estimate using non-linear optimization NonlinearFactorGraph graph; @@ -179,6 +226,25 @@ TEST( TOAFactor, WholeEnchilada ) { Expression knownMicrophone_(microphones[i]); // constant expression graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); } + + /// Print the graph + GTSAM_PRINT(graph); + + // Create initial estimate + Values initialEstimate; + Event estimatedEvent(timeOfEvent + 0.1, 200 * cm, 150 * cm, 50 * cm); + initialEstimate.insert(key, estimatedEvent); + + // Print + initialEstimate.print("Initial Estimate:\n"); + + // Optimize using Levenberg-Marquardt optimization. + LevenbergMarquardtParams params; + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + EXPECT(assert_equal(groundTruthEvent, result.at(key))); } //***************************************************************************** From 0ceb09262c3d35ef7886aeeeca6c53e09171be8e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 16:06:34 +0100 Subject: [PATCH 05/39] Put in prototype derivatives, needs unit test --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 072e0bcb9..0df342677 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -58,7 +58,7 @@ public: /** equals with an tolerance */ bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_-other.time_) < tol + return std::abs(time_ - other.time_) < tol && location_.equals(other.location_, tol); } @@ -82,12 +82,20 @@ public: } /// Time of arrival to given microphone - double toa(const Point3& microphone, OptionalJacobian<1, 4> H1 = boost::none, + double toa(const Point3& microphone, // + OptionalJacobian<1, 4> H1 = boost::none, // OptionalJacobian<1, 3> H2 = boost::none) const { + Matrix13 D1, D2; + double distance = location_.distance(microphone, D1, D2); if (H1) { - + // derivative of toa with respect to event + *H1 << 1, D1 / Speed; } - return time_ + location_.distance(microphone) / Speed; + if (H2) { + // derivative of toa with respect to microphone location + *H2 << D2 / Speed; + } + return time_ + distance / Speed; } }; From 2dcbc72d8d7ad93c7a5d3eb3f392ce06b4cd0a22 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 17:58:35 +0100 Subject: [PATCH 06/39] Derivatives work !!! --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 67 +++++++++++++-------- 1 file changed, 41 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 0df342677..70c6f3a39 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -52,8 +52,8 @@ public: /** print with optional string */ void print(const std::string& s = "") const { - std::cout << s << ", time = " << time_ << std::endl; - location_.print("location"); + std::cout << s << "time = " << time_; + location_.print("; location"); } /** equals with an tolerance */ @@ -87,14 +87,12 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { Matrix13 D1, D2; double distance = location_.distance(microphone, D1, D2); - if (H1) { + if (H1) // derivative of toa with respect to event - *H1 << 1, D1 / Speed; - } - if (H2) { + *H1 << 1.0, D1 / Speed; + if (H2) // derivative of toa with respect to microphone location *H2 << D2 / Speed; - } return time_ + distance / Speed; } }; @@ -108,6 +106,10 @@ template<> struct GTSAM_EXPORT dimension : public boost::integral_constant { }; +template<> +struct GTSAM_EXPORT is_manifold : public boost::true_type { +}; + } /// A "Time of Arrival" factor @@ -149,6 +151,10 @@ static const double cm = 1e-2; typedef Eigen::Matrix Vector1; static SharedNoiseModel model(noiseModel::Unit::Create(1)); +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + //***************************************************************************** TEST( Event, Constructor ) { const double t = 0; @@ -157,35 +163,43 @@ TEST( Event, Constructor ) { //***************************************************************************** TEST( Event, Toa1 ) { - Point3 microphone; Event event(0, 1, 0, 0); double expected = 1 / Event::Speed; - EXPECT_DOUBLES_EQUAL(expected, event.toa(microphone), 1e-9); + EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); } //***************************************************************************** TEST( Event, Toa2 ) { - Point3 microphone; - double timeOfEvent = 25; - Event event(timeOfEvent, 1, 0, 0); double expectedTOA = timeOfEvent + 1 / Event::Speed; - EXPECT_DOUBLES_EQUAL(expectedTOA, event.toa(microphone), 1e-9); + EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +} + +//************************************************************************* +TEST (Event, Derivatives) { + Matrix14 actualH1; + Matrix13 actualH2; + exampleEvent.toa(microphoneAt0, actualH1, actualH2); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&Event::toa, _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), + microphoneAt0); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } //***************************************************************************** TEST( Event, Expression ) { Key key = 12; Expression event_(key); - Point3 microphone; - Expression knownMicrophone_(microphone); // constant expression + Expression knownMicrophone_(microphoneAt0); // constant expression Expression expression(&Event::toa, event_, knownMicrophone_); -// double timeOfEvent = 25; -// Event event12(timeOfEvent, 1, 0, 0); -// Values values; -// values.insert(key,event12); -// double expectedTOA = timeOfEvent + 1 / Event::Speed; -// EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); + Values values; + values.insert(key, exampleEvent); + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); } //***************************************************************************** @@ -200,8 +214,7 @@ TEST(Event, Retract) { TEST( TOAFactor, Construct ) { Key key = 12; Expression event_(key); - Point3 microphone; - Expression knownMicrophone_(microphone); // constant expression + Expression knownMicrophone_(microphoneAt0); // constant expression double measurement = 7; TOAFactor factor(event_, knownMicrophone_, measurement, model); } @@ -236,11 +249,11 @@ TEST( TOAFactor, WholeEnchilada ) { } /// Print the graph - GTSAM_PRINT(graph); +// GTSAM_PRINT(graph); // Create initial estimate Values initialEstimate; - Event estimatedEvent(timeOfEvent + 0.1, 200 * cm, 150 * cm, 50 * cm); + Event estimatedEvent(timeOfEvent + 10, 200 * cm, 150 * cm, 50 * cm); initialEstimate.insert(key, estimatedEvent); // Print @@ -249,10 +262,12 @@ TEST( TOAFactor, WholeEnchilada ) { // Optimize using Levenberg-Marquardt optimization. LevenbergMarquardtParams params; params.setVerbosity("ERROR"); + params.setAbsoluteErrorTol(1e-10); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); Values result = optimizer.optimize(); + result.print("Final Result:\n"); - EXPECT(assert_equal(groundTruthEvent, result.at(key))); + EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } //***************************************************************************** From 5d6e0bc753451c7b7c1325c25af1244456e70c83 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 19:00:52 +0100 Subject: [PATCH 07/39] Optimization test succeeds, but only with 5th microphone... --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 48 ++++++++++++++------- 1 file changed, 32 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 70c6f3a39..6aa7055cc 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -146,10 +146,10 @@ using namespace std; using namespace gtsam; // Create a noise model for the TOA error -//static const double ms = 1e-3; +static const double ms = 1e-3; static const double cm = 1e-2; typedef Eigen::Matrix Vector1; -static SharedNoiseModel model(noiseModel::Unit::Create(1)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); @@ -222,51 +222,67 @@ TEST( TOAFactor, Construct ) { //***************************************************************************** TEST( TOAFactor, WholeEnchilada ) { + static const bool verbose = false; + // Create microphones + const double height = 0.5; vector microphones; - microphones.push_back(Point3(0, 0, 0)); - microphones.push_back(Point3(403 * cm, 0, 0)); - microphones.push_back(Point3(403 * cm, 403 * cm, 0)); - microphones.push_back(Point3(0, 403 * cm, 0)); + 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, height)); EXPECT_LONGS_EQUAL(4, microphones.size()); + microphones.push_back(Point3(200*cm, 200 * cm, height)); // Create a ground truth point const double timeOfEvent = 0; - Event groundTruthEvent(timeOfEvent, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); + Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); // Simulate measurements - vector measurements(4); - for (size_t i = 0; i < 4; i++) + size_t K = microphones.size(); + vector measurements(K); + for (size_t i = 0; i < K; i++) { measurements[i] = groundTruthEvent.toa(microphones[i]); + if (verbose) { + cout << "mic" << i << " = " << microphones[i] << endl; + cout << "z" << i << " = " << measurements[i]/ms << endl; + } + } // Now, estimate using non-linear optimization NonlinearFactorGraph graph; Key key = 12; Expression event_(key); - for (size_t i = 0; i < 4; i++) { + for (size_t i = 0; i < K; i++) { Expression knownMicrophone_(microphones[i]); // constant expression graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); } /// Print the graph -// GTSAM_PRINT(graph); + if (verbose) + GTSAM_PRINT(graph); // Create initial estimate Values initialEstimate; - Event estimatedEvent(timeOfEvent + 10, 200 * cm, 150 * cm, 50 * 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 - initialEstimate.print("Initial Estimate:\n"); + if (verbose) + initialEstimate.print("Initial Estimate:\n"); // Optimize using Levenberg-Marquardt optimization. LevenbergMarquardtParams params; - params.setVerbosity("ERROR"); params.setAbsoluteErrorTol(1e-10); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + if (verbose) + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); + if (verbose) + result.print("Final Result:\n"); - result.print("Final Result:\n"); EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } From d17caa5487afa2eded7ade7d1cb7684cab9c6fe1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 19:14:18 +0100 Subject: [PATCH 08/39] Event header and test file --- .cproject | 106 ++++++++------ gtsam_unstable/geometry/Event.h | 115 +++++++++++++++ gtsam_unstable/geometry/tests/testEvent.cpp | 100 +++++++++++++ gtsam_unstable/slam/tests/testTOAFactor.cpp | 150 +------------------- 4 files changed, 280 insertions(+), 191 deletions(-) create mode 100644 gtsam_unstable/geometry/Event.h create mode 100644 gtsam_unstable/geometry/tests/testEvent.cpp diff --git a/.cproject b/.cproject index bb38b29b1..70c0e4b04 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j4 + testEvent.run + true + true + true + make -j2 @@ -592,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +688,7 @@ make + tests/testBayesTree true false @@ -1138,6 +1152,7 @@ make + testErrors.run true false @@ -1367,6 +1382,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1449,7 +1504,6 @@ make - testSimulated2DOriented.run true false @@ -1489,7 +1543,6 @@ make - testSimulated2D.run true false @@ -1497,7 +1550,6 @@ make - testSimulated3D.run true false @@ -1511,46 +1563,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1808,6 +1820,7 @@ cpack + -G DEB true false @@ -1815,6 +1828,7 @@ cpack + -G RPM true false @@ -1822,6 +1836,7 @@ cpack + -G TGZ true false @@ -1829,6 +1844,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2683,6 +2699,7 @@ make + testGraph.run true false @@ -2690,6 +2707,7 @@ make + testJunctionTree.run true false @@ -2697,6 +2715,7 @@ make + testSymbolicBayesNetB.run true false @@ -3248,7 +3267,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h new file mode 100644 index 000000000..649abc455 --- /dev/null +++ b/gtsam_unstable/geometry/Event.h @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 Event + * @brief Space-time event + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// A space-time event +class Event { + + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated + +public: + + /// Speed of sound + static const double Speed; + + /// Default Constructor + Event() : + time_(0) { + } + + /// Constructor from time and location + 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) { + } + + /** print with optional string */ + void print(const std::string& s = "") const { + std::cout << s << "time = " << time_; + location_.print("; location"); + } + + /** equals with an tolerance */ + bool equals(const Event& other, double tol = 1e-9) const { + return std::abs(time_ - other.time_) < tol + && location_.equals(other.location_, tol); + } + + /// Manifold stuff: + + size_t dim() const { + return 4; + } + static size_t Dim() { + return 4; + } + + /// Updates a with tangent space delta + inline Event retract(const Vector4& v) const { + return Event(time_ + v[0], location_.retract(v.tail(3))); + } + + /// 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 { + Matrix13 D1, D2; + double distance = location_.distance(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; + } +}; + +const double Event::Speed = 330; + +// Define GTSAM traits +namespace traits { + +template<> +struct GTSAM_EXPORT dimension : public boost::integral_constant { +}; + +template<> +struct GTSAM_EXPORT is_manifold : public boost::true_type { +}; + +} + +} //\ namespace gtsam diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp new file mode 100644 index 000000000..433ca7e7f --- /dev/null +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEvent.cpp + * @brief Unit tests for space time "Event" + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the TOA error +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 const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( Event, Constructor ) { + const double t = 0; + Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); +} + +//***************************************************************************** +TEST( Event, Toa1 ) { + Event event(0, 1, 0, 0); + double expected = 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); +} + +//***************************************************************************** +TEST( Event, Toa2 ) { + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +} + +//************************************************************************* +TEST (Event, Derivatives) { + Matrix14 actualH1; + Matrix13 actualH2; + exampleEvent.toa(microphoneAt0, actualH1, actualH2); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&Event::toa, _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), + microphoneAt0); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + +//***************************************************************************** +TEST( Event, Expression ) { + Key key = 12; + Expression event_(key); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression expression(&Event::toa, event_, knownMicrophone_); + + Values values; + values.insert(key, exampleEvent); + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); +} + +//***************************************************************************** +TEST(Event, Retract) { + Event event, expected(1, 2, 3, 4); + Vector4 v; + v << 1, 2, 3, 4; + EXPECT(assert_equal(expected, event.retract(v))); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 6aa7055cc..3227028e6 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,102 +17,12 @@ * @date December 2014 */ -#include #include -#include -#include +#include namespace gtsam { -/// A space-time event -class Event { - - double time_; ///< Time event was generated - Point3 location_; ///< Location at time event was generated - -public: - - /// Speed of sound - static const double Speed; - - /// Default Constructor - Event() : - time_(0) { - } - - /// Constructor from time and location - 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) { - } - - /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "time = " << time_; - location_.print("; location"); - } - - /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_ - other.time_) < tol - && location_.equals(other.location_, tol); - } - - /// Manifold stuff: - - size_t dim() const { - return 4; - } - static size_t Dim() { - return 4; - } - - /// Updates a with tangent space delta - inline Event retract(const Vector4& v) const { - return Event(time_ + v[0], location_.retract(v.tail(3))); - } - - /// 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 { - Matrix13 D1, D2; - double distance = location_.distance(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; - } -}; - -const double Event::Speed = 330; - -// Define GTSAM traits -namespace traits { - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant { -}; - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type { -}; - -} - -/// A "Time of Arrival" factor +/// A "Time of Arrival" factor - so little code seems hardly worth it :-) class TOAFactor: public ExpressionFactor { typedef Expression double_; @@ -137,6 +47,7 @@ public: } //\ namespace gtsam +#include #include #include #include @@ -155,61 +66,6 @@ static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Point3 microphoneAt0; -//***************************************************************************** -TEST( Event, Constructor ) { - const double t = 0; - Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); -} - -//***************************************************************************** -TEST( Event, Toa1 ) { - Event event(0, 1, 0, 0); - double expected = 1 / Event::Speed; - EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); -} - -//***************************************************************************** -TEST( Event, Toa2 ) { - double expectedTOA = timeOfEvent + 1 / Event::Speed; - EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); -} - -//************************************************************************* -TEST (Event, Derivatives) { - Matrix14 actualH1; - Matrix13 actualH2; - exampleEvent.toa(microphoneAt0, actualH1, actualH2); - Matrix expectedH1 = numericalDerivative11( - boost::bind(&Event::toa, _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), - microphoneAt0); - EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); -} - -//***************************************************************************** -TEST( Event, Expression ) { - Key key = 12; - Expression event_(key); - Expression knownMicrophone_(microphoneAt0); // constant expression - Expression expression(&Event::toa, event_, knownMicrophone_); - - Values values; - values.insert(key, exampleEvent); - double expectedTOA = timeOfEvent + 1 / Event::Speed; - EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); -} - -//***************************************************************************** -TEST(Event, Retract) { - Event event, expected(1, 2, 3, 4); - Vector4 v; - v << 1, 2, 3, 4; - EXPECT(assert_equal(expected, event.retract(v))); -} - //***************************************************************************** TEST( TOAFactor, Construct ) { Key key = 12; From dc84b6589e30a2a2aec84375e02520585063f4ca Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 21:12:38 +0100 Subject: [PATCH 09/39] Added in real experimental data gathered by Jay at KU Leuven --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 90 ++++++++++++++++++--- 1 file changed, 80 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 3227028e6..13a4b0996 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -36,11 +36,11 @@ public: * @param toaMeasurement time of arrival at microphone * @param model noise model */ - TOAFactor(const Expression& event_, + TOAFactor(const Expression& eventExpression, const Expression& microphone_, double toaMeasurement, const SharedNoiseModel& model) : ExpressionFactor(model, toaMeasurement, - double_(&Event::toa, event_, microphone_)) { + double_(&Event::toa, eventExpression, microphone_)) { } }; @@ -60,7 +60,7 @@ 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); @@ -69,10 +69,10 @@ static const Point3 microphoneAt0; //***************************************************************************** TEST( TOAFactor, Construct ) { Key key = 12; - Expression event_(key); + Expression eventExpression(key); Expression knownMicrophone_(microphoneAt0); // constant expression double measurement = 7; - TOAFactor factor(event_, knownMicrophone_, measurement, model); + TOAFactor factor(eventExpression, knownMicrophone_, measurement, model); } //***************************************************************************** @@ -88,7 +88,7 @@ TEST( TOAFactor, WholeEnchilada ) { microphones.push_back(Point3(403 * cm, 403 * cm, height)); microphones.push_back(Point3(0, 403 * cm, 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; @@ -101,17 +101,18 @@ TEST( TOAFactor, WholeEnchilada ) { measurements[i] = groundTruthEvent.toa(microphones[i]); if (verbose) { cout << "mic" << i << " = " << microphones[i] << endl; - cout << "z" << i << " = " << measurements[i]/ms << endl; + cout << "z" << i << " = " << measurements[i] / ms << endl; } } // Now, estimate using non-linear optimization NonlinearFactorGraph graph; Key key = 12; - Expression event_(key); + Expression eventExpression(key); for (size_t i = 0; i < K; i++) { Expression knownMicrophone_(microphones[i]); // constant expression - graph.add(TOAFactor(event_, knownMicrophone_, measurements[i], model)); + graph.add( + TOAFactor(eventExpression, knownMicrophone_, measurements[i], model)); } /// Print the graph @@ -121,7 +122,8 @@ TEST( TOAFactor, WholeEnchilada ) { // Create initial estimate Values initialEstimate; //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); - Vector4 delta; delta << 0.1, 0.1, -0.1, 0.1; + Vector4 delta; + delta << 0.1, 0.1, -0.1, 0.1; Event estimatedEvent = groundTruthEvent.retract(delta); initialEstimate.insert(key, estimatedEvent); @@ -141,6 +143,74 @@ TEST( TOAFactor, WholeEnchilada ) { EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } +//***************************************************************************** +/// Test real data +TEST( TOAFactor, RealExperiment1 ) { + + static const bool verbose = true; + + // Create microphones + 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, height)); + EXPECT_LONGS_EQUAL(4, microphones.size()); + + vector data(15); + size_t i = 0; + data[i++] << 1.2648, 1.2648, 1.2677, 1.2643; + data[i++] << 1.7329, 1.7347, 1.7354, 1.7338; + data[i++] << 2.2475, 2.2551, 2.2538, 2.2474; + data[i++] << 2.6945, 2.696, 2.6958, 2.694; + data[i++] << 3.1486, 3.152, 3.1513, 3.1501; + data[i++] << 3.6145, 3.611, 3.6076, 3.6067; + data[i++] << 4.1003, 4.1004, 4.099, 4.0972; + data[i++] << 4.5732, 4.568, 4.5667, 4.5722; + data[i++] << 5.0482, 5.0458, 5.0443, 5.0453; + data[i++] << 5.5311, 5.5256, 5.5254, 5.5305; + data[i++] << 5.9908, 5.9856, 5.9853, 5.9905; + data[i++] << 6.4575, 6.4524, 6.4527, 6.4579; + data[i++] << 6.8983, 6.8971, 6.8984, 6.9016; + data[i++] << 7.3581, 7.3524, 7.3538, 7.3588; + data[i++] << 7.8286, 7.8286, 7.8302, 7.8353; + + // Create unknowns and initial estimate + Event nullEvent; + Values initialEstimate; + vector > eventExpressions; + for (size_t j = 0; j < 15; j++) { + initialEstimate.insert(j, nullEvent); + eventExpressions.push_back(Expression(j)); + } + + // Print + if (verbose) + initialEstimate.print("Initial Estimate:\n"); + + // Create factor graph and initial estimate + NonlinearFactorGraph graph; + for (size_t i = 0; i < 4; i++) { + Expression mic_(microphones[i]); // constant expression + for (size_t j = 0; j < 15; j++) + graph.add(TOAFactor(eventExpressions[j], mic_, data[j][i], model)); + } + + /// Print the graph + if (verbose) + GTSAM_PRINT(graph); + + // 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"); +} //***************************************************************************** int main() { From bb00e375da8baaf51f6c0de92371a86c3f7855cf Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 21:44:56 +0100 Subject: [PATCH 10/39] Print for MATLAB --- gtsam_unstable/geometry/Event.h | 3 +++ gtsam_unstable/slam/tests/testTOAFactor.cpp | 12 ++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 649abc455..039c963d6 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -50,6 +50,9 @@ public: time_(t), location_(x, y, z) { } + double time() const { return time_;} + Point3 location() const { return location_;} + /** print with optional string */ void print(const std::string& s = "") const { std::cout << s << "time = " << time_; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 13a4b0996..8d98f8db5 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -177,7 +178,7 @@ TEST( TOAFactor, RealExperiment1 ) { data[i++] << 7.8286, 7.8286, 7.8302, 7.8353; // Create unknowns and initial estimate - Event nullEvent; + Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm); Values initialEstimate; vector > eventExpressions; for (size_t j = 0; j < 15; j++) { @@ -209,7 +210,14 @@ TEST( TOAFactor, RealExperiment1 ) { LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); if (verbose) - result.print("Final Result:\n"); + for (size_t j = 0; j < 15; j++) { + Event event = result.at(j); + double t = event.time(); + Point3 p = event.location(); + cout + << boost::format("t(%1%) = %2%;\tlocation(%1%,:) = [%3%, %4%, %5%];") + % (j + 1) % t % p.x() % p.y() % p.z() << endl; + } } //***************************************************************************** From d37216cde3a54e729832c98aeaf50e85eb6c3fa6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 10 Dec 2014 22:50:41 +0100 Subject: [PATCH 11/39] TOAFactor header --- gtsam_unstable/slam/TOAFactor.h | 49 +++++++++++++++++++++ gtsam_unstable/slam/tests/testTOAFactor.cpp | 36 ++------------- 2 files changed, 53 insertions(+), 32 deletions(-) create mode 100644 gtsam_unstable/slam/TOAFactor.h diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h new file mode 100644 index 000000000..b500b36e3 --- /dev/null +++ b/gtsam_unstable/slam/TOAFactor.h @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 TOAFactor.h + * @brief "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include + +namespace gtsam { + +/// A "Time of Arrival" factor - so little code seems hardly worth it :-) +class TOAFactor: public ExpressionFactor { + + typedef Expression double_; + +public: + + /** + * Constructor + * @param some expression yielding an event + * @param microphone_ expression yielding a microphone location + * @param toaMeasurement time of arrival at microphone + * @param model noise model + */ + TOAFactor(const Expression& eventExpression, + const Expression& microphone_, double toaMeasurement, + const SharedNoiseModel& model) : + ExpressionFactor(model, toaMeasurement, + double_(&Event::toa, eventExpression, microphone_)) { + } + +}; + +} //\ namespace gtsam + diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 8d98f8db5..80b5ff9d4 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,41 +17,13 @@ * @date December 2014 */ -#include -#include -#include - -namespace gtsam { - -/// A "Time of Arrival" factor - so little code seems hardly worth it :-) -class TOAFactor: public ExpressionFactor { - - typedef Expression double_; - -public: - - /** - * Constructor - * @param some expression yielding an event - * @param microphone_ expression yielding a microphone location - * @param toaMeasurement time of arrival at microphone - * @param model noise model - */ - TOAFactor(const Expression& eventExpression, - const Expression& microphone_, double toaMeasurement, - const SharedNoiseModel& model) : - ExpressionFactor(model, toaMeasurement, - double_(&Event::toa, eventExpression, microphone_)) { - } - -}; - -} //\ namespace gtsam - +#include #include #include #include + #include +#include #include using namespace std; @@ -148,7 +120,7 @@ TEST( TOAFactor, WholeEnchilada ) { /// Test real data TEST( TOAFactor, RealExperiment1 ) { - static const bool verbose = true; + static const bool verbose = false; // Create microphones const double height = 0.5; From 88b4795b290b76319e42ac7490080b16430b41d9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:18:38 +0100 Subject: [PATCH 12/39] Made some more typedefs, added binary method --- gtsam/nonlinear/Expression-inl.h | 34 +++++++++++++++++++------------- gtsam/nonlinear/Expression.h | 16 ++++++++++++--- 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 48445bba5..1074400c9 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -562,8 +562,7 @@ struct GenerateFunctionalNode: Argument, Base { /// Given df/dT, multiply in dT/dA and continue reverse AD process // Cols is always known at compile time template - void reverseAD4(const SomeMatrix & dFdT, - JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD4(dFdT, jacobians); This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } @@ -669,10 +668,12 @@ struct FunctionalNode { template class UnaryExpression: public FunctionalNode >::type { + typedef typename MakeOptionalJacobian::type OJ1; + public: - typedef boost::function< - T(const A1&, typename MakeOptionalJacobian::type)> Function; + typedef T (A1::*Method)(OJ1) const; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -712,13 +713,16 @@ public: /// Binary Expression template -class BinaryExpression: public FunctionalNode >::type { +class BinaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; public: - typedef boost::function< - T(const A1&, const A2&, typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type)> Function; + typedef T (A1::*Method)(const A2&, OJ1, OJ2) const; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -766,15 +770,17 @@ public: /// Ternary Expression template -class TernaryExpression: public FunctionalNode >::type { +class TernaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; + typedef typename MakeOptionalJacobian::type OJ3; public: - typedef boost::function< - T(const A1&, const A2&, const A3&, - typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type)> Function; + typedef T (A1::*Method)(const A2&, const A3&, OJ1, OJ2, OJ3) const; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index e63fbc686..cf87ac2ce 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -75,7 +75,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename MakeOptionalJacobian::type) const) : + typename UnaryExpression::Method method) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,8 +89,7 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type) const, + typename BinaryExpression::Method method, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -104,6 +103,17 @@ public: root_(new BinaryExpression(function, expression1, expression2)) { } + /// Construct a binary method expression + template + Expression(const Expression& expression1, + typename TernaryExpression::Method method, + const Expression& expression2, const Expression& expression3) : + root_( + new TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { + } + /// Construct a ternary function expression template Expression(typename TernaryExpression::Function function, From 96d4ae7eb1be78aa5b163bcc5914000bd4a9e45e Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:18:52 +0100 Subject: [PATCH 13/39] Removed LieScalar --- gtsam/nonlinear/tests/testExpression.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 1ea6236e8..80100af5e 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include @@ -78,9 +77,6 @@ namespace unary { Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) { - return LieScalar(0.0); -} double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } @@ -92,11 +88,6 @@ TEST(Expression, Unary0) { Expression e(f0, p); EXPECT(expected == e.keys()); } -TEST(Expression, Unary1) { - using namespace unary; - Expression e(f1, p); - EXPECT(expected == e.keys()); -} TEST(Expression, Unary2) { using namespace unary; Expression e(f2, p); From f588a3928acf6c323abd56b54a989b89bcb9cd44 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:19:08 +0100 Subject: [PATCH 14/39] Added handy dandy Factor Factory --- gtsam_unstable/nonlinear/ExpressionFactor.h | 43 ++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 56e72a807..af4b8a8ad 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -111,5 +111,46 @@ public: }; // ExpressionFactor -} +/** + * A functor that creates binary expression factors on demand + * Example usage: + * MakeBinaryFactor make(&Event::toa, model); + * ExpressionFactor factor = make(z, eventExpr, microphoneExpr); + * This obviates the need for making Factor classes that are almost empty. + * It also takes a default noise model. + * TODO: unary and ternary versions + */ +template +class MakeBinaryFactor { + + typedef typename BinaryExpression::Method Method; + typedef typename BinaryExpression::Function Function; + + Function function_; + SharedNoiseModel model_; + +public: + + /// Constructor with a binary function + MakeBinaryFactor(Function function, const SharedNoiseModel& model) : + function_(function), model_(model) { + } + + /// Constructor with a unary method pointer + MakeBinaryFactor(Method method, const SharedNoiseModel& model) : + function_(boost::bind(method, _1, _2, _3, _4)), model_(model) { + + } + + /// Operator just needs noise model, measurement, and two expressions + ExpressionFactor operator()(double measurement, + const Expression& expression1, const Expression& expression2) { + // Create expression and return factor + Expression expression(function_, expression1, expression2); + return ExpressionFactor(model_, measurement, expression); + } + +}; + +} // \ namespace gtsam From 29dc382c226cdefedbd96e2edc6db7a3fd204530 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:19:19 +0100 Subject: [PATCH 15/39] Fixed small bug --- gtsam_unstable/timing/timeCameraExpression.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/gtsam_unstable/timing/timeCameraExpression.cpp index 92522c440..f872ce78a 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/gtsam_unstable/timing/timeCameraExpression.cpp @@ -32,7 +32,7 @@ using namespace gtsam; boost::shared_ptr fixedK(new Cal3_S2()); Point2 myProject(const Pose3& pose, const Point3& point, - boost::optional H1, boost::optional H2) { + OptionalJacobian<2, 6> H1, OptionalJacobian<2, 3> H2) { PinholeCamera camera(pose, *fixedK); return camera.project(point, H1, H2, boost::none); } From cedf2647d23630c3166e0c05e0f497c9e6b4f23c Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:19:37 +0100 Subject: [PATCH 16/39] Use handy dandy Factor Factory --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 80b5ff9d4..6944befbf 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,7 +17,8 @@ * @date December 2014 */ -#include +#include +#include #include #include #include @@ -39,13 +40,16 @@ static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Point3 microphoneAt0; +// A TOA factor factory :-) +MakeBinaryFactor makeFactor(&Event::toa, model); + //***************************************************************************** -TEST( TOAFactor, Construct ) { +TEST( TOAFactor, NewWay ) { Key key = 12; Expression eventExpression(key); - Expression knownMicrophone_(microphoneAt0); // constant expression - double measurement = 7; - TOAFactor factor(eventExpression, knownMicrophone_, measurement, model); + Expression microphoneConstant(microphoneAt0); // constant expression + double z = 7; + ExpressionFactor factor = makeFactor(z, eventExpression, microphoneConstant); } //***************************************************************************** @@ -83,9 +87,8 @@ TEST( TOAFactor, WholeEnchilada ) { Key key = 12; Expression eventExpression(key); for (size_t i = 0; i < K; i++) { - Expression knownMicrophone_(microphones[i]); // constant expression - graph.add( - TOAFactor(eventExpression, knownMicrophone_, measurements[i], model)); + Expression microphoneConstant(microphones[i]); // constant expression + graph.add(makeFactor(measurements[i], eventExpression, microphoneConstant)); } /// Print the graph @@ -167,7 +170,7 @@ TEST( TOAFactor, RealExperiment1 ) { for (size_t i = 0; i < 4; i++) { Expression mic_(microphones[i]); // constant expression for (size_t j = 0; j < 15; j++) - graph.add(TOAFactor(eventExpressions[j], mic_, data[j][i], model)); + graph.add(makeFactor(data[j][i], eventExpressions[j], mic_)); } /// Print the graph From 31ca0d76dbf6f245b5add2621f999b905f14ca27 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:44:47 +0100 Subject: [PATCH 17/39] Added unary version --- gtsam_unstable/nonlinear/ExpressionFactor.h | 52 +++++++++++++++++++-- 1 file changed, 47 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index af4b8a8ad..680fd9878 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -111,14 +111,56 @@ public: }; // ExpressionFactor +/** + * ExpressionFactor Factories + * They obviate the need for making Factor classes that are almost empty. + * They also takes a default noise model. + * TODO: Ternary version + */ + +/** + * A functor that creates unary expression factors on demand + * Example usage: + * MakeUnaryFactor MakePrior(&Event::height, model); + * ExpressionFactor factor = MakePrior(z, eventExpr); + */ +template +class MakeUnaryFactor { + + typedef typename UnaryExpression::Method Method; + typedef typename UnaryExpression::Function Function; + + Function function_; + SharedNoiseModel model_; + +public: + + /// Constructor with a binary function + MakeUnaryFactor(Function function, const SharedNoiseModel& model) : + function_(function), model_(model) { + } + + /// Constructor with a unary method pointer + MakeUnaryFactor(Method method, const SharedNoiseModel& model) : + function_(boost::bind(method, _1, _2)), model_(model) { + + } + + /// Operator just needs noise model, measurement, and two expressions + ExpressionFactor operator()(double measurement, + const Expression& expression1) { + // Create expression and return factor + Expression expression(function_, expression1); + return ExpressionFactor(model_, measurement, expression); + } + +}; + /** * A functor that creates binary expression factors on demand * Example usage: - * MakeBinaryFactor make(&Event::toa, model); - * ExpressionFactor factor = make(z, eventExpr, microphoneExpr); - * This obviates the need for making Factor classes that are almost empty. - * It also takes a default noise model. - * TODO: unary and ternary versions + * MakeBinaryFactor MakeFactor(&Event::toa, model); + * ExpressionFactor factor = MakeFactor(z, eventExpr, microphoneExpr); */ template class MakeBinaryFactor { From a9121fc3fcb47bc4abc3ff13188b3007f4ddbce4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Dec 2014 13:45:15 +0100 Subject: [PATCH 18/39] Now it's super-easy to create priors on pieces of state. --- gtsam_unstable/geometry/Event.h | 8 +++++++ gtsam_unstable/slam/tests/testTOAFactor.cpp | 26 ++++++++++++++------- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 039c963d6..143f849bc 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -34,6 +34,7 @@ public: /// Speed of sound static const double Speed; + static const Matrix14 JacobianZ; /// Default Constructor Event() : @@ -53,6 +54,12 @@ public: 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 { + if (H) *H = JacobianZ; + return location_.z(); + } + /** print with optional string */ void print(const std::string& s = "") const { std::cout << s << "time = " << time_; @@ -101,6 +108,7 @@ public: }; const double Event::Speed = 330; +const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); // Define GTSAM traits namespace traits { diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 6944befbf..ef95917e1 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -40,8 +40,12 @@ static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Point3 microphoneAt0; -// A TOA factor factory :-) -MakeBinaryFactor makeFactor(&Event::toa, model); +// A TOA factor factory +static MakeBinaryFactor MakeFactor(&Event::toa, model); + +// A height prior factory +static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100*cm)); +static MakeUnaryFactor MakePrior(&Event::height, heightModel); //***************************************************************************** TEST( TOAFactor, NewWay ) { @@ -49,7 +53,7 @@ TEST( TOAFactor, NewWay ) { Expression eventExpression(key); Expression microphoneConstant(microphoneAt0); // constant expression double z = 7; - ExpressionFactor factor = makeFactor(z, eventExpression, microphoneConstant); + ExpressionFactor factor = MakeFactor(z, eventExpression, microphoneConstant); } //***************************************************************************** @@ -63,9 +67,9 @@ TEST( TOAFactor, WholeEnchilada ) { 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, 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; @@ -88,7 +92,7 @@ TEST( TOAFactor, WholeEnchilada ) { Expression eventExpression(key); for (size_t i = 0; i < K; i++) { Expression microphoneConstant(microphones[i]); // constant expression - graph.add(makeFactor(measurements[i], eventExpression, microphoneConstant)); + graph.add(MakeFactor(measurements[i], eventExpression, microphoneConstant)); } /// Print the graph @@ -123,7 +127,7 @@ TEST( TOAFactor, WholeEnchilada ) { /// Test real data TEST( TOAFactor, RealExperiment1 ) { - static const bool verbose = false; + static const bool verbose = true; // Create microphones const double height = 0.5; @@ -170,10 +174,14 @@ TEST( TOAFactor, RealExperiment1 ) { for (size_t i = 0; i < 4; i++) { Expression mic_(microphones[i]); // constant expression for (size_t j = 0; j < 15; j++) - graph.add(makeFactor(data[j][i], eventExpressions[j], mic_)); + graph.add(MakeFactor(data[j][i], eventExpressions[j], mic_)); } - /// Print the graph + // Add height priors + for (size_t j = 0; j < 15; j++) + graph.add(MakePrior((212 - 45) * cm, eventExpressions[j])); + + /// Print the graph if (verbose) GTSAM_PRINT(graph); From 8cc813f03ee686b634f0c9e31cd45009a2b82015 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 07:31:33 +0100 Subject: [PATCH 19/39] Removed misguided Factory attempt in favor of direct expressions --- gtsam_unstable/nonlinear/ExpressionFactor.h | 83 ------------------ gtsam_unstable/slam/tests/testTOAFactor.cpp | 96 ++++++++++++++------- 2 files changed, 64 insertions(+), 115 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 680fd9878..94d79fbd4 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -111,88 +111,5 @@ public: }; // ExpressionFactor -/** - * ExpressionFactor Factories - * They obviate the need for making Factor classes that are almost empty. - * They also takes a default noise model. - * TODO: Ternary version - */ - -/** - * A functor that creates unary expression factors on demand - * Example usage: - * MakeUnaryFactor MakePrior(&Event::height, model); - * ExpressionFactor factor = MakePrior(z, eventExpr); - */ -template -class MakeUnaryFactor { - - typedef typename UnaryExpression::Method Method; - typedef typename UnaryExpression::Function Function; - - Function function_; - SharedNoiseModel model_; - -public: - - /// Constructor with a binary function - MakeUnaryFactor(Function function, const SharedNoiseModel& model) : - function_(function), model_(model) { - } - - /// Constructor with a unary method pointer - MakeUnaryFactor(Method method, const SharedNoiseModel& model) : - function_(boost::bind(method, _1, _2)), model_(model) { - - } - - /// Operator just needs noise model, measurement, and two expressions - ExpressionFactor operator()(double measurement, - const Expression& expression1) { - // Create expression and return factor - Expression expression(function_, expression1); - return ExpressionFactor(model_, measurement, expression); - } - -}; - -/** - * A functor that creates binary expression factors on demand - * Example usage: - * MakeBinaryFactor MakeFactor(&Event::toa, model); - * ExpressionFactor factor = MakeFactor(z, eventExpr, microphoneExpr); - */ -template -class MakeBinaryFactor { - - typedef typename BinaryExpression::Method Method; - typedef typename BinaryExpression::Function Function; - - Function function_; - SharedNoiseModel model_; - -public: - - /// Constructor with a binary function - MakeBinaryFactor(Function function, const SharedNoiseModel& model) : - function_(function), model_(model) { - } - - /// Constructor with a unary method pointer - MakeBinaryFactor(Method method, const SharedNoiseModel& model) : - function_(boost::bind(method, _1, _2, _3, _4)), model_(model) { - - } - - /// Operator just needs noise model, measurement, and two expressions - ExpressionFactor operator()(double measurement, - const Expression& expression1, const Expression& expression2) { - // Create expression and return factor - Expression expression(function_, expression1, expression2); - return ExpressionFactor(model_, measurement, expression); - } - -}; - } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index ef95917e1..ae32211be 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -30,30 +30,57 @@ using namespace std; using namespace gtsam; -// Create a noise model for the TOA error +// typedefs +typedef Eigen::Matrix Vector1; +typedef Expression Double_; +typedef Expression Point3_; +typedef Expression Event_; + +// units static const double ms = 1e-3; static const double cm = 1e-2; -typedef Eigen::Matrix Vector1; + +// Create a noise model for the TOA error static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); +// And for height prior +static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100 * cm)); + static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Point3 microphoneAt0; -// A TOA factor factory -static MakeBinaryFactor MakeFactor(&Event::toa, model); +/** + * Factor graph that supports adding Expression Factors directly + */ +class ExpressionFactorGraph: public NonlinearFactorGraph { -// A height prior factory -static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100*cm)); -static MakeUnaryFactor MakePrior(&Event::height, heightModel); +public: + + /// @name Adding Factors + /// @{ + + /** + * Add an Expression factor directly + * Which implements |h(x)-z|^2_R + */ + template + void addExpressionFactor(const Expression& h, const T& z, + const SharedNoiseModel& R) { + push_back(boost::make_shared >(R, z, h)); + } + + /// @} +}; //***************************************************************************** TEST( TOAFactor, NewWay ) { Key key = 12; - Expression eventExpression(key); - Expression microphoneConstant(microphoneAt0); // constant expression - double z = 7; - ExpressionFactor factor = MakeFactor(z, eventExpression, microphoneConstant); + Event_ eventExpression(key); + Point3_ microphoneConstant(microphoneAt0); // constant expression + double measurement = 7; + Double_ expression(&Event::toa, eventExpression, microphoneConstant); + ExpressionFactor factor(model, measurement, expression); } //***************************************************************************** @@ -67,7 +94,7 @@ TEST( TOAFactor, WholeEnchilada ) { 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)); + microphones.push_back(Point3(0, 403 * cm, 2 * height)); EXPECT_LONGS_EQUAL(4, microphones.size()); // microphones.push_back(Point3(200 * cm, 200 * cm, height)); @@ -75,24 +102,25 @@ TEST( TOAFactor, WholeEnchilada ) { const double timeOfEvent = 0; Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); - // Simulate measurements + // Simulate simulatedTOA size_t K = microphones.size(); - vector measurements(K); + vector simulatedTOA(K); for (size_t i = 0; i < K; i++) { - measurements[i] = groundTruthEvent.toa(microphones[i]); + simulatedTOA[i] = groundTruthEvent.toa(microphones[i]); if (verbose) { cout << "mic" << i << " = " << microphones[i] << endl; - cout << "z" << i << " = " << measurements[i] / ms << endl; + cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; } } // Now, estimate using non-linear optimization - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; Key key = 12; - Expression eventExpression(key); + Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { - Expression microphoneConstant(microphones[i]); // constant expression - graph.add(MakeFactor(measurements[i], eventExpression, microphoneConstant)); + Point3_ microphone_i(microphones[i]); // constant expression + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); + graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); } /// Print the graph @@ -129,9 +157,9 @@ TEST( TOAFactor, RealExperiment1 ) { static const bool verbose = true; - // Create microphones + // Create constant expressions for microphones const double height = 0.5; - vector microphones; + 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)); @@ -159,29 +187,33 @@ TEST( TOAFactor, RealExperiment1 ) { // Create unknowns and initial estimate Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm); Values initialEstimate; - vector > eventExpressions; + vector unknownEvents; for (size_t j = 0; j < 15; j++) { initialEstimate.insert(j, nullEvent); - eventExpressions.push_back(Expression(j)); + unknownEvents.push_back(Event_(j)); } // Print if (verbose) initialEstimate.print("Initial Estimate:\n"); - // Create factor graph and initial estimate - NonlinearFactorGraph graph; + // Create factor graph with TOA factors + ExpressionFactorGraph graph; for (size_t i = 0; i < 4; i++) { - Expression mic_(microphones[i]); // constant expression - for (size_t j = 0; j < 15; j++) - graph.add(MakeFactor(data[j][i], eventExpressions[j], mic_)); + for (size_t j = 0; j < 15; j++) { + Double_ predictTOA_ij(&Event::toa, unknownEvents[j], microphones[i]); + graph.addExpressionFactor(predictTOA_ij, data[j][i], model); + } } // Add height priors - for (size_t j = 0; j < 15; j++) - graph.add(MakePrior((212 - 45) * cm, eventExpressions[j])); + const double heightPrior = (212 - 45) * cm; + for (size_t j = 0; j < 15; j++) { + Double_ height_j(&Event::height, unknownEvents[j]); + graph.addExpressionFactor(height_j, heightPrior, heightModel); + } - /// Print the graph + /// Print the graph if (verbose) GTSAM_PRINT(graph); From fa7071bdd8acb4203af775440030053b99701af6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 07:45:09 +0100 Subject: [PATCH 20/39] Comments, renaming --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index ae32211be..0565cbcbd 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -61,8 +61,10 @@ public: /// @{ /** - * Add an Expression factor directly - * Which implements |h(x)-z|^2_R + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model */ template void addExpressionFactor(const Expression& h, const T& z, @@ -201,8 +203,8 @@ TEST( TOAFactor, RealExperiment1 ) { ExpressionFactorGraph graph; for (size_t i = 0; i < 4; i++) { for (size_t j = 0; j < 15; j++) { - Double_ predictTOA_ij(&Event::toa, unknownEvents[j], microphones[i]); - graph.addExpressionFactor(predictTOA_ij, data[j][i], model); + Double_ h_ij(&Event::toa, unknownEvents[j], microphones[i]); + graph.addExpressionFactor(h_ij, data[j][i], model); } } From 541217175c200ea97c622d53af5f4355ec06a9d9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 08:00:02 +0100 Subject: [PATCH 21/39] Added ExpressionFactorGraph (should go away with move of ExpressionFactor to gtsam) --- .../nonlinear/ExpressionFactorGraph.h | 51 +++++++++++++++++++ gtsam_unstable/slam/tests/testTOAFactor.cpp | 28 +--------- 2 files changed, 52 insertions(+), 27 deletions(-) create mode 100644 gtsam_unstable/nonlinear/ExpressionFactorGraph.h diff --git a/gtsam_unstable/nonlinear/ExpressionFactorGraph.h b/gtsam_unstable/nonlinear/ExpressionFactorGraph.h new file mode 100644 index 000000000..0edac858d --- /dev/null +++ b/gtsam_unstable/nonlinear/ExpressionFactorGraph.h @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExpressionFactorGraph.h + * @brief Factor graph that supports adding ExpressionFactors directly + * @author Frank Dellaert + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor graph that supports adding ExpressionFactors directly + */ +class ExpressionFactorGraph: public NonlinearFactorGraph { + +public: + + /// @name Adding Factors + /// @{ + + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const Expression& h, const T& z, + const SharedNoiseModel& R) { + push_back(boost::make_shared >(R, z, h)); + } + + /// @} +}; + +} diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 0565cbcbd..fd1c7a20d 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,10 +17,9 @@ * @date December 2014 */ +#include #include -#include #include -#include #include #include @@ -50,31 +49,6 @@ static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Point3 microphoneAt0; -/** - * Factor graph that supports adding Expression Factors directly - */ -class ExpressionFactorGraph: public NonlinearFactorGraph { - -public: - - /// @name Adding Factors - /// @{ - - /** - * Directly add ExpressionFactor that implements |h(x)-z|^2_R - * @param h expression that implements measurement function - * @param z measurement - * @param R model - */ - template - void addExpressionFactor(const Expression& h, const T& z, - const SharedNoiseModel& R) { - push_back(boost::make_shared >(R, z, h)); - } - - /// @} -}; - //***************************************************************************** TEST( TOAFactor, NewWay ) { Key key = 12; From 8f27b048f1886a7b025f7399e023271f162cc7bb Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 08:53:32 +0100 Subject: [PATCH 22/39] Moved experiment to doubleExpresso project --- gtsam_unstable/slam/tests/testTOAFactor.cpp | 87 --------------------- 1 file changed, 87 deletions(-) diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index fd1c7a20d..1d1b8801b 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -42,9 +42,6 @@ static const double cm = 1e-2; // Create a noise model for the TOA error static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); -// And for height prior -static SharedNoiseModel heightModel(noiseModel::Isotropic::Sigma(1, 100 * cm)); - static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); static const Point3 microphoneAt0; @@ -127,90 +124,6 @@ TEST( TOAFactor, WholeEnchilada ) { EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); } -//***************************************************************************** -/// Test real data -TEST( TOAFactor, RealExperiment1 ) { - - static const bool verbose = true; - - // Create constant expressions for microphones - 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, height)); - EXPECT_LONGS_EQUAL(4, microphones.size()); - - vector data(15); - size_t i = 0; - data[i++] << 1.2648, 1.2648, 1.2677, 1.2643; - data[i++] << 1.7329, 1.7347, 1.7354, 1.7338; - data[i++] << 2.2475, 2.2551, 2.2538, 2.2474; - data[i++] << 2.6945, 2.696, 2.6958, 2.694; - data[i++] << 3.1486, 3.152, 3.1513, 3.1501; - data[i++] << 3.6145, 3.611, 3.6076, 3.6067; - data[i++] << 4.1003, 4.1004, 4.099, 4.0972; - data[i++] << 4.5732, 4.568, 4.5667, 4.5722; - data[i++] << 5.0482, 5.0458, 5.0443, 5.0453; - data[i++] << 5.5311, 5.5256, 5.5254, 5.5305; - data[i++] << 5.9908, 5.9856, 5.9853, 5.9905; - data[i++] << 6.4575, 6.4524, 6.4527, 6.4579; - data[i++] << 6.8983, 6.8971, 6.8984, 6.9016; - data[i++] << 7.3581, 7.3524, 7.3538, 7.3588; - data[i++] << 7.8286, 7.8286, 7.8302, 7.8353; - - // Create unknowns and initial estimate - Event nullEvent(3, 403 / 2 * cm, 403 / 2 * cm, (212 - 45) * cm); - Values initialEstimate; - vector unknownEvents; - for (size_t j = 0; j < 15; j++) { - initialEstimate.insert(j, nullEvent); - unknownEvents.push_back(Event_(j)); - } - - // Print - if (verbose) - initialEstimate.print("Initial Estimate:\n"); - - // Create factor graph with TOA factors - ExpressionFactorGraph graph; - for (size_t i = 0; i < 4; i++) { - for (size_t j = 0; j < 15; j++) { - Double_ h_ij(&Event::toa, unknownEvents[j], microphones[i]); - graph.addExpressionFactor(h_ij, data[j][i], model); - } - } - - // Add height priors - const double heightPrior = (212 - 45) * cm; - for (size_t j = 0; j < 15; j++) { - Double_ height_j(&Event::height, unknownEvents[j]); - graph.addExpressionFactor(height_j, heightPrior, heightModel); - } - - /// Print the graph - if (verbose) - GTSAM_PRINT(graph); - - // 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) - for (size_t j = 0; j < 15; j++) { - Event event = result.at(j); - double t = event.time(); - Point3 p = event.location(); - cout - << boost::format("t(%1%) = %2%;\tlocation(%1%,:) = [%3%, %4%, %5%];") - % (j + 1) % t % p.x() % p.y() % p.z() << endl; - } -} - //***************************************************************************** int main() { TestResult tr; From 012827dc98a1c232c203d71c926189d9e71c0834 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 10:55:36 +0100 Subject: [PATCH 23/39] Unfortunately, Method typedefs do not work for non-class types. --- gtsam/nonlinear/Expression-inl.h | 3 --- gtsam/nonlinear/Expression.h | 10 +++++++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 1074400c9..8c3f8b24c 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -672,7 +672,6 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef T (A1::*Method)(OJ1) const; typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -721,7 +720,6 @@ class BinaryExpression: public: - typedef T (A1::*Method)(const A2&, OJ1, OJ2) const; typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -779,7 +777,6 @@ class TernaryExpression: public: - typedef T (A1::*Method)(const A2&, const A3&, OJ1, OJ2, OJ3) const; typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index cf87ac2ce..fe07de039 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -75,7 +75,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - typename UnaryExpression::Method method) : + T (A::*method)(typename UnaryExpression::OJ1) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,7 +89,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - typename BinaryExpression::Method method, + T (A1::*method)(const A2&, typename BinaryExpression::OJ1, + typename BinaryExpression::OJ2) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -106,7 +107,10 @@ public: /// Construct a binary method expression template Expression(const Expression& expression1, - typename TernaryExpression::Method method, + T (A1::*method)(const A2&, const A3&, + typename TernaryExpression::OJ1, + typename TernaryExpression::OJ2, + typename TernaryExpression::OJ3) const, const Expression& expression2, const Expression& expression3) : root_( new TernaryExpression( From c1f464625be16fabb9f7e37ed83e71bedfdbeec0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 12:39:32 +0100 Subject: [PATCH 24/39] Provided (some) printing functionality --- gtsam/nonlinear/Expression-inl.h | 8 ++++++++ gtsam/nonlinear/Expression.h | 5 +++++ 2 files changed, 13 insertions(+) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 8c3f8b24c..c5a4aaeaa 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -34,6 +34,7 @@ #include namespace MPL = boost::mpl::placeholders; +#include // operator typeid #include class ExpressionFactorBinaryTest; @@ -247,6 +248,13 @@ public: virtual ~ExpressionNode() { } + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(int).name() << std::endl; + if (node.traceSize_>0) os << node.traceSize_ << std::endl; + } + /// Return keys that play in this expression as a set virtual std::set keys() const { std::set keys; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index fe07de039..0bd07b97d 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -52,6 +52,11 @@ private: public: + /// Print + void print(const std::string& s) const { + std::cout << s << root_ << std::endl; + } + // Construct a constant expression Expression(const T& value) : root_(new ConstantExpression(value)) { From 3238c59a39b423f8b2365a729b2495497e02a821 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Dec 2014 13:56:50 +0100 Subject: [PATCH 25/39] Fixed printing --- gtsam/nonlinear/Expression-inl.h | 6 ++++-- gtsam/nonlinear/Expression.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index c5a4aaeaa..6c8561650 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -251,8 +251,10 @@ public: /// Streaming GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const ExpressionNode& node) { - os << "Expression of type " << typeid(int).name() << std::endl; - if (node.traceSize_>0) os << node.traceSize_ << std::endl; + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; } /// Return keys that play in this expression as a set diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 0bd07b97d..6b68c7de2 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -54,7 +54,7 @@ public: /// Print void print(const std::string& s) const { - std::cout << s << root_ << std::endl; + std::cout << s << *root_ << std::endl; } // Construct a constant expression From 07177662f2251b3bf6087ca6e438bbe3b344e88d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 13 Dec 2014 08:16:40 +0100 Subject: [PATCH 26/39] Fixed plotting and and silenced warnings... --- matlab/+gtsam/plot3DTrajectory.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index 05483e589..2900aed99 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -33,12 +33,12 @@ for i = 0:keys.size-1 end gtsam.plotPose3(lastPose, P, scale); catch err - warning(['no Pose3 at ' lastKey]); + % warning(['no Pose3 at ' lastKey]); end - lastIndex = i; end + lastIndex = i; catch - warning(['no Pose3 at ' key]); + % warning(['no Pose3 at ' key]); end % Draw final pose @@ -53,7 +53,7 @@ for i = 0:keys.size-1 end gtsam.plotPose3(lastPose, P, scale); catch - warning(['no Pose3 at ' lastIndex]); + % warning(['no Pose3 at ' lastIndex]); end end From 2225c10fc03da24c61130f649673234241fa6b60 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 13 Dec 2014 08:35:22 +0100 Subject: [PATCH 27/39] use ExpressionFactorGraph --- .../examples/SFMExampleExpressions.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/gtsam_unstable/examples/SFMExampleExpressions.cpp index 941f3dcb8..254d25718 100644 --- a/gtsam_unstable/examples/SFMExampleExpressions.cpp +++ b/gtsam_unstable/examples/SFMExampleExpressions.cpp @@ -24,14 +24,11 @@ // The two new headers that allow using our Automatic Differentiation Expression framework #include -#include +#include // Header order is close to far #include -#include -#include #include -#include #include #include #include @@ -40,27 +37,29 @@ using namespace std; using namespace gtsam; +using namespace noiseModel; /* ************************************************************************* */ int main(int argc, char* argv[]) { Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses vector points = createPoints(); vector poses = createPoses(); // Create a factor graph - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Specify uncertainty on first pose prior - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1); + Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas); // Here we don't use a PriorFactor but directly the ExpressionFactor class - // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] + // x0 is an Expression, and we create a factor wanting it to be equal to poses[0] Pose3_ x0('x',0); - graph.push_back(ExpressionFactor(poseNoise, poses[0], x0)); + graph.addExpressionFactor(x0, poses[0], poseNoise); // We create a constant Expression for the calibration here Cal3_S2_ cK(K); @@ -74,25 +73,26 @@ int main(int argc, char* argv[]) { // Below an expression for the prediction of the measurement: Point3_ p('l', j); Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); - // Again, here we use a ExpressionFactor - graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction)); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(prediction, measurement, measurementNoise); } } // Add prior on first point to constrain scale, again with ExpressionFactor - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0))); + Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1); + graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise); // Create perturbed initial - Values initialEstimate; + Values initial; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); - cout << "initial error = " << graph.error(initialEstimate) << endl; + initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + Values result = DoglegOptimizer(graph, initial).optimize(); cout << "final error = " << graph.error(result) << endl; return 0; From 4542292af99a76898b0d0b7d6e97a37a361ae94a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 13 Dec 2014 22:07:07 +0000 Subject: [PATCH 28/39] Fixed version of at --- gtsam.h | 8 +++++--- gtsam/nonlinear/Values.cpp | 18 ++++++++++++++++++ gtsam/nonlinear/Values.h | 4 ++++ 3 files changed, 27 insertions(+), 3 deletions(-) diff --git a/gtsam.h b/gtsam.h index 37040234d..440fece31 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1733,9 +1733,6 @@ class Values { void insert(size_t j, Vector t); void insert(size_t j, Matrix t); - // Fixed size version - void insertFixed(size_t j, Vector t, size_t n); - void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -1752,6 +1749,11 @@ class Values { template T at(size_t j); + + // Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + Vector atFixed(size_t j, size_t n); + }; // Actually a FastList diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 3cad10f31..372bced8e 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -112,6 +112,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + Vector Values::atFixed(Key j, size_t n) { + switch (n) { + case 1: return at(j); + case 2: return at(j); + case 3: return at(j); + case 4: return at(j); + case 5: return at(j); + case 6: return at(j); + case 7: return at(j); + case 8: return at(j); + case 9: return at(j); + default: + throw runtime_error( + "Values::at fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 9a4c7e798..4acf5ad17 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -180,6 +180,10 @@ namespace gtsam { template const ValueType& at(Key j) const; + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + Vector atFixed(Key j, size_t n); + /** Retrieve a variable by key \c j. This version returns a reference * to the base Value class, and needs to be casted before use. * @param j Retrieve the value associated with this key From 3ddd7d3e7afa119d0a45169a469df61208ee358f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 18:23:11 +0100 Subject: [PATCH 29/39] Fix merge error --- gtsam/geometry/PinholeCamera.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index e2d0e6aec..b409c956d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -496,7 +496,6 @@ private: }; -template template struct traits< PinholeCamera > : public internal::Manifold > {}; From 2ee192e950b63a5db81c62337aaf7995fd53fd49 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 18:23:22 +0100 Subject: [PATCH 30/39] Fix header --- gtsam_unstable/nonlinear/ExpressionFactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactorGraph.h b/gtsam_unstable/nonlinear/ExpressionFactorGraph.h index 0edac858d..122bd429f 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactorGraph.h +++ b/gtsam_unstable/nonlinear/ExpressionFactorGraph.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include namespace gtsam { From 4e9d31d40da9b3b6dfd96bd3119ce46f988be6c7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 18:23:39 +0100 Subject: [PATCH 31/39] Fixed const correctness --- gtsam/geometry/Point2.h | 12 ++++++------ gtsam/geometry/Point3.h | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 01a6d6052..6f4f93cf9 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -165,12 +165,12 @@ public: /// @{ inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} inline void operator *= (double s) {x_*=s;y_*=s;} - Point2 inverse() { return -(*this);} - Point2 compose(const Point2& q) { return (*this)+q;} - Point2 between(const Point2& q) { return q-(*this);} - Vector2 localCoordinates(const Point2& q) { return between(q).vector();} - Point2 retract(const Vector2& v) {return compose(Point2(v));} - static Vector2 Logmap(const Point2& p) {return p.vector();} + Point2 inverse() const { return -(*this);} + Point2 compose(const Point2& q) const { return (*this)+q;} + Point2 between(const Point2& q) const { return q-(*this);} + Vector2 localCoordinates(const Point2& q) const { return between(q).vector();} + Point2 retract(const Vector2& v) const { return compose(Point2(v));} + static Vector2 Logmap(const Point2& p) { return p.vector();} static Point2 Expmap(const Vector2& v) { return Point2(v);} /// @} diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 5b133dc60..c9dee9003 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -164,12 +164,12 @@ namespace gtsam { /// @name Deprecated /// @{ - Point3 inverse() { return -(*this);} - Point3 compose(const Point3& q) { return (*this)+q;} - Point3 between(const Point3& q) { return q-(*this);} - Vector3 localCoordinates(const Point3& q) { return between(q).vector();} - Point3 retract(const Vector3& v) {return compose(Point3(v));} - static Vector3 Logmap(const Point3& p) {return p.vector();} + Point3 inverse() const { return -(*this);} + Point3 compose(const Point3& q) const { return (*this)+q;} + Point3 between(const Point3& q) const { return q-(*this);} + Vector3 localCoordinates(const Point3& q) const { return between(q).vector();} + Point3 retract(const Vector3& v) const { return compose(Point3(v));} + static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} /// @} From 7c455efe50c04d61da02d0b992714f64af141ef3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 18:24:00 +0100 Subject: [PATCH 32/39] Made Event into new-style manifold --- gtsam_unstable/geometry/Event.h | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 143f849bc..9d35331bb 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -31,6 +31,7 @@ class Event { Point3 location_; ///< Location at time event was generated public: + enum { dimension = 4 }; /// Speed of sound static const double Speed; @@ -72,15 +73,6 @@ public: && location_.equals(other.location_, tol); } - /// Manifold stuff: - - size_t dim() const { - return 4; - } - static size_t Dim() { - return 4; - } - /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { return Event(time_ + v[0], location_.retract(v.tail(3))); @@ -111,16 +103,7 @@ const double Event::Speed = 330; const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); // Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant { -}; - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type { -}; - -} +struct GTSAM_EXPORT traits : internal::Manifold {}; } //\ namespace gtsam From 6b9c21b942e1ac65372411c863f8e24ded9d508a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 18:44:44 +0100 Subject: [PATCH 33/39] Reinstated 'fix/matlab_examples_wrapper' changes --- .../FlightCameraTransformIMU.m | 12 +++++----- .../IMUKittiExampleAdvanced.m | 14 +++++------ .../SmartRangeFactorExample.m | 4 ++-- .../TransformCalProjectionFactorExampleISAM.m | 24 +++++++++---------- ...ansformCalProjectionFactorIMUExampleISAM.m | 4 ++-- .../TransformProjectionFactorExample.m | 12 +++++----- .../TransformProjectionFactorExampleISAM.m | 18 +++++++------- .../plot_projected_landmarks.m | 2 +- matlab/unstable_examples/project_landmarks.m | 2 +- matlab/unstable_examples/testTSAMFactors.m | 12 +++++----- 10 files changed, 52 insertions(+), 52 deletions(-) diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index 1b05d2877..b0b37ee33 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -57,7 +57,7 @@ isamParams.setFactorization('QR'); isam = ISAM2(isamParams); %% Get initial conditions for the estimated trajectory -currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!) +currentVelocityGlobal = [10;0;0]; % (This is slightly wrong!) Zhaoyang: Fixed currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); @@ -101,7 +101,7 @@ axis equal; for i=1:size(trajectory)-1 xKey = symbol('x',i); - pose = trajectory.at(xKey); % GT pose + pose = trajectory.atPose3(xKey); % GT pose pose_t = pose.translation(); % GT pose-translation if exist('h_cursor','var') @@ -165,13 +165,13 @@ for i=1:size(trajectory)-1 if i > 1 xKey_prev = symbol('x',i-1); - pose_prev = trajectory.at(xKey_prev); + pose_prev = trajectory.atPose3(xKey_prev); step = pose_prev.between(pose); % insert estimate for current pose with some normal noise on % translation - initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)]))); + initial.insert(xKey,result.atPose3(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)]))); % visual measurements if measurements.size > 0 && use_camera @@ -181,12 +181,12 @@ for i=1:size(trajectory)-1 zKey = measurementKeys.at(zz); lKey = symbol('l',symbolIndex(zKey)); - fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ... + fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ... z_cov, xKey, transformKey, lKey, calibrationKey, false, true)); % only add landmark to values if doesn't exist yet if ~result.exists(lKey) - noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); + noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); initial.insert(lKey, noisy_landmark); % and add a prior since its position is known diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index 1db60a5ad..cb13eacee 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -7,7 +7,7 @@ disp('Example of application of ISAM2 for visual-inertial navigation on the KITT %% Read metadata and compute relative sensor pose transforms % IMU metadata disp('-- Reading sensor metadata') -IMU_metadata = importdata('KittiEquivBiasedImu_metadata.txt'); +IMU_metadata = importdata(findExampleDataFile('KittiEquivBiasedImu_metadata.txt')); IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2); IMUinBody = Pose3.Expmap([IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz; IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]); @@ -16,14 +16,14 @@ if ~IMUinBody.equals(Pose3, 1e-5) end % VO metadata -VO_metadata = importdata('KittiRelativePose_metadata.txt'); +VO_metadata = importdata(findExampleDataFile('KittiRelativePose_metadata.txt')); VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2); VOinBody = Pose3.Expmap([VO_metadata.BodyPtx; VO_metadata.BodyPty; VO_metadata.BodyPtz; VO_metadata.BodyPrx; VO_metadata.BodyPry; VO_metadata.BodyPrz; ]); VOinIMU = IMUinBody.inverse().compose(VOinBody); % GPS metadata -GPS_metadata = importdata('KittiGps_metadata.txt'); +GPS_metadata = importdata(findExampleDataFile('KittiGps_metadata.txt')); GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2); GPSinBody = Pose3.Expmap([GPS_metadata.BodyPtx; GPS_metadata.BodyPty; GPS_metadata.BodyPtz; GPS_metadata.BodyPrx; GPS_metadata.BodyPry; GPS_metadata.BodyPrz; ]); @@ -32,7 +32,7 @@ GPSinIMU = IMUinBody.inverse().compose(GPSinBody); %% Read data and change coordinate frame of GPS and VO measurements to IMU frame disp('-- Reading sensor data from file') % IMU data -IMU_data = importdata('KittiEquivBiasedImu.txt'); +IMU_data = importdata(findExampleDataFile('KittiEquivBiasedImu.txt')); IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2); imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_data.accelZ]' [IMU_data.omegaX]' [IMU_data.omegaY]' [IMU_data.omegaZ]' ], 2), 'UniformOutput', false); [IMU_data.acc_omega] = deal(imum{:}); @@ -40,7 +40,7 @@ imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_da clear imum % VO data -VO_data = importdata('KittiRelativePose.txt'); +VO_data = importdata(findExampleDataFile('KittiRelativePose.txt')); VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2); % Merge relative pose fields and convert to Pose3 logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ]; @@ -71,7 +71,7 @@ clear logposes relposes %% Get initial conditions for the estimated trajectory % % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame) currentPoseGlobal = Pose3; -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -120,7 +120,7 @@ for measurementIndex = 1:length(timestamps) newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = timestamps(measurementIndex-1, 1); diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m index 93e5dce0c..7535447df 100644 --- a/matlab/unstable_examples/SmartRangeFactorExample.m +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -82,8 +82,8 @@ for ind_pose = 2:7 key_prev = poseKey(ind_pose-1); key_curr = poseKey(ind_pose); - prev_pose = smartValues.at(key_prev); - curr_pose = smartValues.at(key_curr); + prev_pose = smartValues.atPose2(key_prev); + curr_pose = smartValues.atPose2(key_curr); GTDeltaPose = prev_pose.between(curr_pose); noisePose = Pose2([t_noise*rand; t_noise*rand; r_noise*rand]); NoisyDeltaPose = GTDeltaPose.compose(noisePose); diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index 236327b05..c9e912ea4 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -96,17 +96,17 @@ for i=1:20 if i > 1 if i < 11 - initial.insert(i,result.at(i-1).compose(move_forward)); + initial.insert(i,result.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,result.at(i-1).compose(move_circle)); + initial.insert(i,result.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -137,10 +137,10 @@ for i=1:20 hold on; %% plot results - result_camera_transform = result.at(transformKey); + result_camera_transform = result.atPose3(transformKey); for j=1:i - gtsam.plotPose3(result.at(j),[],0.5); - gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + gtsam.plotPose3(result.atPose3(j),[],0.5); + gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5); end xlabel('x (m)'); @@ -153,12 +153,12 @@ for i=1:20 % axis equal for l=101:100+nrPoints - plotPoint3(result.at(l),'g'); + plotPoint3(result.atPoint3(l),'g'); end - ty = result.at(transformKey).translation().y(); - fx = result.at(calibrationKey).fx(); - fy = result.at(calibrationKey).fy(); + ty = result.atPose3(transformKey).translation().y(); + fx = result.atCal3_S2(calibrationKey).fx(); + fy = result.atCal3_S2(calibrationKey).fy(); text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty)); text(1,5,3,sprintf('fx(900): %.0f',fx)); text(1,5,1,sprintf('fy(900): %.0f',fy)); @@ -180,10 +180,10 @@ end fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); disp('Transform after optimization'); -result.at(transformKey) +result.atPose3(transformKey) disp('Calibration after optimization'); -result.at(calibrationKey) +result.atCal3_S2(calibrationKey) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index 834472a7d..fd4a17469 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -89,7 +89,7 @@ isam = ISAM2(isamParams); currentIMUPoseGlobal = Pose3(); %% Get initial conditions for the estimated trajectory -currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [1;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); @@ -127,7 +127,7 @@ for i=1:steps fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov)); % velocity and bias evolution - fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); result = initial; diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 410b7df0f..669073e56 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -61,17 +61,17 @@ cheirality_exception_count = 0; for i=1:20 if i > 1 if i < 11 - initial.insert(i,initial.at(i-1).compose(move_forward)); + initial.insert(i,initial.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,initial.at(i-1).compose(move_circle)); + initial.insert(i,initial.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -93,14 +93,14 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); %% camera plotting for i=1:20 - gtsam.plotPose3(initial.at(i).compose(camera_transform)); + gtsam.plotPose3(initial.atPose3(i).compose(camera_transform)); end xlabel('x (m)'); ylabel('y (m)'); disp('Transform before optimization'); -initial.at(1000) +initial.atPose3(1000) params = LevenbergMarquardtParams; params.setAbsoluteErrorTol(1e-15); @@ -112,7 +112,7 @@ optimizer = LevenbergMarquardtOptimizer(fg, initial, params); result = optimizer.optimizeSafely(); disp('Transform after optimization'); -result.at(1000) +result.atPose3(1000) % results is empty here. optimizer doesn't generate result? axis([0 25 0 25 0 10]); axis equal diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 169736f4b..8edcfade7 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -86,17 +86,17 @@ for i=1:20 if i > 1 if i < 11 - initial.insert(i,result.at(i-1).compose(move_forward)); + initial.insert(i,result.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,result.at(i-1).compose(move_circle)); + initial.insert(i,result.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -127,10 +127,10 @@ for i=1:20 hold on; %% plot results - result_camera_transform = result.at(1000); + result_camera_transform = result.atPose3(1000); for j=1:i - gtsam.plotPose3(result.at(j)); - gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + gtsam.plotPose3(result.atPose3(j)); + gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5); end xlabel('x (m)'); @@ -143,10 +143,10 @@ for i=1:20 % axis equal for l=101:100+nrPoints - plotPoint3(result.at(l),'g'); + plotPoint3(result.atPoint3(l),'g'); end - ty = result.at(1000).translation().y(); + ty = result.atPose3(1000).translation().y(); text(5,5,5,sprintf('Y-Transform: %0.2g',ty)); if(write_video) @@ -168,7 +168,7 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); disp('Transform after optimization'); -result.at(1000) +result.atPose3(1000) diff --git a/matlab/unstable_examples/plot_projected_landmarks.m b/matlab/unstable_examples/plot_projected_landmarks.m index cfe08ec54..6b8101844 100644 --- a/matlab/unstable_examples/plot_projected_landmarks.m +++ b/matlab/unstable_examples/plot_projected_landmarks.m @@ -23,7 +23,7 @@ z = zeros(1,nrMeasurements); for i = 0:measurement_keys.size-1 key = measurement_keys.at(i); key_index = gtsam.symbolIndex(key); - p = landmarks.at(gtsam.symbol('l',key_index)); + p = landmarks.atPoint3(gtsam.symbol('l',key_index)); x(i+1) = p.x; y(i+1) = p.y; diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index b11c43bc5..629c6d994 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -8,7 +8,7 @@ function [ measurements ] = project_landmarks( pose, landmarks, K ) measurements = Values; for i=1:size(landmarks)-1 - z = camera.project(landmarks.at(symbol('l',i))); + z = camera.project(landmarks.atPoint3(symbol('l',i))); % check bounding box if z.x < 0 || z.x > 1280 diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index abdfc5922..341725723 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -48,12 +48,12 @@ optimizer = LevenbergMarquardtOptimizer(graph, initial, params); result = optimizer.optimize(); % Check result -CHECK('error',result.at(100).equals(b1,1e-5)) -CHECK('error',result.at(10).equals(origin,1e-5)) -CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) -CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) -CHECK('error',result.at(20).equals(origin,1e-5)) -CHECK('error',result.at(200).equals(b2,1e-5)) +CHECK('error',result.atPose2(100).equals(b1,1e-5)) +CHECK('error',result.atPose2(10).equals(origin,1e-5)) +CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPose2(20).equals(origin,1e-5)) +CHECK('error',result.atPose2(200).equals(b2,1e-5)) % Check error CHECK('error',abs(graph.error(result))<1e-9) From c58f2c15dc7bf2593462ceb2552b5e2528e9790b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 19:09:12 +0100 Subject: [PATCH 34/39] Moved to gtsam --- {gtsam_unstable => gtsam}/nonlinear/ExpressionFactorGraph.h | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {gtsam_unstable => gtsam}/nonlinear/ExpressionFactorGraph.h (100%) diff --git a/gtsam_unstable/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h similarity index 100% rename from gtsam_unstable/nonlinear/ExpressionFactorGraph.h rename to gtsam/nonlinear/ExpressionFactorGraph.h From e1198462806cf85f50b6e049cd79ab735a6f578d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 19:10:41 +0100 Subject: [PATCH 35/39] Updated examples and test --- examples/Pose2SLAMExampleExpressions.cpp | 4 ++-- examples/SFMExampleExpressions.cpp | 4 ++-- gtsam_unstable/slam/tests/testTOAFactor.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 3a3b97b77..74635efe0 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -18,8 +18,8 @@ */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include // Header order is close to far #include diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 254d25718..e9c9e920d 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -23,8 +23,8 @@ */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include // Header order is close to far #include diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 1d1b8801b..879f7095e 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -17,8 +17,8 @@ * @date December 2014 */ -#include #include +#include #include #include From c8bfeb6692b6722acb3c1fa51492e1a080944a5c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 19:26:00 +0100 Subject: [PATCH 36/39] Now using awesome ExpressionFactorGraph --- examples/Pose2SLAMExampleExpressions.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 74635efe0..92f94c3f3 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -19,7 +19,7 @@ // The two new headers that allow using our Automatic Differentiation Expression framework #include -#include +#include // Header order is close to far #include @@ -35,26 +35,26 @@ using namespace gtsam; int main(int argc, char** argv) { // 1. Create a factor graph container and add factors to it - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Create Expressions for unknowns Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); + graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model); + graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution From 4d6455fa7fd248be62737270c5ac6b7cd4362437 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 20:35:26 +0100 Subject: [PATCH 37/39] Made tests work again after removing overloads (in develop) --- gtsam/base/numericalDerivative.h | 42 ++++++++++++++++++++++++++++++++ gtsam/base/testLie.h | 27 +++++++++++--------- 2 files changed, 58 insertions(+), 11 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index e5cfbdd3b..a7dc37d55 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -296,6 +296,48 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* x2, x3, delta); } +/** + * Compute numerical derivative in argument 1 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative41( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3, x4), x1, delta); +} + +/** + * Compute numerical derivative in argument 2 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative42( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index 4bbca85ca..791d5c04d 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -34,23 +34,25 @@ void testLieGroupDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; + typedef OptionalJacobian OJ; // Inverse + OJ none; EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1)); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t1, none),H1)); EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); - EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1)); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t2, none),H1)); // Compose EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Compose, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Compose, t1, t2, none, none), H2)); // Between EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Between, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Between, t1, t2, none, none), H2)); } // Do a comprehensive test of Lie Group Chart derivatives template @@ -59,17 +61,20 @@ void testChartDerivatives(TestResult& result_, const std::string& name_, Matrix H1, H2; typedef traits T; + typedef typename G::TangentVector V; + typedef OptionalJacobian OJ; // Retract - typename G::TangentVector w12 = T::Local(t1, t2); + OJ none; + V w12 = T::Local(t1, t2); EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Retract, t1, w12, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Retract, t1, w12, none, none), H2)); // Local EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); - EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1)); - EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2)); + EXPECT(assert_equal(numericalDerivative41(T::Local, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Local, t1, t2, none, none), H2)); } } // namespace gtsam From bed5132ca13b74c4d724694e9b64f4d709bfcbbb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 21:31:16 +0100 Subject: [PATCH 38/39] Removed obsolete tests --- gtsam/geometry/tests/testSO3.cpp | 40 -------------------------------- 1 file changed, 40 deletions(-) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3d8e41af5..0fe699116 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -55,46 +55,6 @@ TEST(SO3 , Retract) { EXPECT(actual.isApprox(R2)); } -//****************************************************************************** -TEST(SO3 , Compose) { - SO3 expected = R1 * R2; - Matrix actualH1, actualH2; - SO3 actual = traits::Compose(R1, R2, actualH1, actualH2); - EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(traits::Compose, R1, R2); - EXPECT(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(traits::Compose, R1, R2); - EXPECT(assert_equal(numericalH2,actualH2)); -} - -//****************************************************************************** -TEST(SO3 , Between) { - SO3 expected = R1.inverse() * R2; - Matrix actualH1, actualH2; - SO3 actual = traits::Between(R1, R2, actualH1, actualH2); - EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(traits::Between, R1, R2); - EXPECT(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(traits::Between, R1, R2); - EXPECT(assert_equal(numericalH2,actualH2)); -} - -//****************************************************************************** -TEST(SO3 , Inverse) { - SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); - - Matrix actualH; - SO3 actual = traits::Inverse(R1, actualH); - EXPECT(traits::Equals(expected,actual)); - - Matrix numericalH = numericalDerivative11(traits::Inverse, R1); - EXPECT(assert_equal(numericalH,actualH)); -} - //****************************************************************************** TEST(SO3 , Invariants) { check_group_invariants(id,id); From 9367dc2fb5bd3ea80780ec9043bf8b773b279fdd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 28 Dec 2014 21:31:45 +0100 Subject: [PATCH 39/39] Fixed derivatives... --- gtsam/base/Lie.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 9c410ac25..bf2056cc8 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -110,7 +110,7 @@ struct LieGroup { Class retract(const TangentVector& v, // ChartJacobian H1, ChartJacobian H2 = boost::none) const { Jacobian D_g_v; - Class g = Class::ChartAtOrigin::Retract(v,D_g_v); + Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); Class h = compose(g,H1,H2); if (H2) *H2 = (*H2) * D_g_v; return h; @@ -120,7 +120,7 @@ struct LieGroup { ChartJacobian H1, ChartJacobian H2 = boost::none) const { Class h = between(g,H1,H2); Jacobian D_v_h; - TangentVector v = Class::ChartAtOrigin::Local(h, D_v_h); + TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); if (H1) *H1 = D_v_h * (*H1); if (H2) *H2 = D_v_h * (*H2); return v;