Add printing for CustomFactor

gtsam/gtsam.i

@@ -2185,7 +2185,10 @@ virtual class CustomFactor: gtsam::NoiseModelFactor {
    * cf = CustomFactor(noise_model, keys, error_func)
    * ```
    */
-  CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction);
+  CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys,
+               const gtsam::CustomErrorFunction& errorFunction);
+
+  void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
 };
 
 #include <gtsam/nonlinear/Values.h>

gtsam/nonlinear/CustomFactor.h

@@ -23,7 +23,7 @@ using namespace gtsam;
 
 namespace gtsam {
 
-typedef std::vector<Matrix> JacobianVector;
+using JacobianVector = std::vector<Matrix>;
 
 class CustomFactor;
 
@@ -35,7 +35,7 @@ class CustomFactor;
  * 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;
+using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;
 
 /**
  * @brief Custom factor that takes a std::function as the error
@@ -73,10 +73,31 @@ public:
 
   ~CustomFactor() override = default;
 
-  /** Calls the errorFunction closure, which is a std::function object
+  /**
+    * 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;
+  Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
+
+  /** 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;
+  }
+
 
 private:
 

python/gtsam/tests/test_custom_factor.py

@@ -17,15 +17,26 @@ 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_new_keylist(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, [0], error_func)
+
     def test_call(self):
         """Test if calling the factor works (only error)"""
         expected_pose = Pose2(1, 1, 0)
@@ -34,22 +45,22 @@ class TestCustomFactor(GtsamTestCase):
             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)
+        gT1 = Pose2(1, 2, np.pi / 2)
         gT2 = Pose2(-1, 4, np.pi)
 
-        expected = Pose2(2, 2, np.pi/2)
+        expected = Pose2(2, 2, np.pi / 2)
 
         def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
             """
@@ -62,19 +73,19 @@ class TestCustomFactor(GtsamTestCase):
             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:
+            if H is not 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)
@@ -85,13 +96,34 @@ class TestCustomFactor(GtsamTestCase):
         np.testing.assert_allclose(J_cf, J_bf)
         np.testing.assert_allclose(b_cf, b_bf)
 
+    def test_printing(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)
+
+        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
+
+        noise_model = gtsam.noiseModel.Unit.Create(3)
+        from gtsam.symbol_shorthand import X
+        cf = CustomFactor(noise_model, [X(0), X(1)], error_func)
+
+        cf_string = """CustomFactor on x0, x1
+  noise model: unit (3) 
+"""
+        self.assertEqual(cf_string, repr(cf))
+
     def test_no_jacobian(self):
         """Tests that we will not calculate the Jacobian if not requested"""
 
-        gT1 = Pose2(1, 2, np.pi/2)
+        gT1 = Pose2(1, 2, np.pi / 2)
         gT2 = Pose2(-1, 4, np.pi)
 
-        expected = Pose2(2, 2, np.pi/2)
+        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) = }")
@@ -101,9 +133,9 @@ class TestCustomFactor(GtsamTestCase):
             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
+            self.assertTrue(H is None)  # Should be true if we only request the error
 
-            if not H is None:
+            if H is not None:
                 result = gT1.between(gT2)
                 H[0] = -result.inverse().AdjointMap()
                 H[1] = np.eye(3)
@@ -121,5 +153,6 @@ class TestCustomFactor(GtsamTestCase):
         e_bf = bf.error(v)
         np.testing.assert_allclose(e_cf, e_bf)
 
+
 if __name__ == "__main__":
     unittest.main()