Merge pull request #767 from borglab/feature/custom_factor

Custom Factors in Python

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();

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 {
+  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;
+}
+
+}

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::string &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));
+  }
+};
+
+};

gtsam/nonlinear/NonlinearFactor.h

@@ -95,7 +95,7 @@ public:
 
   /**
    * 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 @@ public:
    */
   virtual 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

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -325,7 +325,7 @@ public:
   // 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());

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}"

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.

python/gtsam/examples/CustomFactorExample.py

@@ -0,0 +1,179 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+CustomFactor demo that simulates a 1-D sensor fusion task.
+Author: Fan Jiang, Frank Dellaert
+"""
+
+import gtsam
+import numpy as np
+
+from typing import List, Optional
+from functools import partial
+
+
+def simulate_car():
+    # Simulate a car for one second
+    x0 = 0
+    dt = 0.25  # 4 Hz, typical GPS
+    v = 144 * 1000 / 3600  # 144 km/hour = 90mph, pretty fast
+    x = [x0 + v * dt * i for i in range(5)]
+
+    return x
+
+
+x = simulate_car()
+print(f"Simulated car trajectory: {x}")
+
+# %%
+add_noise = True  # set this to False to run with "perfect" measurements
+
+# GPS measurements
+sigma_gps = 3.0  # assume GPS is +/- 3m
+g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
+     for k in range(5)]
+
+# Odometry measurements
+sigma_odo = 0.1  # assume Odometry is 10cm accurate at 4Hz
+o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0)
+     for k in range(4)]
+
+# Landmark measurements:
+sigma_lm = 1  # assume landmark measurement is accurate up to 1m
+
+# Assume first landmark is at x=5, we measure it at time k=0
+lm_0 = 5.0
+z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
+
+# Assume other landmark is at x=28, we measure it at time k=3
+lm_3 = 28.0
+z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
+
+unknown = [gtsam.symbol('x', k) for k in range(5)]
+
+print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
+
+# We now can use nonlinear factor graphs
+factor_graph = gtsam.NonlinearFactorGraph()
+
+# Add factors for GPS measurements
+I = np.eye(1)
+gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
+
+
+def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
+    """GPS Factor error function
+    :param measurement: GPS measurement, to be filled with `partial`
+    :param this: gtsam.CustomFactor handle
+    :param values: gtsam.Values
+    :param jacobians: Optional list of Jacobians
+    :return: the unwhitened error
+    """
+    key = this.keys()[0]
+    estimate = values.atVector(key)
+    error = estimate - measurement
+    if jacobians is not None:
+        jacobians[0] = I
+
+    return error
+
+
+# Add the GPS factors
+for k in range(5):
+    gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]])))
+    factor_graph.add(gf)
+
+# New Values container
+v = gtsam.Values()
+
+# Add initial estimates to the Values container
+for i in range(5):
+    v.insert(unknown[i], np.array([0.0]))
+
+# Initialize optimizer
+params = gtsam.GaussNewtonParams()
+optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
+
+# Optimize the factor graph
+result = optimizer.optimize()
+
+# calculate the error from ground truth
+error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
+
+print("Result with only GPS")
+print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
+
+# Adding odometry will improve things a lot
+odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
+
+
+def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
+    """Odometry Factor error function
+    :param measurement: Odometry measurement, to be filled with `partial`
+    :param this: gtsam.CustomFactor handle
+    :param values: gtsam.Values
+    :param jacobians: Optional list of Jacobians
+    :return: the unwhitened error
+    """
+    key1 = this.keys()[0]
+    key2 = this.keys()[1]
+    pos1, pos2 = values.atVector(key1), values.atVector(key2)
+    error = measurement - (pos1 - pos2)
+    if jacobians is not None:
+        jacobians[0] = I
+        jacobians[1] = -I
+
+    return error
+
+
+for k in range(4):
+    odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]])))
+    factor_graph.add(odof)
+
+params = gtsam.GaussNewtonParams()
+optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
+
+result = optimizer.optimize()
+
+error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
+
+print("Result with GPS+Odometry")
+print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")
+
+# This is great, but GPS noise is still apparent, so now we add the two landmarks
+lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
+
+
+def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]):
+    """Landmark Factor error function
+    :param measurement: Landmark measurement, to be filled with `partial`
+    :param this: gtsam.CustomFactor handle
+    :param values: gtsam.Values
+    :param jacobians: Optional list of Jacobians
+    :return: the unwhitened error
+    """
+    key = this.keys()[0]
+    pos = values.atVector(key)
+    error = pos - measurement
+    if jacobians is not None:
+        jacobians[0] = I
+
+    return error
+
+
+factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0]))))
+factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3]))))
+
+params = gtsam.GaussNewtonParams()
+optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
+
+result = optimizer.optimize()
+
+error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)])
+
+print("Result with GPS+Odometry+Landmark")
+print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}")

python/gtsam/preamble.h

@@ -1,14 +1,30 @@
-// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
-// These are required to save one copy operation on Python calls
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
+ * such that the raw objects can be accessed in Python. Without this they will be automatically
+ * converted to a Python object, and all mutations on Python side will not be reflected on C++.
+ */
 #ifdef GTSAM_ALLOCATOR_TBB
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
 #else
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
 #endif
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> >);
+PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
+PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
 PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
 PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>); // JacobianVector
+
+// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this.
+namespace std {
+  using gtsam::operator<<;
+}

python/gtsam/specializations.h

@@ -1,10 +1,25 @@
-// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
-// These are required to save one copy operation on Python calls
+ * These are required to save one copy operation on Python calls.
+ *
+ * NOTES
+ * =================
+ *
+ * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding,
+ * such that the raw objects can be accessed in Python. Without this they will be automatically
+ * converted to a Python object, and all mutations on Python side will not be reflected on C++.
+ *
+ * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but
+ * without the `<pybind11/stl.h>` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this
+ * allows the types to be modified with Python, and saves one copy operation.
+ */
 #ifdef GTSAM_ALLOCATOR_TBB
 py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
+py::implicitly_convertible<py::list, std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >();
 #else
 py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
+py::implicitly_convertible<py::list, std::vector<gtsam::Key> >();
 #endif
+
 py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
 py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "Point3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
@@ -17,3 +32,4 @@ py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
+py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

python/gtsam/tests/test_custom_factor.py

@@ -0,0 +1,207 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+CustomFactor unit tests.
+Author: Fan Jiang
+"""
+from typing import List
+import unittest
+from gtsam import Values, Pose2, CustomFactor
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestCustomFactor(GtsamTestCase):
+    def test_new(self):
+        """Test the creation of a new CustomFactor"""
+
+        def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+            """Minimal error function stub"""
+            return np.array([1, 0, 0])
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func)
+
+    def test_new_keylist(self):
+        """Test the creation of a new CustomFactor"""
+
+        def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+            """Minimal error function stub"""
+            return np.array([1, 0, 0])
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        cf = CustomFactor(noise_model, [0], error_func)
+
+    def test_call(self):
+        """Test if calling the factor works (only error)"""
+        expected_pose = Pose2(1, 1, 0)
+
+        def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray:
+            """Minimal error function with no Jacobian"""
+            key0 = this.keys()[0]
+            error = -v.atPose2(key0).localCoordinates(expected_pose)
+            return error
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        cf = CustomFactor(noise_model, [0], error_func)
+        v = Values()
+        v.insert(0, Pose2(1, 0, 0))
+        e = cf.error(v)
+
+        self.assertEqual(e, 0.5)
+
+    def test_jacobian(self):
+        """Tests if the factor result matches the GTSAM Pose2 unit test"""
+
+        gT1 = Pose2(1, 2, np.pi / 2)
+        gT2 = Pose2(-1, 4, np.pi)
+
+        expected = Pose2(2, 2, np.pi / 2)
+
+        def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
+            """
+            the custom error function. One can freely use variables captured
+            from the outside scope. Or the variables can be acquired by indexing `v`.
+            Jacobian is passed by modifying the H array of numpy matrices.
+            """
+
+            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 = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
+        v = Values()
+        v.insert(0, gT1)
+        v.insert(1, gT2)
+
+        bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
+
+        gf = cf.linearize(v)
+        gf_b = bf.linearize(v)
+
+        J_cf, b_cf = gf.jacobian()
+        J_bf, b_bf = gf_b.jacobian()
+        np.testing.assert_allclose(J_cf, J_bf)
+        np.testing.assert_allclose(b_cf, b_bf)
+
+    def test_printing(self):
+        """Tests if the factor result matches the GTSAM Pose2 unit test"""
+        gT1 = Pose2(1, 2, np.pi / 2)
+        gT2 = Pose2(-1, 4, np.pi)
+
+        def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]):
+            """Minimal error function stub"""
+            return np.array([1, 0, 0])
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        from gtsam.symbol_shorthand import X
+        cf = CustomFactor(noise_model, [X(0), X(1)], error_func)
+
+        cf_string = """CustomFactor on x0, x1
+  noise model: unit (3) 
+"""
+        self.assertEqual(cf_string, repr(cf))
+
+    def test_no_jacobian(self):
+        """Tests that we will not calculate the Jacobian if not requested"""
+
+        gT1 = Pose2(1, 2, np.pi / 2)
+        gT2 = Pose2(-1, 4, np.pi)
+
+        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))
+
+            self.assertTrue(H is None)  # Should be true if we only request the error
+
+            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 = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)
+        v = Values()
+        v.insert(0, gT1)
+        v.insert(1, gT2)
+
+        bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model)
+
+        e_cf = cf.error(v)
+        e_bf = bf.error(v)
+        np.testing.assert_allclose(e_cf, e_bf)
+
+    def test_optimization(self):
+        """Tests if a factor graph with a CustomFactor can be properly optimized"""
+        gT1 = Pose2(1, 2, np.pi / 2)
+        gT2 = Pose2(-1, 4, np.pi)
+
+        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 = CustomFactor(noise_model, [0, 1], error_func)
+
+        fg = gtsam.NonlinearFactorGraph()
+        fg.add(cf)
+        fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
+
+        v = Values()
+        v.insert(0, Pose2(0, 0, 0))
+        v.insert(1, Pose2(0, 0, 0))
+
+        params = gtsam.LevenbergMarquardtParams()
+        optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params)
+        result = optimizer.optimize()
+
+        self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5)
+        self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
+
+
+if __name__ == "__main__":
+    unittest.main()