From 252af8f0c59837513d704c0841571fad013989eb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 15 Jan 2021 12:41:31 -0500 Subject: [PATCH] added FunctorizedFactor2 --- gtsam/nonlinear/FunctorizedFactor.h | 116 +++++++++++++++++- .../nonlinear/tests/testFunctorizedFactor.cpp | 101 ++++++++++++++- 2 files changed, 212 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 688b3aaa26..53a48f0972 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -87,8 +87,8 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); } - Vector evaluateError(const T ¶ms, - boost::optional H = boost::none) const override { + Vector evaluateError(const T ¶ms, boost::optional H = + boost::none) const override { R x = func_(params, H); Vector error = traits::Local(measured_, x); return error; @@ -96,8 +96,9 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { /// @name Testable /// @{ - void print(const std::string &s = "", - const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { + void print( + const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" << keyFormatter(this->key()) << ")" << std::endl; @@ -144,4 +145,111 @@ FunctorizedFactor MakeFunctorizedFactor(Key key, const R &z, return FunctorizedFactor(key, z, model, func); } +/** + * Factor which evaluates provided binary 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 T1: The first argument type for the functor. + * @param T2: The second argument type for the functor. + */ +template +class GTSAM_EXPORT FunctorizedFactor2 : public NoiseModelFactor2 { + private: + using Base = NoiseModelFactor2; + + R measured_; ///< value that is compared with functor return value + SharedNoiseModel noiseModel_; ///< noise model + using FunctionType = std::function, + boost::optional)>; + FunctionType func_; ///< functor instance + + public: + /** default constructor - only use for serialization */ + FunctorizedFactor2() {} + + /** 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 + */ + FunctorizedFactor2(Key key1, Key key2, const R &z, + const SharedNoiseModel &model, const FunctionType func) + : Base(model, key1, key2), + measured_(z), + noiseModel_(model), + func_(func) {} + + virtual ~FunctorizedFactor2() {} + + /// @return a deep copy of this factor + NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new FunctorizedFactor2(*this))); + } + + Vector evaluateError( + const T1 ¶ms1, const T2 ¶ms2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + R x = func_(params1, params2, H1, H2); + Vector error = traits::Local(measured_, x); + return error; + } + + /// @name Testable + /// @{ + void print( + const std::string &s = "", + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override { + Base::print(s, keyFormatter); + std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" + << keyFormatter(this->key1()) << ", " + << keyFormatter(this->key2()) << ")" << std::endl; + traits::Print(measured_, " measurement: "); + std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() + << std::endl; + } + + bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { + const FunctorizedFactor2 *e = + dynamic_cast *>(&other); + 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( + "NoiseModelFactor2", 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 +FunctorizedFactor2 MakeFunctorizedFactor2( + Key key1, Key key2, const R &z, const SharedNoiseModel &model, + const FUNC func) { + return FunctorizedFactor2(key1, key2, z, model, func); +} + } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index cb91320cf9..b0ec5e7229 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -27,8 +27,15 @@ using namespace std; using namespace gtsam; +// Key for FunctorizedFactor Key key = Symbol('X', 0); + +// Keys for FunctorizedFactor2 +Key keyA = Symbol('A', 0); +Key keyx = Symbol('x', 0); + auto model = noiseModel::Isotropic::Sigma(9, 1); +auto model2 = noiseModel::Isotropic::Sigma(3, 1); /// Functor that takes a matrix and multiplies every element by m class MultiplyFunctor { @@ -44,6 +51,21 @@ class MultiplyFunctor { } }; +/// Functor that performs Ax where A is a matrix and x is a vector. +class ProjectionFunctor { + public: + Vector operator()(const Matrix &A, const Vector &x, + OptionalJacobian<-1, -1> H1 = boost::none, + OptionalJacobian<-1, -1> H2 = boost::none) const { + if (H1) { + H1->resize(x.size(), A.size()); + *H1 << I_3x3, I_3x3, I_3x3; + } + if (H2) *H2 = A; + return A * x; + } +}; + /* ************************************************************************* */ // Test identity operation for FunctorizedFactor. TEST(FunctorizedFactor, Identity) { @@ -88,7 +110,7 @@ TEST(FunctorizedFactor, Equality) { EXPECT(factor1.equals(factor2)); } -/* *************************************************************************** */ +/* ************************************************************************* */ // Test Jacobians of FunctorizedFactor. TEST(FunctorizedFactor, Jacobians) { Matrix X = Matrix::Identity(3, 3); @@ -168,6 +190,83 @@ TEST(FunctorizedFactor, Lambda) { EXPECT(assert_equal(Vector::Zero(9), error, 1e-9)); } +/* ************************************************************************* */ +// Test identity operation for FunctorizedFactor2. +TEST(FunctorizedFactor, Identity2) { + // x = Ax since A is I_3x3 + Matrix A = Matrix::Identity(3, 3); + Vector x = Vector::Ones(3); + + auto functor = ProjectionFunctor(); + auto factor = + MakeFunctorizedFactor2(keyA, keyx, x, model2, functor); + + Vector error = factor.evaluateError(A, x); + + EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); +} + +/* ************************************************************************* */ +// Test Jacobians of FunctorizedFactor2. +TEST(FunctorizedFactor, Jacobians2) { + Matrix A = Matrix::Identity(3, 3); + Vector x = Vector::Ones(3); + Matrix actualH1, actualH2; + + auto factor = MakeFunctorizedFactor2(keyA, keyx, x, model2, + ProjectionFunctor()); + + Values values; + values.insert(keyA, A); + values.insert(keyx, x); + + // Check Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +// Test FunctorizedFactor2 using a std::function type. +TEST(FunctorizedFactor, Functional2) { + Matrix A = Matrix::Identity(3, 3); + Vector3 x(1, 2, 3); + Vector measurement = A * x; + + std::function, + boost::optional)> + functional = ProjectionFunctor(); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, functional); + + Vector error = factor.evaluateError(A, x); + + EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); +} + +/* ************************************************************************* */ +// Test FunctorizedFactor2 with a lambda function. +TEST(FunctorizedFactor, Lambda2) { + Matrix A = Matrix::Identity(3, 3); + Vector3 x = Vector3(1, 2, 3); + Matrix measurement = A * x; + + auto lambda = [](const Matrix &A, const Vector &x, + OptionalJacobian<-1, -1> H1 = boost::none, + OptionalJacobian<-1, -1> H2 = boost::none) { + if (H1) { + H1->resize(x.size(), A.size()); + *H1 << I_3x3, I_3x3, I_3x3; + } + if (H2) *H2 = A; + return A * x; + }; + // FunctorizedFactor factor(key, measurement, model, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + + Vector error = factor.evaluateError(A, x); + + EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr;