diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index 0f2c542bc..f9f7be7b0 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -19,13 +19,36 @@ namespace gtsam { +/* + * Calculates the unwhitened error by invoking the callback functor (i.e. from Python). + */ Vector CustomFactor::unwhitenedError(const Values& x, boost::optional&> H) const { if(this->active(x)) { if(H) { + /* + * In this case, we pass the raw pointer to the `std::vector` object directly to pybind. + * As the type `std::vector` 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]): + * + * if not H is None: + * + * H[0] = J1 + * H[1] = J2 + * ... + * return error + * ``` + */ return this->errorFunction(*this, x, H.get_ptr()); } else { - JacobianVector dummy; - return this->errorFunction(*this, x, &dummy); + /* + * 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()); diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index 2ad7b929d..e8f06b27a 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -17,10 +17,9 @@ 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]) @@ -28,7 +27,7 @@ class TestCustomFactor(GtsamTestCase): 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: @@ -53,14 +52,18 @@ class TestCustomFactor(GtsamTestCase): 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) = }") + """ + 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 len(H) > 0: + if not H is None: result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) @@ -82,5 +85,41 @@ class TestCustomFactor(GtsamTestCase): 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()