fixes, better tests, docs

gtsam/nonlinear/FunctorizedFactor.h

@@ -11,6 +11,7 @@
 
 /**
  * @file FunctorizedFactor.h
+ * @date May 31, 2020
  * @author Varun Agrawal
  **/
 
@@ -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,26 +105,23 @@ 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)
+  virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
-    const {
     const FunctorizedFactor<FUNCTOR> *e =
-          dynamic_cast<const FunctorizedFactor<FUNCTOR>*>(&other);
+        dynamic_cast<const FunctorizedFactor<FUNCTOR> *>(&other);
     const bool base = Base::equals(*e, tol);
-      return e != nullptr && base;
+    return e && Base::equals(other, tol) &&
+           traits<typename FUNCTOR::return_type>::Equals(this->measured_, e->measured_,
+                                                tol);
   }
   /// @}
 
@@ -117,7 +137,9 @@ private:
   }
 };
 
-// TODO(Varun): Include or kill?
+/// traits
-// template <> struct traits<Functorized> : public Testable<ImuFactor2> {};
+template <typename FUNCTOR>
+struct traits<FunctorizedFactor<FUNCTOR>>
+    : public Testable<FunctorizedFactor<FUNCTOR>> {};
 
 } // namespace gtsam

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);
+Key key = Symbol('X', 0);
-auto model = noiseModel::Isotropic::Sigma(3, 1);
+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;
+  Vector error = factor.evaluateError(X);
-  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;
+  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;
+  Vector error = factor.evaluateError(X);
-  values.insert<Matrix>(keyX, X);
 
-  Matrix error = factor.evaluateError(X);
+  EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
-
-  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);
+  Matrix measurement = Matrix::Identity(2, 2);
 
   double multiplier = 2.0;
 
-  FunctorizedFactor<MultiplyFunctor> factor1(keyX, X, model, multiplier);
+  FunctorizedFactor<MultiplyFunctor> factor1(key, measurement, model,
-  FunctorizedFactor<MultiplyFunctor> factor2(keyX, X, model, multiplier);
+                                             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);
 }