Merge pull request #671 from borglab/feature/functorizedfactor2

FunctorizedFactor2

gtsam/nonlinear/FunctorizedFactor.h

@@ -87,8 +87,8 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
         NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
   }
 
-  Vector evaluateError(const T &params,
-                       boost::optional<Matrix &> H = boost::none) const override {
+  Vector evaluateError(const T &params, boost::optional<Matrix &> H =
+                                            boost::none) const override {
     R x = func_(params, H);
     Vector error = traits<R>::Local(measured_, x);
     return error;
@@ -96,8 +96,9 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
 
   /// @name Testable
   /// @{
-  void print(const std::string &s = "",
-             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
+  void print(
+      const std::string &s = "",
+      const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
               << keyFormatter(this->key()) << ")" << std::endl;
@@ -144,4 +145,111 @@ FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
   return FunctorizedFactor<R, T>(key, z, model, func);
 }
 
+/**
+ * Factor which evaluates provided binary 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.
+ * @param T1: The first argument type for the functor.
+ * @param T2: The second argument type for the functor.
+ */
+template <typename R, typename T1, typename T2>
+class GTSAM_EXPORT FunctorizedFactor2 : public NoiseModelFactor2<T1, T2> {
+ private:
+  using Base = NoiseModelFactor2<T1, T2>;
+
+  R measured_;  ///< value that is compared with functor return value
+  SharedNoiseModel noiseModel_;  ///< noise model
+  using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
+                                       boost::optional<Matrix &>)>;
+  FunctionType func_;  ///< functor instance
+
+ public:
+  /** default constructor - only use for serialization */
+  FunctorizedFactor2() {}
+
+  /** Construct with given x and the parameters of the basis
+   *
+   * @param key: Factor key
+   * @param z: Measurement object of same type as that returned by functor
+   * @param model: Noise model
+   * @param func: The instance of the functor object
+   */
+  FunctorizedFactor2(Key key1, Key key2, const R &z,
+                     const SharedNoiseModel &model, const FunctionType func)
+      : Base(model, key1, key2),
+        measured_(z),
+        noiseModel_(model),
+        func_(func) {}
+
+  virtual ~FunctorizedFactor2() {}
+
+  /// @return a deep copy of this factor
+  NonlinearFactor::shared_ptr clone() const override {
+    return boost::static_pointer_cast<NonlinearFactor>(
+        NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
+  }
+
+  Vector evaluateError(
+      const T1 &params1, const T2 &params2,
+      boost::optional<Matrix &> H1 = boost::none,
+      boost::optional<Matrix &> H2 = boost::none) const override {
+    R x = func_(params1, params2, H1, H2);
+    Vector error = traits<R>::Local(measured_, x);
+    return error;
+  }
+
+  /// @name Testable
+  /// @{
+  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:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &boost::serialization::make_nvp(
+        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
+    ar &BOOST_SERIALIZATION_NVP(measured_);
+    ar &BOOST_SERIALIZATION_NVP(func_);
+  }
+};
+
+/// traits
+template <typename R, typename T1, typename T2>
+struct traits<FunctorizedFactor2<R, T1, T2>>
+    : public Testable<FunctorizedFactor2<R, T1, T2>> {};
+
+/**
+ * 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 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

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

@@ -27,8 +27,15 @@
 using namespace std;
 using namespace gtsam;
 
+// Key for FunctorizedFactor
 Key key = Symbol('X', 0);
+
+// Keys for FunctorizedFactor2
+Key keyA = Symbol('A', 0);
+Key keyx = Symbol('x', 0);
+
 auto model = noiseModel::Isotropic::Sigma(9, 1);
+auto model2 = noiseModel::Isotropic::Sigma(3, 1);
 
 /// Functor that takes a matrix and multiplies every element by m
 class MultiplyFunctor {
@@ -44,6 +51,21 @@ class MultiplyFunctor {
   }
 };
 
+/// Functor that performs Ax where A is a matrix and x is a vector.
+class ProjectionFunctor {
+ public:
+  Vector operator()(const Matrix &A, const Vector &x,
+                    OptionalJacobian<-1, -1> H1 = boost::none,
+                    OptionalJacobian<-1, -1> H2 = boost::none) const {
+    if (H1) {
+      H1->resize(x.size(), A.size());
+      *H1 << I_3x3, I_3x3, I_3x3;
+    } 
+    if (H2) *H2 = A;
+    return A * x;
+  }
+};
+
 /* ************************************************************************* */
 // Test identity operation for FunctorizedFactor.
 TEST(FunctorizedFactor, Identity) {
@@ -88,7 +110,7 @@ TEST(FunctorizedFactor, Equality) {
   EXPECT(factor1.equals(factor2));
 }
 
-/* *************************************************************************** */
+/* ************************************************************************* */
 // Test Jacobians of FunctorizedFactor.
 TEST(FunctorizedFactor, Jacobians) {
   Matrix X = Matrix::Identity(3, 3);
@@ -168,6 +190,83 @@ TEST(FunctorizedFactor, Lambda) {
   EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
 }
 
+/* ************************************************************************* */
+// Test identity operation for FunctorizedFactor2.
+TEST(FunctorizedFactor, Identity2) {
+  // x = Ax since A is I_3x3
+  Matrix A = Matrix::Identity(3, 3);
+  Vector x = Vector::Ones(3);
+
+  auto functor = ProjectionFunctor();
+  auto factor =
+      MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2, functor);
+
+  Vector error = factor.evaluateError(A, x);
+
+  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test Jacobians of FunctorizedFactor2.
+TEST(FunctorizedFactor, Jacobians2) {
+  Matrix A = Matrix::Identity(3, 3);
+  Vector x = Vector::Ones(3);
+  Matrix actualH1, actualH2;
+
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, x, model2,
+                                                       ProjectionFunctor());
+
+  Values values;
+  values.insert<Matrix>(keyA, A);
+  values.insert<Vector>(keyx, x);
+
+  // Check Jacobians
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor2 using a std::function type.
+TEST(FunctorizedFactor, Functional2) {
+  Matrix A = Matrix::Identity(3, 3);
+  Vector3 x(1, 2, 3);
+  Vector measurement = A * x;
+
+  std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>,
+                       boost::optional<Matrix &>)>
+      functional = ProjectionFunctor();
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
+                                                       model2, functional);
+
+  Vector error = factor.evaluateError(A, x);
+
+  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
+}
+
+/* ************************************************************************* */
+// Test FunctorizedFactor2 with a lambda function.
+TEST(FunctorizedFactor, Lambda2) {
+  Matrix A = Matrix::Identity(3, 3);
+  Vector3 x = Vector3(1, 2, 3);
+  Matrix measurement = A * x;
+
+  auto lambda = [](const Matrix &A, const Vector &x,
+                   OptionalJacobian<-1, -1> H1 = boost::none,
+                   OptionalJacobian<-1, -1> H2 = boost::none) {
+    if (H1) {
+      H1->resize(x.size(), A.size());
+      *H1 << I_3x3, I_3x3, I_3x3;
+    } 
+    if (H2) *H2 = A;
+    return A * x;
+  };
+  // FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
+  auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement, model2, lambda);
+
+  Vector error = factor.evaluateError(A, x);
+
+  EXPECT(assert_equal(Vector::Zero(3), error, 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;