diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 82d2f822e..c88579587 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -27,14 +27,11 @@ namespace gtsam { /** * Factor which evaluates functor and uses the result to compute * error on provided measurement. - * The provided FUNCTOR should provide two type aliases: `argument_type` which - * corresponds to the type of input it accepts and `return_type` which indicates - * the type of the return value. This factor uses those type values to construct - * the functor. * * Template parameters are - * @param FUNCTOR: A class which operates as a functor. - * + * @param R: The return type of the functor after evaluation. + * @param T: The argument type for the functor. + * * Example: * Key key = Symbol('X', 0); * auto model = noiseModel::Isotropic::Sigma(9, 1); @@ -48,58 +45,53 @@ namespace gtsam { * MultiplyFunctor(double m) : m_(m) {} * Matrix operator()(const Matrix &X, * OptionalJacobian<-1, -1> H = boost::none) const { - * if (H) *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols()); - * return m_ * X; + * if (H) *H = m_ * Matrix::Identity(X.rows()*X.cols(), + * X.rows()*X.cols()); return m_ * X; * } * }; * * Matrix measurement = Matrix::Identity(3, 3); * double multiplier = 2.0; - * FunctorizedFactor factor(keyX, measurement, model, multiplier); + * + * FunctorizedFactor factor(keyX, measurement, model, + * MultiplyFunctor(multiplier)); */ -template -class GTSAM_EXPORT FunctorizedFactor - : public NoiseModelFactor1 { -private: - using T = typename FUNCTOR::argument_type; +template +class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { + private: using Base = NoiseModelFactor1; - typename FUNCTOR::return_type - measured_; ///< value that is compared with functor return value - SharedNoiseModel noiseModel_; ///< noise model - FUNCTOR func_; ///< functor instance + R measured_; ///< value that is compared with functor return value + SharedNoiseModel noiseModel_; ///< noise model + std::function)> func_; ///< functor instance -public: + public: /** default constructor - only use for serialization */ FunctorizedFactor() {} /** Construct with given x and the parameters of the basis - * - * @param Args: Variadic template parameter for functor arguments. * * @param key: Factor key - * @param z: Measurement object of type FUNCTOR::return_type + * @param z: Measurement object of type R * @param model: Noise model - * @param args: Variable number of arguments used to instantiate functor + * @param func: The instance of the functor object */ - template - FunctorizedFactor(Key key, const typename FUNCTOR::return_type &z, - const SharedNoiseModel &model, Args &&... args) - : Base(model, key), measured_(z), noiseModel_(model), - func_(std::forward(args)...) {} + FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, + const std::function)> func) + : Base(model, key), measured_(z), noiseModel_(model), func_(func) {} virtual ~FunctorizedFactor() {} /// @return a deep copy of this factor virtual NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( - NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); + NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); } Vector evaluateError(const T ¶ms, boost::optional H = boost::none) const { - typename FUNCTOR::return_type x = func_(params, H); - Vector error = traits::Local(measured_, x); + R x = func_(params, H); + Vector error = traits::Local(measured_, x); return error; } @@ -110,22 +102,21 @@ public: Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" << keyFormatter(this->key()) << ")" << std::endl; - traits::Print(measured_, " measurement: "); + traits::Print(measured_, " measurement: "); std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() << std::endl; } virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const { - const FunctorizedFactor *e = - dynamic_cast *>(&other); + const FunctorizedFactor *e = + dynamic_cast *>(&other); const bool base = Base::equals(*e, tol); return e && Base::equals(other, tol) && - traits::Equals(this->measured_, e->measured_, - tol); + traits::Equals(this->measured_, e->measured_, tol); } /// @} -private: + private: /** Serialization function */ friend class boost::serialization::access; template @@ -138,8 +129,8 @@ private: }; /// traits -template -struct traits> - : public Testable> {}; +template +struct traits> + : public Testable> {}; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 9393a4410..9ff6b8e24 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -17,13 +17,12 @@ * @brief unit tests for FunctorizedFactor class */ +#include #include #include #include #include -#include - using namespace std; using namespace gtsam; @@ -32,9 +31,9 @@ auto model = noiseModel::Isotropic::Sigma(9, 1); /// Functor that takes a matrix and multiplies every element by m class MultiplyFunctor { - double m_; ///< simple multiplier + double m_; ///< simple multiplier -public: + public: using argument_type = Matrix; using return_type = Matrix; @@ -42,32 +41,33 @@ public: Matrix operator()(const Matrix &X, OptionalJacobian<-1, -1> H = boost::none) const { - if (H) - *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); + if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); return m_ * X; } }; +/* ************************************************************************* */ TEST(FunctorizedFactor, Identity) { - Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3); double multiplier = 1.0; - FunctorizedFactor factor(key, measurement, model, - multiplier); + FunctorizedFactor factor(key, measurement, model, + MultiplyFunctor(multiplier)); Vector error = factor.evaluateError(X); EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); } +/* ************************************************************************* */ TEST(FunctorizedFactor, Multiply2) { double multiplier = 2.0; Matrix X = Matrix::Identity(3, 3); Matrix measurement = multiplier * Matrix::Identity(3, 3); - FunctorizedFactor factor(key, measurement, model, multiplier); + FunctorizedFactor factor(key, measurement, model, + MultiplyFunctor(multiplier)); Vector error = factor.evaluateError(X); @@ -79,10 +79,10 @@ TEST(FunctorizedFactor, Equality) { double multiplier = 2.0; - FunctorizedFactor factor1(key, measurement, model, - multiplier); - FunctorizedFactor factor2(key, measurement, model, - multiplier); + FunctorizedFactor factor1(key, measurement, model, + MultiplyFunctor(multiplier)); + FunctorizedFactor factor2(key, measurement, model, + MultiplyFunctor(multiplier)); EXPECT(factor1.equals(factor2)); } @@ -94,7 +94,8 @@ TEST(FunctorizedFactor, Jacobians) { double multiplier = 2.0; - FunctorizedFactor factor(key, X, model, multiplier); + FunctorizedFactor factor(key, X, model, + MultiplyFunctor(multiplier)); Values values; values.insert(key, X); @@ -103,12 +104,14 @@ TEST(FunctorizedFactor, Jacobians) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); } +/* ************************************************************************* */ TEST(FunctorizedFactor, Print) { Matrix X = Matrix::Identity(2, 2); double multiplier = 2.0; - FunctorizedFactor factor(key, X, model, multiplier); + FunctorizedFactor factor(key, X, model, + MultiplyFunctor(multiplier)); // redirect output to buffer so we can compare stringstream buffer; @@ -120,18 +123,56 @@ TEST(FunctorizedFactor, Print) { string actual = buffer.str(); cout.rdbuf(old); - string expected = " keys = { X0 }\n" - " noise model: unit (9) \n" - "FunctorizedFactor(X0)\n" - " measurement: [\n" - " 1, 0;\n" - " 0, 1\n" - "]\n" - " noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; + string expected = + " keys = { X0 }\n" + " noise model: unit (9) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 1, 0;\n" + " 0, 1\n" + "]\n" + " noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; CHECK_EQUAL(expected, actual); } +/* ************************************************************************* */ +// Test factor using a std::function type. +TEST(FunctorizedFactor, Functional) { + double multiplier = 2.0; + Matrix X = Matrix::Identity(3, 3); + Matrix measurement = multiplier * Matrix::Identity(3, 3); + + std::function)> functional = + MultiplyFunctor(multiplier); + FunctorizedFactor factor(key, measurement, model, functional); + + Vector error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + +/* ************************************************************************* */ +TEST(FunctorizedFactor, Lambda) { + double multiplier = 2.0; + Matrix X = Matrix::Identity(3, 3); + Matrix measurement = multiplier * Matrix::Identity(3, 3); + + auto lambda = [multiplier](const Matrix &X, + OptionalJacobian<-1, -1> H = boost::none) { + if (H) + *H = multiplier * + Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); + return multiplier * X; + }; + // FunctorizedFactor factor(key, measurement, model, lambda); + auto factor = FunctorizedFactor(key, measurement, model, lambda); + + Vector error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + /* ************************************************************************* */ int main() {