Custom Factors in Python

gtsam/gtsam.i

@@ -2166,6 +2166,28 @@ 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.
+  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);
+};
+
 #include <gtsam/nonlinear/Values.h>
 class Values {
   Values();

gtsam/nonlinear/CustomFactor.cpp

@@ -0,0 +1,58 @@
+/* ----------------------------------------------------------------------------
+
+ * 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->errorFunction(*this, x, H.get_ptr());
+    } else {
+      /*
+       * In this case, we pass the a `nullptr` to pybind, and it will translated to `None` in Python.
+       * Users can check for `None` in their callback to determine if the Jacobian is requested.
+       */
+      return this->errorFunction(*this, x, nullptr);
+    }
+  } else {
+    return Vector::Zero(this->dim());
+  }
+}
+
+}

gtsam/nonlinear/CustomFactor.h

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 {
+
+typedef std::vector<Matrix> JacobianVector;
+
+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.
+ */
+typedef std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)> CustomErrorFunction;
+
+/**
+ * @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 {
+public:
+   CustomErrorFunction errorFunction;
+
+protected:
+
+  typedef NoiseModelFactor Base;
+  typedef CustomFactor This;
+
+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->errorFunction = 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;
+
+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));
+  }
+};
+
+};

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/gtsam/preamble.h

@@ -6,9 +6,17 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
 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

@@ -2,9 +2,12 @@
 // These are required to save one copy operation on Python calls
 #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 +20,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,125 @@
+"""
+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]):
+            return np.array([1, 0, 0])
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        cf = CustomFactor(noise_model, gtsam.KeyVector([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:
+            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 = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
+
+            if not H is 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, Pose2(0, 0, 0), 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_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]):
+            # print(f"{this = },\n{v = },\n{len(H) = }")
+
+            key0 = this.keys()[0]
+            key1 = this.keys()[1]
+            gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
+            error = Pose2(0, 0, 0).localCoordinates(gT1.between(gT2))
+
+            self.assertTrue(H is None) # Should be true if we only request the error
+
+            if not H is 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, Pose2(0, 0, 0), noise_model)
+
+        e_cf = cf.error(v)
+        e_bf = bf.error(v)
+        np.testing.assert_allclose(e_cf, e_bf)
+
+if __name__ == "__main__":
+    unittest.main()