Added FunctorizedFactor and corresponding tests

gtsam/linear/NoiseModel.cpp

@@ -619,6 +619,7 @@ void Isotropic::WhitenInPlace(Eigen::Block<Matrix> 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;
 }
 

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 <gtsam/base/Testable.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <cmath>
+
+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 <typename FUNCTOR>
+class GTSAM_EXPORT FunctorizedFactor
+    : public NoiseModelFactor1<typename FUNCTOR::argument_type> {
+private:
+  using T = typename FUNCTOR::argument_type;
+  using Base = NoiseModelFactor1<T>;
+
+  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 <typename... Args>
+  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>(args)...) {}
+
+  virtual ~FunctorizedFactor() {}
+
+  /// @return a deep copy of this factor
+  virtual NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<NonlinearFactor>(
+        NonlinearFactor::shared_ptr(new FunctorizedFactor<FUNCTOR>(*this)));
+  }
+
+  Vector evaluateError(const T &params,
+                       boost::optional<Matrix &> H = boost::none) const {
+    typename FUNCTOR::return_type x = func_(params, H);
+    Vector error = traits<typename FUNCTOR::return_type>::Local(measured_, x);
+    return error;
+  }
+
+  /// @name Testable
+  /// @{
+  GTSAM_EXPORT friend std::ostream &
+  operator<<(std::ostream &os, const FunctorizedFactor<FUNCTOR> &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<typename FUNCTOR::return_type>::Print(measured_, "  measurement: ");
+    std::cout << *this << std::endl;
+  }
+
+    virtual bool equals(const NonlinearFactor &other, double tol = 1e-9)
+    const {
+      const FunctorizedFactor<FUNCTOR> *e =
+          dynamic_cast<const FunctorizedFactor<FUNCTOR>*>(&other);
+      const bool base = Base::equals(*e, tol);
+      return e != nullptr && base;
+    }
+  /// @}
+
+private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
+    ar &BOOST_SERIALIZATION_NVP(measured_);
+    ar &BOOST_SERIALIZATION_NVP(func_);
+  }
+};
+
+// TODO(Varun): Include or kill?
+// template <> struct traits<Functorized> : public Testable<ImuFactor2> {};
+
+} // namespace gtsam

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 <gtsam/base/Testable.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/FunctorizedFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+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<MultiplyFunctor> factor(keyX, X, model, multiplier);
+
+  Values values;
+  values.insert<Matrix>(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<MultiplyFunctor> factor(keyX, X, model, multiplier);
+
+  Values values;
+  values.insert<Matrix>(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<MultiplyFunctor> factor1(keyX, X, model, multiplier);
+  FunctorizedFactor<MultiplyFunctor> factor2(keyX, X, model, multiplier);
+
+  EXPECT(factor1.equals(factor2));
+}
+
+TEST(FunctorizedFactor, Print) {
+  Matrix X = Matrix::Identity(2, 2);
+
+  double multiplier = 2.0;
+
+  FunctorizedFactor<MultiplyFunctor> 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);
+}
+/* ************************************************************************* */