Custom Factors in Python

gtsam/gtsam.i

@@ -2166,6 +2166,12 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor {
   Vector whitenedError(const gtsam::Values& x) const;
 };
 
+#include <gtsam/nonlinear/CustomFactor.h>
+virtual class CustomFactor: gtsam::NoiseModelFactor {
+  CustomFactor();
+  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,35 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 {
+
+Vector CustomFactor::unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H) const {
+  if(this->active(x)) {
+    if(H) {
+      return this->errorFunction(*this, x, H.get_ptr());
+    } else {
+      JacobianVector dummy;
+      return this->errorFunction(*this, x, &dummy);
+    }
+  } else {
+    return Vector::Zero(this->dim());
+  }
+}
+
+}

gtsam/nonlinear/CustomFactor.h

@@ -0,0 +1,84 @@
+/* ----------------------------------------------------------------------------
+
+ * 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;
+
+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

@@ -17,3 +17,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,86 @@
+"""
+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):
+        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):
+
+        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, gtsam.KeyVector([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]):
+            # 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))
+
+            if len(H) > 0:
+                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)
+
+if __name__ == "__main__":
+    unittest.main()