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;