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;