From 131213a983521b2095f15d3ce0d798645f513538 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 1 Jun 2020 19:52:50 -0400 Subject: [PATCH] fixes, better tests, docs --- gtsam/nonlinear/FunctorizedFactor.h | 58 +++++++++++------ .../nonlinear/tests/testFunctorizedFactor.cpp | 62 ++++++++++++------- 2 files changed, 78 insertions(+), 42 deletions(-) diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index b990ac04f..03fd47a46 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -10,8 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file FunctorizedFactor.h - * @author Varun Agrawal + * @file FunctorizedFactor.h + * @date May 31, 2020 + * @author Varun Agrawal **/ #pragma once @@ -26,13 +27,35 @@ 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 + * 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. + * + * 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: + * 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 { + * 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); */ template class GTSAM_EXPORT FunctorizedFactor @@ -82,27 +105,24 @@ public: /// @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; + 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 != nullptr && base; - } + 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: @@ -117,7 +137,9 @@ private: } }; -// TODO(Varun): Include or kill? -// template <> struct traits : public Testable {}; +/// traits +template +struct traits> + : public Testable> {}; } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 7536aadc5..9393a4410 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,14 +20,15 @@ #include #include #include +#include #include using namespace std; using namespace gtsam; -Key keyX = Symbol('X', 0); -auto model = noiseModel::Isotropic::Sigma(3, 1); +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 { @@ -41,60 +42,73 @@ 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()); return m_ * X; } }; TEST(FunctorizedFactor, Identity) { - Matrix X = Matrix::Identity(3, 3); + Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3); double multiplier = 1.0; - FunctorizedFactor factor(keyX, X, model, multiplier); + FunctorizedFactor factor(key, measurement, model, + multiplier); - Values values; - values.insert(keyX, X); - - Matrix error = factor.evaluateError(X); + Vector 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; + Matrix X = Matrix::Identity(3, 3); + Matrix measurement = multiplier * Matrix::Identity(3, 3); - FunctorizedFactor factor(keyX, X, model, multiplier); + FunctorizedFactor factor(key, measurement, model, multiplier); - Values values; - values.insert(keyX, X); + Vector error = factor.evaluateError(X); - Matrix error = factor.evaluateError(X); - - Matrix expected = Matrix::Identity(3, 3); - expected.resize(9, 1); - EXPECT(assert_equal(expected, error, 1e-9)); + EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); } TEST(FunctorizedFactor, Equality) { - Matrix X = Matrix::Identity(2, 2); + Matrix measurement = Matrix::Identity(2, 2); double multiplier = 2.0; - FunctorizedFactor factor1(keyX, X, model, multiplier); - FunctorizedFactor factor2(keyX, X, model, multiplier); + FunctorizedFactor factor1(key, measurement, model, + multiplier); + FunctorizedFactor factor2(key, measurement, model, + multiplier); EXPECT(factor1.equals(factor2)); } +//****************************************************************************** +TEST(FunctorizedFactor, Jacobians) { + Matrix X = Matrix::Identity(3, 3); + Matrix actualH; + + double multiplier = 2.0; + + FunctorizedFactor factor(key, X, model, multiplier); + + Values values; + values.insert(key, X); + + // Check 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(keyX, X, model, multiplier); + FunctorizedFactor factor(key, X, model, multiplier); // redirect output to buffer so we can compare stringstream buffer; @@ -107,13 +121,13 @@ TEST(FunctorizedFactor, Print) { cout.rdbuf(old); string expected = " keys = { X0 }\n" - " noise model: unit (3) \n" + " noise model: unit (9) \n" "FunctorizedFactor(X0)\n" " measurement: [\n" " 1, 0;\n" " 0, 1\n" "]\n" - " noise model sigmas: 1 1 1\n"; + " noise model sigmas: 1 1 1 1 1 1 1 1 1\n"; CHECK_EQUAL(expected, actual); }