From 1db0f441bcd91878e562c9ad61c1a958be082cf0 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 31 May 2020 21:52:13 -0400
Subject: [PATCH 1/6] 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<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;
 }
 
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 <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
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 <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);
+}
+/* ************************************************************************* */

From 131213a983521b2095f15d3ce0d798645f513538 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 1 Jun 2020 19:52:50 -0400
Subject: [PATCH 2/6] 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<MultiplyFunctor> factor(keyX, measurement, model, multiplier);
  */
 template <typename FUNCTOR>
 class GTSAM_EXPORT FunctorizedFactor
@@ -82,27 +105,24 @@ public:
 
   /// @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;
+    std::cout << "  noise model sigmas: " << noiseModel_->sigmas().transpose()
+              << 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;
-    }
+  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 && Base::equals(other, tol) &&
+           traits<typename FUNCTOR::return_type>::Equals(this->measured_, e->measured_,
+                                                tol);
+  }
   /// @}
 
 private:
@@ -117,7 +137,9 @@ private:
   }
 };
 
-// TODO(Varun): Include or kill?
-// template <> struct traits<Functorized> : public Testable<ImuFactor2> {};
+/// traits
+template <typename FUNCTOR>
+struct traits<FunctorizedFactor<FUNCTOR>>
+    : public Testable<FunctorizedFactor<FUNCTOR>> {};
 
 } // 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 <gtsam/base/Testable.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/FunctorizedFactor.h>
+#include <gtsam/nonlinear/factorTesting.h>
 
 #include <CppUnitLite/TestHarness.h>
 
 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<MultiplyFunctor> factor(keyX, X, model, multiplier);
+  FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model,
+                                            multiplier);
 
-  Values values;
-  values.insert<Matrix>(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<MultiplyFunctor> factor(keyX, X, model, multiplier);
+  FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model, multiplier);
 
-  Values values;
-  values.insert<Matrix>(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<MultiplyFunctor> factor1(keyX, X, model, multiplier);
-  FunctorizedFactor<MultiplyFunctor> factor2(keyX, X, model, multiplier);
+  FunctorizedFactor<MultiplyFunctor> factor1(key, measurement, model,
+                                             multiplier);
+  FunctorizedFactor<MultiplyFunctor> 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<MultiplyFunctor> factor(key, X, model, multiplier);
+
+  Values values;
+  values.insert<Matrix>(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<MultiplyFunctor> factor(keyX, X, model, multiplier);
+  FunctorizedFactor<MultiplyFunctor> 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);
 }

From ea8b319c43ea64b563792c3c2b8d9ccdd298c5cc Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 1 Jun 2020 19:54:04 -0400
Subject: [PATCH 3/6] remove TODO

---
 gtsam/linear/NoiseModel.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index 72ca054b4..d7fd2d1ea 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -619,7 +619,6 @@ 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;
 }
 

From 0bbb39687f5c1b4f2e34e288786a9acd58a7e17d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 1 Jun 2020 19:55:10 -0400
Subject: [PATCH 4/6] improved documentation

---
 gtsam/nonlinear/FunctorizedFactor.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
index 03fd47a46..82d2f822e 100644
--- a/gtsam/nonlinear/FunctorizedFactor.h
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -37,8 +37,8 @@ namespace gtsam {
  * 
  * 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

From 7d0e440293fe87b2f5ff6e142bf10588d9c051ee Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jul 2020 17:38:34 -0400
Subject: [PATCH 5/6] new definition for FunctorizedFactor to allow for using
 std::function and lambdas

---
 gtsam/nonlinear/FunctorizedFactor.h           | 73 +++++++--------
 .../nonlinear/tests/testFunctorizedFactor.cpp | 89 ++++++++++++++-----
 2 files changed, 97 insertions(+), 65 deletions(-)

diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
index 82d2f822e..c88579587 100644
--- a/gtsam/nonlinear/FunctorizedFactor.h
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -27,14 +27,11 @@ namespace gtsam {
 /**
  * Factor which evaluates functor and uses the result to compute
  * error on provided measurement.
- * 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.
- * 
+ * @param R: The return type of the functor after evaluation.
+ * @param T: The argument type for the functor.
+ *
  * Example:
  *   Key key = Symbol('X', 0);
  *   auto model = noiseModel::Isotropic::Sigma(9, 1);
@@ -48,58 +45,53 @@ namespace gtsam {
  *     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;
+ *       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<MultiplyFunctor> factor(keyX, measurement, model, multiplier);
+ *
+ *   FunctorizedFactor<Matrix, Matrix> factor(keyX, measurement, model,
+ *     MultiplyFunctor(multiplier));
  */
-template <typename FUNCTOR>
-class GTSAM_EXPORT FunctorizedFactor
-    : public NoiseModelFactor1<typename FUNCTOR::argument_type> {
-private:
-  using T = typename FUNCTOR::argument_type;
+template <typename R, typename T>
+class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
+ private:
   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
+  R measured_;  ///< value that is compared with functor return value
+  SharedNoiseModel noiseModel_;                          ///< noise model
+  std::function<R(T, boost::optional<Matrix &>)> func_;  ///< functor instance
 
-public:
+ 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 z: Measurement object of type R
    * @param model: Noise model
-   * @param args: Variable number of arguments used to instantiate functor
+   * @param func: The instance of the functor object
    */
-  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)...) {}
+  FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
+                    const std::function<R(T, boost::optional<Matrix &>)> func)
+      : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
 
   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)));
+        NonlinearFactor::shared_ptr(new FunctorizedFactor<T, R>(*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);
+    R x = func_(params, H);
+    Vector error = traits<R>::Local(measured_, x);
     return error;
   }
 
@@ -110,22 +102,21 @@ public:
     Base::print(s, keyFormatter);
     std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
               << keyFormatter(this->key()) << ")" << std::endl;
-    traits<typename FUNCTOR::return_type>::Print(measured_, "  measurement: ");
+    traits<R>::Print(measured_, "  measurement: ");
     std::cout << "  noise model sigmas: " << noiseModel_->sigmas().transpose()
               << std::endl;
   }
 
   virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
-    const FunctorizedFactor<FUNCTOR> *e =
-        dynamic_cast<const FunctorizedFactor<FUNCTOR> *>(&other);
+    const FunctorizedFactor<T, R> *e =
+        dynamic_cast<const FunctorizedFactor<T, R> *>(&other);
     const bool base = Base::equals(*e, tol);
     return e && Base::equals(other, tol) &&
-           traits<typename FUNCTOR::return_type>::Equals(this->measured_, e->measured_,
-                                                tol);
+           traits<R>::Equals(this->measured_, e->measured_, tol);
   }
   /// @}
 
-private:
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -138,8 +129,8 @@ private:
 };
 
 /// traits
-template <typename FUNCTOR>
-struct traits<FunctorizedFactor<FUNCTOR>>
-    : public Testable<FunctorizedFactor<FUNCTOR>> {};
+template <typename T, typename R>
+struct traits<FunctorizedFactor<T, R>>
+    : public Testable<FunctorizedFactor<T, R>> {};
 
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
index 9393a4410..9ff6b8e24 100644
--- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
+++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
@@ -17,13 +17,12 @@
  * @brief unit tests for FunctorizedFactor class
  */
 
+#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/FunctorizedFactor.h>
 #include <gtsam/nonlinear/factorTesting.h>
 
-#include <CppUnitLite/TestHarness.h>
-
 using namespace std;
 using namespace gtsam;
 
@@ -32,9 +31,9 @@ auto model = noiseModel::Isotropic::Sigma(9, 1);
 
 /// Functor that takes a matrix and multiplies every element by m
 class MultiplyFunctor {
-  double m_; ///< simple multiplier
+  double m_;  ///< simple multiplier
 
-public:
+ public:
   using argument_type = Matrix;
   using return_type = Matrix;
 
@@ -42,32 +41,33 @@ 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());
+    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), measurement = Matrix::Identity(3, 3);
 
   double multiplier = 1.0;
 
-  FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model,
-                                            multiplier);
+  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
+                                           MultiplyFunctor(multiplier));
 
   Vector error = factor.evaluateError(X);
 
   EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
 }
 
+/* ************************************************************************* */
 TEST(FunctorizedFactor, Multiply2) {
   double multiplier = 2.0;
   Matrix X = Matrix::Identity(3, 3);
   Matrix measurement = multiplier * Matrix::Identity(3, 3);
 
-  FunctorizedFactor<MultiplyFunctor> factor(key, measurement, model, multiplier);
+  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
+                                           MultiplyFunctor(multiplier));
 
   Vector error = factor.evaluateError(X);
 
@@ -79,10 +79,10 @@ TEST(FunctorizedFactor, Equality) {
 
   double multiplier = 2.0;
 
-  FunctorizedFactor<MultiplyFunctor> factor1(key, measurement, model,
-                                             multiplier);
-  FunctorizedFactor<MultiplyFunctor> factor2(key, measurement, model,
-                                             multiplier);
+  FunctorizedFactor<Matrix, Matrix> factor1(key, measurement, model,
+                                            MultiplyFunctor(multiplier));
+  FunctorizedFactor<Matrix, Matrix> factor2(key, measurement, model,
+                                            MultiplyFunctor(multiplier));
 
   EXPECT(factor1.equals(factor2));
 }
@@ -94,7 +94,8 @@ TEST(FunctorizedFactor, Jacobians) {
 
   double multiplier = 2.0;
 
-  FunctorizedFactor<MultiplyFunctor> factor(key, X, model, multiplier);
+  FunctorizedFactor<Matrix, Matrix> factor(key, X, model,
+                                           MultiplyFunctor(multiplier));
 
   Values values;
   values.insert<Matrix>(key, X);
@@ -103,12 +104,14 @@ TEST(FunctorizedFactor, 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<MultiplyFunctor> factor(key, X, model, multiplier);
+  FunctorizedFactor<Matrix, Matrix> factor(key, X, model,
+                                           MultiplyFunctor(multiplier));
 
   // redirect output to buffer so we can compare
   stringstream buffer;
@@ -120,18 +123,56 @@ TEST(FunctorizedFactor, Print) {
   string actual = buffer.str();
   cout.rdbuf(old);
 
-  string expected = "  keys = { X0 }\n"
-                    "  noise model: unit (9) \n"
-                    "FunctorizedFactor(X0)\n"
-                    "  measurement: [\n"
-                    "	1, 0;\n"
-                    " 	0, 1\n"
-                    "]\n"
-                    "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
+  string expected =
+      "  keys = { X0 }\n"
+      "  noise model: unit (9) \n"
+      "FunctorizedFactor(X0)\n"
+      "  measurement: [\n"
+      "	1, 0;\n"
+      " 	0, 1\n"
+      "]\n"
+      "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
 
   CHECK_EQUAL(expected, actual);
 }
 
+/* ************************************************************************* */
+// Test factor using a std::function type.
+TEST(FunctorizedFactor, Functional) {
+  double multiplier = 2.0;
+  Matrix X = Matrix::Identity(3, 3);
+  Matrix measurement = multiplier * Matrix::Identity(3, 3);
+
+  std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
+      MultiplyFunctor(multiplier);
+  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model, functional);
+
+  Vector error = factor.evaluateError(X);
+
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(FunctorizedFactor, Lambda) {
+  double multiplier = 2.0;
+  Matrix X = Matrix::Identity(3, 3);
+  Matrix measurement = multiplier * Matrix::Identity(3, 3);
+
+  auto lambda = [multiplier](const Matrix &X,
+                             OptionalJacobian<-1, -1> H = boost::none) {
+    if (H)
+      *H = multiplier *
+           Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
+    return multiplier * X;
+  };
+  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
+  auto factor = FunctorizedFactor<Matrix>(key, measurement, model, lambda);
+
+  Vector error = factor.evaluateError(X);
+
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
+}
+
 /* *************************************************************************
  */
 int main() {

From 30ffcdd1371985b5415e7b22918c65ecbc42789e Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 6 Jul 2020 21:48:51 -0400
Subject: [PATCH 6/6] Simplified FunctorizedFactor

By adding the helper function MakeFunctorizedFactor, we now only need to provide the argument type in the template parameter list. This considerably simplifies the factor declaration, while removing the need for argument type and return type in the functor definition.

Also added tests for std::function and lambda functions.
---
 gtsam/nonlinear/FunctorizedFactor.h           | 38 ++++++++++-----
 .../nonlinear/tests/testFunctorizedFactor.cpp | 47 ++++++++++---------
 2 files changed, 50 insertions(+), 35 deletions(-)

diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
index c88579587..a83198967 100644
--- a/gtsam/nonlinear/FunctorizedFactor.h
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -25,8 +25,8 @@
 namespace gtsam {
 
 /**
- * Factor which evaluates functor and uses the result to compute
- * error on provided measurement.
+ * Factor which evaluates provided unary 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.
@@ -40,13 +40,12 @@ namespace gtsam {
  *   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;
+ *       if (H)
+ *         *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols());
+ *       return m_ * X;
  *     }
  *   };
  *
@@ -72,7 +71,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
   /** Construct with given x and the parameters of the basis
    *
    * @param key: Factor key
-   * @param z: Measurement object of type R
+   * @param z: Measurement object of same type as that returned by functor
    * @param model: Noise model
    * @param func: The instance of the functor object
    */
@@ -85,7 +84,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
   /// @return a deep copy of this factor
   virtual NonlinearFactor::shared_ptr clone() const {
     return boost::static_pointer_cast<NonlinearFactor>(
-        NonlinearFactor::shared_ptr(new FunctorizedFactor<T, R>(*this)));
+        NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
   }
 
   Vector evaluateError(const T &params,
@@ -108,8 +107,8 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
   }
 
   virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
-    const FunctorizedFactor<T, R> *e =
-        dynamic_cast<const FunctorizedFactor<T, R> *>(&other);
+    const FunctorizedFactor<R, T> *e =
+        dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
     const bool base = Base::equals(*e, tol);
     return e && Base::equals(other, tol) &&
            traits<R>::Equals(this->measured_, e->measured_, tol);
@@ -129,8 +128,21 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
 };
 
 /// traits
-template <typename T, typename R>
-struct traits<FunctorizedFactor<T, R>>
-    : public Testable<FunctorizedFactor<T, R>> {};
+template <typename R, typename T>
+struct traits<FunctorizedFactor<R, T>>
+    : public Testable<FunctorizedFactor<R, T>> {};
+
+/**
+ * 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 <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);
+}
 
 }  // namespace gtsam
diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
index 9ff6b8e24..12dd6b91c 100644
--- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
+++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
@@ -34,9 +34,6 @@ 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,
@@ -47,13 +44,13 @@ class MultiplyFunctor {
 };
 
 /* ************************************************************************* */
+// Test identity operation for FunctorizedFactor.
 TEST(FunctorizedFactor, Identity) {
   Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
 
   double multiplier = 1.0;
-
-  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
-                                           MultiplyFunctor(multiplier));
+  auto functor = MultiplyFunctor(multiplier);
+  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, functor);
 
   Vector error = factor.evaluateError(X);
 
@@ -61,41 +58,45 @@ TEST(FunctorizedFactor, Identity) {
 }
 
 /* ************************************************************************* */
+// Test FunctorizedFactor with multiplier value of 2.
 TEST(FunctorizedFactor, Multiply2) {
   double multiplier = 2.0;
   Matrix X = Matrix::Identity(3, 3);
   Matrix measurement = multiplier * Matrix::Identity(3, 3);
 
-  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
-                                           MultiplyFunctor(multiplier));
+  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model,
+                                              MultiplyFunctor(multiplier));
 
   Vector error = factor.evaluateError(X);
 
   EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
 }
 
+/* ************************************************************************* */
+// Test equality function for FunctorizedFactor.
 TEST(FunctorizedFactor, Equality) {
   Matrix measurement = Matrix::Identity(2, 2);
 
   double multiplier = 2.0;
 
-  FunctorizedFactor<Matrix, Matrix> factor1(key, measurement, model,
-                                            MultiplyFunctor(multiplier));
-  FunctorizedFactor<Matrix, Matrix> factor2(key, measurement, model,
-                                            MultiplyFunctor(multiplier));
+  auto factor1 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
+                                               MultiplyFunctor(multiplier));
+  auto factor2 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
+                                               MultiplyFunctor(multiplier));
 
   EXPECT(factor1.equals(factor2));
 }
 
-//******************************************************************************
+/* *************************************************************************** */
+// Test Jacobians of FunctorizedFactor.
 TEST(FunctorizedFactor, Jacobians) {
   Matrix X = Matrix::Identity(3, 3);
   Matrix actualH;
 
   double multiplier = 2.0;
 
-  FunctorizedFactor<Matrix, Matrix> factor(key, X, model,
-                                           MultiplyFunctor(multiplier));
+  auto factor =
+      MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
 
   Values values;
   values.insert<Matrix>(key, X);
@@ -105,13 +106,14 @@ TEST(FunctorizedFactor, Jacobians) {
 }
 
 /* ************************************************************************* */
+// Test print result of FunctorizedFactor.
 TEST(FunctorizedFactor, Print) {
   Matrix X = Matrix::Identity(2, 2);
 
   double multiplier = 2.0;
 
-  FunctorizedFactor<Matrix, Matrix> factor(key, X, model,
-                                           MultiplyFunctor(multiplier));
+  auto factor =
+      MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
 
   // redirect output to buffer so we can compare
   stringstream buffer;
@@ -137,7 +139,7 @@ TEST(FunctorizedFactor, Print) {
 }
 
 /* ************************************************************************* */
-// Test factor using a std::function type.
+// Test FunctorizedFactor using a std::function type.
 TEST(FunctorizedFactor, Functional) {
   double multiplier = 2.0;
   Matrix X = Matrix::Identity(3, 3);
@@ -145,7 +147,8 @@ TEST(FunctorizedFactor, Functional) {
 
   std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
       MultiplyFunctor(multiplier);
-  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model, functional);
+  auto factor =
+      MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
 
   Vector error = factor.evaluateError(X);
 
@@ -153,6 +156,7 @@ TEST(FunctorizedFactor, Functional) {
 }
 
 /* ************************************************************************* */
+// Test FunctorizedFactor with a lambda function.
 TEST(FunctorizedFactor, Lambda) {
   double multiplier = 2.0;
   Matrix X = Matrix::Identity(3, 3);
@@ -166,15 +170,14 @@ TEST(FunctorizedFactor, Lambda) {
     return multiplier * X;
   };
   // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
-  auto factor = FunctorizedFactor<Matrix>(key, measurement, model, lambda);
+  auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, lambda);
 
   Vector error = factor.evaluateError(X);
 
   EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
 }
 
-/* *************************************************************************
- */
+/* ************************************************************************* */
 int main() {
   TestResult tr;
   return TestRegistry::runAllTests(tr);