borglab · ProfFan · Jun 7, 2021 · May 14, 2021 · May 17, 2021 · May 17, 2021
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
@@ -2166,6 +2166,34 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor {
   Vector whitenedError(const gtsam::Values& x) const;
 };
 
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor: gtsam::NoiseModelFactor {
+  /*
+   * Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there.
+   * This is achieved by adding `gtsam::CustomFactor` to the ignore list in `matlab/CMakeLists.txt`.
+   */
+  CustomFactor();
+  /*
+   * Example:
+   * ```
+   * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+   *    <calculated error>
+   *    if not H is None:
+   *        <calculate the Jacobian>
+   *        H[0] = J1 # 2-d numpy array for a Jacobian block
+   *        H[1] = J2
+   *        ...
+   *    return error # 1-d numpy array
+   *
+   * cf = CustomFactor(noise_model, keys, error_func)
+   * ```
+   */
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
+};
+
 #include <gtsam/nonlinear/Values.h>
 class Values {
   Values();

diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp
@@ -0,0 +1,76 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    CustomFactor.cpp
+ * @brief   Class to enable arbitrary factors with runtime swappable error function.
+ * @author  Fan Jiang
+ */
+
+#include <gtsam/nonlinear/CustomFactor.h>
+
+namespace gtsam {
+
+/*
+ * Calculates the unwhitened error by invoking the callback functor (i.e. from Python).
+ */
+Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
+  if(this->active(x)) {
+
+    if(H) {
+      /*
+       * In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
+       * As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in
+       * Python will be immediately reflected on the C++ side.
+       *
+       * Example:
+       * ```
+       * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+       *    <calculated error>
+       *    if not H is None:
+       *        <calculate the Jacobian>
+       *        H[0] = J1
+       *        H[1] = J2
+       *        ...
+       *    return error
+       * ```
+       */
+      return this->error_function_(*this, x, H.get_ptr());
+    } else {
+      /*
+       * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.
+       * Users can check for `None` in their callback to determine if the Jacobian is requested.
+       */
+      return this->error_function_(*this, x, nullptr);
+    }
+  } else {
+    return Vector::Zero(this->dim());
+  }
+}
+
+void CustomFactor::print(const std::string &s, const KeyFormatter &keyFormatter) const {
8000  std::cout << s << "CustomFactor on ";
+  auto keys_ = this->keys();
+  bool f = false;
+  for (const Key &k: keys_) {
+    if (f)
+      std::cout << ", ";
+    std::cout << keyFormatter(k);
+    f = true;
+  }
+  std::cout << "\n";
+  if (this->noiseModel_)
+    this->noiseModel_->print("  noise model: ");
+  else
+    std::cout << "no noise model" << std::endl;
+}
+
+}
diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h
@@ -0,0 +1,104 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    CustomFactor.h
+ * @brief   Class to enable arbitrary factors with runtime swappable error function.
+ * @author  Fan Jiang
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+using namespace gtsam;
+
+namespace gtsam {
+
+using JacobianVector = std::vector<Matrix>;
+
+class CustomFactor;
+
+/*
+ * NOTE
+ * ==========
+ * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
+ *
+ * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
+ * Thus the pointer will never be invalidated.
+ */
+using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
+
+/**
+ * @brief Custom factor that takes a std::function as the error
+ * @addtogroup nonlinear
+ * \nosubgrouping
+ *
+ * This factor is mainly for creating a custom factor in Python.
+ */
+class CustomFactor: public NoiseModelFactor {
+protected:
+  CustomErrorFunction error_function_;
+
+protected:
+
+  using Base = NoiseModelFactor;
+  using This = CustomFactor;
+
+public:
+
+  /**
+   * Default Constructor for I/O
+   */
+  CustomFactor() = default;
+
+  /**
+   * Constructor
+   * @param noiseModel shared pointer to noise model
+   * @param keys keys of the variables
+   * @param errorFunction the error functional
+   */
+  CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
+      Base(noiseModel, keys) {
+    this->error_function_ = errorFunction;
+  }
+
+  ~CustomFactor() override = default;
+
+  /**
+    * Calls the errorFunction closure, which is a std::function object
+    * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
+    */
+  Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
+
+  /** print */
+  void print(const std::str
6D40
ing &s,
+             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
+
+  /**
+   * Mark not sendable
+   */
+  bool sendable() const override {
+    return false;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar & boost::serialization::make_nvp("CustomFactor",
+                                        boost::serialization::base_object<Base>(*this));
+  }
+};
+
+};
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
@@ -95,7 +95,7 @@ class GTSAM_EXPORT NonlinearFactor: public Factor {
 
   /**
    * Checks whether a factor should be used based on a set of values.
-   * This is primarily used to implment inequality constraints that
+   * This is primarily used to implement inequality constraints that
    * require a variable active set. For all others, the default implementation
    * returning true solves this problem.
    *
@@ -134,6 +134,14 @@ class GTSAM_EXPORT NonlinearFactor: public Factor {
    */
   shared_ptr rekey(const KeyVector& new_keys) const;
 
+  /**
+   * Should the factor be evaluated in the same thread as the caller
+   * This is to enable factors that has shared states (like the Python GIL lock)
+   */
+   virtual bool sendable() const {
+    return true;
+  }
+
 }; // \class NonlinearFactor
 
 /// traits

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -325,7 +325,7 @@ class _LinearizeOneFactor {
   // Operator that linearizes a given range of the factors
   void operator()(const tbb::blocked_range<size_t>& blocked_range) const {
     for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) {
-      if (nonlinearGraph_[i])
+      if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable())
         result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_);
       else
         result_[i] = GaussianFactor::shared_ptr();
@@ -348,9 +348,19 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
 
   linearFG->resize(size());
   TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
+
+  // First linearize all sendable factors
   tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
     _LinearizeOneFactor(*this, linearizationPoint, *linearFG));
 
+  // Linearize all non-sendable factors
+  for(int i = 0; i < size(); i++) {
+    auto& factor = (*this)[i];
+    if(factor && !(factor->sendable())) {
+      (*linearFG)[i] = factor->linearize(linearizationPoint);
+    }
+  }
+
 #else
 
   linearFG->reserve(size());

diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt
@@ -61,7 +61,8 @@ endif()
 # ignoring the non-concrete types (type aliases)
 set(ignore
     gtsam::Point2
-    gtsam::Point3)
+    gtsam::Point3
+    gtsam::CustomFactor)
 
 # Wrap
 matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"

diff --git a/python/CustomFactors.md b/python/CustomFactors.md
@@ -0,0 +1,111 @@
+# GTSAM Python-based factors
+
+One now can build factors purely in Python using the `CustomFactor` factor.
+
+## Usage
+
+In order to use a Python-based factor, one needs to have a Python function with the following signature:
+
+```python
+import gtsam
+import numpy as np
+from typing import List
+
+def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+    ...
+```
+
+`this` is a reference to the `CustomFactor` object. This is required because one can reuse the same
+`error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of
+**references** to the list of required Jacobians (see the corresponding C++ documentation).
+
+If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error`
+method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`,
+each entry of `H` can be assigned a `numpy` array, as the Jacobian for the corresponding variable.
+
+After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM:
+
+```python
+noise_model = gtsam.noiseModel.Unit.Create(3)
+# constructor(<noise model>, <list of keys>, <error callback>)
+cf = gtsam.CustomFactor(noise_model, [X(0), X(1)], error_func)
+```
+
+## Example
+
+The following is a simple `BetweenFactor` implemented in Python.
+
+```python
+import gtsam
+import numpy as np
+from typing import List
+
+expected = Pose2(2, 2, np.pi / 2)
+
+def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+    """
+    Error function that mimics a BetweenFactor
+    :param this: reference to the current CustomFactor being evaluated
+    :param v: Values object
+    :param H: list of references to the Jacobian arrays
+    :return: the non-linear error
+    """
+    key0 = this.keys()[0]
+    key1 = this.keys()[1]
+    gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
+    error = expected.localCoordinates(gT1.between(gT2))
+
+    if H is not None:
+        result = gT1.between(gT2)
+        H[0] = -result.inverse().AdjointMap()
+        H[1] = np.eye(3)
+    return error
+
+noise_model = gtsam.noiseModel.Unit.Create(3)
+cf = gtsam.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
+```
+
+In general, the Python-based factor works just like their C++ counterparts.
+
+## Known Issues
+
+Because of the `pybind11`-based translation, the performance of `CustomFactor` is not guaranteed.
+Also, because `pybind11` needs to lock the Python GIL lock for evaluation of each factor, parallel
+evaluation of `CustomFactor` is not possible.
+
+## Implementation
+
+`CustomFactor` is a `NonlinearFactor` that has a `std::function` as its callback.
+This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.
+
+The constructor of `CustomFactor` is
+```c++
+/**
+* Constructor
+* @param noiseModel shared pointer to noise model
+* @param keys keys of the variables
+* @param errorFunction the error functional
+*/
+CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :
+  Base(noiseModel, keys) {
+  this->error_function_ = errorFunction;
+}
+```
+
+At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.
+
+Something worth special mention is this:
+```c++
+/*
+ * NOTE
+ * ==========
+ * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
+ *
+ * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
+ * Thus the pointer will never be invalidated.
+ */
+using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;
+```
+
+which is not documented in `pybind11` docs. One needs to be aware of this if they wanted to implement similar
+"mutable" arguments going across the Python-C++ boundary.