From 5d1fd83a2c8786abda9af53764b096227d9e01ed Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 19:19:20 -0400 Subject: [PATCH] Add printing for CustomFactor --- gtsam/gtsam.i | 5 +- gtsam/nonlinear/CustomFactor.h | 31 ++++++++++-- python/gtsam/tests/test_custom_factor.py | 61 ++++++++++++++++++------ 3 files changed, 77 insertions(+), 20 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 077285bb0..c4c601f2e 100644 --- a/gtsam/gtsam.i +++ b/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 diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 34cb5ad51..41de338f3 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -23,7 +23,7 @@ using namespace gtsam; namespace gtsam { -typedef std::vector JacobianVector; +using JacobianVector = std::vector; 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 CustomErrorFunction; +using CustomErrorFunction = std::function; /** * @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&> H = boost::none) const override; + */ + Vector unwhitenedError(const Values &x, boost::optional &> 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: diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index 32ae50590..b41eec2ec 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/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()