Change to using nullptr

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

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