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;