From 1db0f441bcd91878e562c9ad61c1a958be082cf0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 31 May 2020 21:52:13 -0400 Subject: [PATCH] Added FunctorizedFactor and corresponding tests --- gtsam/linear/NoiseModel.cpp | 1 + gtsam/nonlinear/FunctorizedFactor.h | 123 +++++++++++++++++ .../nonlinear/tests/testFunctorizedFactor.cpp | 127 ++++++++++++++++++ 3 files changed, 251 insertions(+) create mode 100644 gtsam/nonlinear/FunctorizedFactor.h create mode 100644 gtsam/nonlinear/tests/testFunctorizedFactor.cpp diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index d7fd2d1ea..72ca054b4 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -619,6 +619,7 @@ void Isotropic::WhitenInPlace(Eigen::Block H) const { // Unit /* ************************************************************************* */ void Unit::print(const std::string& name) const { + //TODO(Varun): Do we need that space at the end? cout << name << "unit (" << dim_ << ") " << endl; } diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h new file mode 100644 index 000000000..b990ac04f --- /dev/null +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * @author Varun Agrawal + **/ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Factor which evaluates functor and uses the result to compute + * error on provided measurement. + * The provided FUNCTOR should provide two definitions: `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. + */ +template +class GTSAM_EXPORT FunctorizedFactor + : public NoiseModelFactor1 { +private: + using T = typename FUNCTOR::argument_type; + using Base = NoiseModelFactor1; + + typename FUNCTOR::return_type + measured_; ///< value that is compared with functor return value + SharedNoiseModel noiseModel_; ///< noise model + FUNCTOR func_; ///< functor instance + +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 model: Noise model + * @param args: Variable number of arguments used to instantiate functor + */ + 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)...) {} + + 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 { + typename FUNCTOR::return_type x = func_(params, H); + Vector error = traits::Local(measured_, x); + return error; + } + + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream & + operator<<(std::ostream &os, const FunctorizedFactor &f) { + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; + } + 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 << *this << 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 != nullptr && base; + } + /// @} + +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_); + } +}; + +// TODO(Varun): Include or kill? +// template <> struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp new file mode 100644 index 000000000..7536aadc5 --- /dev/null +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -0,0 +1,127 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +using namespace std; +using namespace gtsam; + +Key keyX = Symbol('X', 0); +auto model = noiseModel::Isotropic::Sigma(3, 1); + +/// Functor that takes a matrix and multiplies every element by m +class MultiplyFunctor { + double m_; ///< simple multiplier + +public: + using argument_type = Matrix; + using return_type = Matrix; + + MultiplyFunctor(double m) : m_(m) {} + + Matrix operator()(const Matrix &X, + OptionalJacobian<-1, -1> H = boost::none) const { + return m_ * X; + } +}; + +TEST(FunctorizedFactor, Identity) { + + Matrix X = Matrix::Identity(3, 3); + + double multiplier = 1.0; + + FunctorizedFactor factor(keyX, X, model, multiplier); + + Values values; + values.insert(keyX, X); + + Matrix error = factor.evaluateError(X); + + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); +} + +TEST(FunctorizedFactor, Multiply2) { + Matrix X = Matrix::Identity(3, 3); + + double multiplier = 2.0; + + FunctorizedFactor factor(keyX, X, model, multiplier); + + Values values; + values.insert(keyX, X); + + Matrix error = factor.evaluateError(X); + + Matrix expected = Matrix::Identity(3, 3); + expected.resize(9, 1); + EXPECT(assert_equal(expected, error, 1e-9)); +} + +TEST(FunctorizedFactor, Equality) { + Matrix X = Matrix::Identity(2, 2); + + double multiplier = 2.0; + + FunctorizedFactor factor1(keyX, X, model, multiplier); + FunctorizedFactor factor2(keyX, X, model, multiplier); + + EXPECT(factor1.equals(factor2)); +} + +TEST(FunctorizedFactor, Print) { + Matrix X = Matrix::Identity(2, 2); + + double multiplier = 2.0; + + FunctorizedFactor factor(keyX, X, model, 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 (3) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 1, 0;\n" + " 0, 1\n" + "]\n" + " noise model sigmas: 1 1 1\n"; + + CHECK_EQUAL(expected, actual); +} + +/* ************************************************************************* + */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */