FunctorizedFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
 
#include <cmath>
 
namespace gtsam {

template <typename R, typename T>
class FunctorizedFactor : public NoiseModelFactorN<T> {
 private:
  using Base = NoiseModelFactorN<T>;
 
  R measured_;  
  SharedNoiseModel noiseModel_;                          
  std::function<R(T, OptionalMatrixType)> func_;  
 
 public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  FunctorizedFactor() {}

  FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
                    const std::function<R(T, OptionalMatrixType)> func)
      : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
 
  ~FunctorizedFactor() override {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
  }
 
  Vector evaluateError(const T &params, OptionalMatrixType H) const override {
#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
    if constexpr (internal::HasLocalJacobians<R>::value) {
      if (H) {
        Matrix Hprojection;
        const R value = func_(params, &Hprojection);
        typename traits<R>::ChartJacobian::Jacobian Hlocal;
        const Vector error =
            traits<R>::Local(measured_, value, OptionalNone, &Hlocal);
        *H = Hlocal * Hprojection;
        return error;
      }
    }
#endif
    R x = func_(params, H);
    Vector error = traits<R>::Local(measured_, x);
    return error;
  }

  void print(
      const std::string &s = "",
      const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s, keyFormatter);
    std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
              << keyFormatter(this->key1()) << ")" << std::endl;
    traits<R>::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 FunctorizedFactor<R, T> *e =
        dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
    return e != nullptr && Base::equals(other, tol) &&
           traits<R>::Equals(this->measured_, e->measured_, tol);
  }

 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
    // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
    ar &boost::serialization::make_nvp(
        "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
    ar &BOOST_SERIALIZATION_NVP(measured_);
    ar &BOOST_SERIALIZATION_NVP(func_);
  }
#endif
};

template <typename R, typename T>
struct traits<FunctorizedFactor<R, T>>
    : public Testable<FunctorizedFactor<R, T>> {};

template <typename T, typename R, typename FUNC>
FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
                                              const SharedNoiseModel &model,
                                              const FUNC func) {
  return FunctorizedFactor<R, T>(key, z, model, func);
}

template <typename R, typename T1, typename T2>
class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
 private:
  using Base = NoiseModelFactorN<T1, T2>;
 
  R measured_;  
  SharedNoiseModel noiseModel_;  
  using FunctionType = std::function<R(T1, T2, OptionalMatrixType, OptionalMatrixType)>;
  FunctionType func_;  
 
 public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  FunctorizedFactor2() {}

  FunctorizedFactor2(Key key1, Key key2, const R &z,
                     const SharedNoiseModel &model, const FunctionType func)
      : Base(model, key1, key2),
        measured_(z),
        noiseModel_(model),
        func_(func) {}
 
  ~FunctorizedFactor2() override {}

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
  }
 
  Vector evaluateError(
      const T1 &params1, const T2 &params2,
      OptionalMatrixType H1, OptionalMatrixType H2) const override {
#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
    if constexpr (internal::HasLocalJacobians<R>::value) {
      if (H1 || H2) {
        Matrix Hprojection1, Hprojection2;
        const R value = func_(params1, params2,
                              H1 ? &Hprojection1 : nullptr,
                              H2 ? &Hprojection2 : nullptr);
        typename traits<R>::ChartJacobian::Jacobian Hlocal;
        const Vector error =
            traits<R>::Local(measured_, value, OptionalNone, &Hlocal);
        if (H1) *H1 = Hlocal * Hprojection1;
        if (H2) *H2 = Hlocal * Hprojection2;
        return error;
      }
    }
#endif
    R x = func_(params1, params2, H1, H2);
    Vector error = traits<R>::Local(measured_, x);
    return error;
  }

  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<R>::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<R, T1, T2> *e =
        dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
    return e && Base::equals(other, tol) &&
           traits<R>::Equals(this->measured_, e->measured_, tol);
  }

 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
    ar &boost::serialization::make_nvp(
        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
    ar &BOOST_SERIALIZATION_NVP(measured_);
    ar &BOOST_SERIALIZATION_NVP(func_);
  }
#endif
};

template <typename R, typename T1, typename T2>
struct traits<FunctorizedFactor2<R, T1, T2>>
    : public Testable<FunctorizedFactor2<R, T1, T2>> {};

template <typename T1, typename T2, typename R, typename FUNC>
FunctorizedFactor2<R, T1, T2> MakeFunctorizedFactor2(
    Key key1, Key key2, const R &z, const SharedNoiseModel &model,
    const FUNC func) {
  return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
}
 
}  // namespace gtsam