diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index c4c601f2e..fa0a5c499 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2168,7 +2168,10 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor { #include virtual class CustomFactor: gtsam::NoiseModelFactor { - // Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there. + /* + * 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: diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index f9f7be7b0..a6d6f1f7b 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -42,17 +42,34 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optionalerrorFunction(*this, x, H.get_ptr()); + return this->error_function_(*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); + 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; +} + } diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 41de338f3..7ee5f1f77 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -45,13 +45,13 @@ using CustomErrorFunction = std::functionerrorFunction = errorFunction; + this->error_function_ = errorFunction; } ~CustomFactor() override = default; @@ -81,22 +81,7 @@ public: /** print */ void print(const std::string& s, - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - 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; - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; private: diff --git a/python/FACTORS.md b/python/CustomFactors.md similarity index 69% rename from python/FACTORS.md rename to python/CustomFactors.md index 36fd5f8f1..abba9e00b 100644 --- a/python/FACTORS.md +++ b/python/CustomFactors.md @@ -2,11 +2,6 @@ One now can build factors purely in Python using the `CustomFactor` factor. -## Theory - -`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. - ## Usage In order to use a Python-based factor, one needs to have a Python function with the following signature: @@ -76,3 +71,40 @@ In general, the Python-based factor works just like their C++ counterparts. 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; +``` + +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. diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index e1ebfcd69..d57496537 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -23,6 +23,7 @@ class TestCustomFactor(GtsamTestCase): """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) @@ -32,6 +33,7 @@ class TestCustomFactor(GtsamTestCase): """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) @@ -42,6 +44,7 @@ class TestCustomFactor(GtsamTestCase): 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 @@ -102,11 +105,8 @@ class TestCustomFactor(GtsamTestCase): gT2 = Pose2(-1, 4, np.pi) def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): - 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)) - return error + """Minimal error function stub""" + return np.array([1, 0, 0]) noise_model = gtsam.noiseModel.Unit.Create(3) from gtsam.symbol_shorthand import X @@ -126,6 +126,13 @@ class TestCustomFactor(GtsamTestCase): 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)