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;