new definition for FunctorizedFactor to allow for using std::function and lambdas

gtsam/nonlinear/FunctorizedFactor.h

@@ -27,13 +27,10 @@ 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);
@@ -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());
+ *       if (H) *H = m_ * Matrix::Identity(X.rows()*X.cols(),
- *       return m_ * X;
+ * 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>
+template <typename R, typename T>
-class GTSAM_EXPORT FunctorizedFactor
+class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
-    : public NoiseModelFactor1<typename FUNCTOR::argument_type> {
+ private:
-private:
-  using T = typename FUNCTOR::argument_type;
   using Base = NoiseModelFactor1<T>;
 
-  typename FUNCTOR::return_type
+  R measured_;  ///< value that is compared with functor return value
-      measured_; ///< value that is compared with functor return value
   SharedNoiseModel noiseModel_;                          ///< noise model
-  FUNCTOR func_;                ///< functor instance
+  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 R &z, const SharedNoiseModel &model,
-  FunctorizedFactor(Key key, const typename FUNCTOR::return_type &z,
+                    const std::function<R(T, boost::optional<Matrix &>)> func)
-                    const SharedNoiseModel &model, Args &&... args)
+      : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
-      : 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)));
+        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);
+    R x = func_(params, H);
-    Vector error = traits<typename FUNCTOR::return_type>::Local(measured_, x);
+    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 =
+    const FunctorizedFactor<T, R> *e =
-        dynamic_cast<const FunctorizedFactor<FUNCTOR> *>(&other);
+        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_,
+           traits<R>::Equals(this->measured_, e->measured_, tol);
-                                                tol);
   }
   /// @}
 
-private:
+ private:
   /** Serialization function */
   friend class boost::serialization::access;
   template <class ARCHIVE>
@@ -138,8 +129,8 @@ private:
 };
 
 /// traits
-template <typename FUNCTOR>
+template <typename T, typename R>
-struct traits<FunctorizedFactor<FUNCTOR>>
+struct traits<FunctorizedFactor<T, R>>
-    : public Testable<FunctorizedFactor<FUNCTOR>> {};
+    : public Testable<FunctorizedFactor<T, R>> {};
 
 }  // namespace gtsam

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;
 
@@ -34,7 +33,7 @@ auto model = noiseModel::Isotropic::Sigma(9, 1);
 class MultiplyFunctor {
   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)
+    if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
-      *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,
+  FunctorizedFactor<Matrix, Matrix> factor(key, measurement, model,
-                                            multiplier);
+                                           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,
+  FunctorizedFactor<Matrix, Matrix> factor1(key, measurement, model,
-                                             multiplier);
+                                            MultiplyFunctor(multiplier));
-  FunctorizedFactor<MultiplyFunctor> factor2(key, measurement, model,
+  FunctorizedFactor<Matrix, Matrix> factor2(key, measurement, model,
-                                             multiplier);
+                                            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,7 +123,8 @@ TEST(FunctorizedFactor, Print) {
   string actual = buffer.str();
   cout.rdbuf(old);
 
-  string expected = "  keys = { X0 }\n"
+  string expected =
+      "  keys = { X0 }\n"
       "  noise model: unit (9) \n"
       "FunctorizedFactor(X0)\n"
       "  measurement: [\n"
@@ -132,6 +136,43 @@ TEST(FunctorizedFactor, Print) {
   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() {