diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h new file mode 100644 index 000000000..a83198967 --- /dev/null +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * 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 FunctorizedFactor.h + * @date May 31, 2020 + * @author Varun Agrawal + **/ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Factor which evaluates provided unary functor and uses the result to compute + * error with respect to the provided measurement. + * + * Template parameters are + * @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); + * + * /// Functor that takes a matrix and multiplies every element by m + * class MultiplyFunctor { + * double m_; ///< simple multiplier + * public: + * 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; + * } + * }; + * + * Matrix measurement = Matrix::Identity(3, 3); + * double multiplier = 2.0; + * + * FunctorizedFactor factor(keyX, measurement, model, + * MultiplyFunctor(multiplier)); + */ +template +class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { + private: + using Base = NoiseModelFactor1; + + R measured_; ///< value that is compared with functor return value + SharedNoiseModel noiseModel_; ///< noise model + std::function)> func_; ///< functor instance + + public: + /** default constructor - only use for serialization */ + FunctorizedFactor() {} + + /** Construct with given x and the parameters of the basis + * + * @param key: Factor key + * @param z: Measurement object of same type as that returned by functor + * @param model: Noise model + * @param func: The instance of the functor object + */ + 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))); + } + + Vector evaluateError(const T ¶ms, + boost::optional H = boost::none) const { + R x = func_(params, H); + Vector error = traits::Local(measured_, x); + return error; + } + + /// @name Testable + /// @{ + void print(const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" + << keyFormatter(this->key()) << ")" << std::endl; + 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 bool base = Base::equals(*e, tol); + return e && Base::equals(other, tol) && + traits::Equals(this->measured_, e->measured_, tol); + } + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(measured_); + ar &BOOST_SERIALIZATION_NVP(func_); + } +}; + +/// traits +template +struct traits> + : public Testable> {}; + +/** + * Helper function to create a functorized factor. + * + * Uses function template deduction to identify return type and functor type, so + * template list only needs the functor argument type. + */ +template +FunctorizedFactor MakeFunctorizedFactor(Key key, const R &z, + const SharedNoiseModel &model, + const FUNC func) { + return FunctorizedFactor(key, z, model, func); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp new file mode 100644 index 000000000..12dd6b91c --- /dev/null +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- + */ + +/** + * @file testFunctorizedFactor.cpp + * @date May 31, 2020 + * @author Varun Agrawal + * @brief unit tests for FunctorizedFactor class + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key key = Symbol('X', 0); +auto model = noiseModel::Isotropic::Sigma(9, 1); + +/// Functor that takes a matrix and multiplies every element by m +class MultiplyFunctor { + double m_; ///< simple multiplier + + public: + 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; + } +}; + +/* ************************************************************************* */ +// Test identity operation for FunctorizedFactor. +TEST(FunctorizedFactor, Identity) { + Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3); + + double multiplier = 1.0; + auto functor = MultiplyFunctor(multiplier); + auto factor = MakeFunctorizedFactor(key, measurement, model, functor); + + Vector error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + +/* ************************************************************************* */ +// Test FunctorizedFactor with multiplier value of 2. +TEST(FunctorizedFactor, Multiply2) { + double multiplier = 2.0; + Matrix X = Matrix::Identity(3, 3); + Matrix measurement = multiplier * Matrix::Identity(3, 3); + + auto factor = MakeFunctorizedFactor(key, measurement, model, + MultiplyFunctor(multiplier)); + + Vector error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + +/* ************************************************************************* */ +// Test equality function for FunctorizedFactor. +TEST(FunctorizedFactor, Equality) { + Matrix measurement = Matrix::Identity(2, 2); + + double multiplier = 2.0; + + auto factor1 = MakeFunctorizedFactor(key, measurement, model, + MultiplyFunctor(multiplier)); + auto factor2 = MakeFunctorizedFactor(key, measurement, model, + MultiplyFunctor(multiplier)); + + EXPECT(factor1.equals(factor2)); +} + +/* *************************************************************************** */ +// Test Jacobians of FunctorizedFactor. +TEST(FunctorizedFactor, Jacobians) { + Matrix X = Matrix::Identity(3, 3); + Matrix actualH; + + double multiplier = 2.0; + + auto factor = + MakeFunctorizedFactor(key, X, model, MultiplyFunctor(multiplier)); + + Values values; + values.insert(key, X); + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +// Test print result of FunctorizedFactor. +TEST(FunctorizedFactor, Print) { + Matrix X = Matrix::Identity(2, 2); + + double multiplier = 2.0; + + auto factor = + MakeFunctorizedFactor(key, X, model, MultiplyFunctor(multiplier)); + + // redirect output to buffer so we can compare + stringstream buffer; + streambuf *old = cout.rdbuf(buffer.rdbuf()); + + factor.print(); + + // get output string and reset stdout + 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"; + + CHECK_EQUAL(expected, actual); +} + +/* ************************************************************************* */ +// Test FunctorizedFactor 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); + auto factor = + MakeFunctorizedFactor(key, measurement, model, functional); + + Vector error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + +/* ************************************************************************* */ +// Test FunctorizedFactor with a lambda function. +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 = MakeFunctorizedFactor(key, measurement, model, lambda); + + Vector error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */