From 866d6b1fa1a68a8f4afdb084178fda07493d914d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 14 May 2021 16:24:31 -0400 Subject: [PATCH 01/15] Working CustomFactor --- gtsam/gtsam.i | 6 ++ gtsam/nonlinear/CustomFactor.cpp | 35 ++++++++++ gtsam/nonlinear/CustomFactor.h | 84 +++++++++++++++++++++++ matlab/CMakeLists.txt | 3 +- python/gtsam/preamble.h | 8 +++ python/gtsam/specializations.h | 1 + python/gtsam/tests/test_custom_factor.py | 86 ++++++++++++++++++++++++ 7 files changed, 222 insertions(+), 1 deletion(-) create mode 100644 gtsam/nonlinear/CustomFactor.cpp create mode 100644 gtsam/nonlinear/CustomFactor.h create mode 100644 python/gtsam/tests/test_custom_factor.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d053c84222..295d5237f6 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2166,6 +2166,12 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor { Vector whitenedError(const gtsam::Values& x) const; }; +#include +virtual class CustomFactor: gtsam::NoiseModelFactor { + CustomFactor(); + CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction); +}; + #include class Values { Values(); diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp new file mode 100644 index 0000000000..0f2c542bcf --- /dev/null +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CustomFactor.cpp + * @brief Class to enable arbitrary factors with runtime swappable error function. + * @author Fan Jiang + */ + +#include + +namespace gtsam { + +Vector CustomFactor::unwhitenedError(const Values& x, boost::optional&> H) const { + if(this->active(x)) { + if(H) { + return this->errorFunction(*this, x, H.get_ptr()); + } else { + JacobianVector dummy; + return this->errorFunction(*this, x, &dummy); + } + } else { + return Vector::Zero(this->dim()); + } +} + +} diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h new file mode 100644 index 0000000000..9d9db9eda5 --- /dev/null +++ b/gtsam/nonlinear/CustomFactor.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CustomFactor.h + * @brief Class to enable arbitrary factors with runtime swappable error function. + * @author Fan Jiang + */ + +#pragma once + +#include + +using namespace gtsam; + +namespace gtsam { + +typedef std::vector JacobianVector; + +class CustomFactor; + +typedef std::function CustomErrorFunction; + +/** + * @brief Custom factor that takes a std::function as the error + * @addtogroup nonlinear + * \nosubgrouping + * + * This factor is mainly for creating a custom factor in Python. + */ +class CustomFactor: public NoiseModelFactor { +public: + CustomErrorFunction errorFunction; + +protected: + + typedef NoiseModelFactor Base; + typedef CustomFactor This; + +public: + + /** + * Default Constructor for I/O + */ + CustomFactor() = default; + + /** + * 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->errorFunction = errorFunction; + } + + ~CustomFactor() override = default; + + /** 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; + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("CustomFactor", + boost::serialization::base_object(*this)); + } +}; + +}; diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 3c234d106d..28e7cce6e4 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -61,7 +61,8 @@ endif() # ignoring the non-concrete types (type aliases) set(ignore gtsam::Point2 - gtsam::Point3) + gtsam::Point3 + gtsam::CustomFactor) # Wrap matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}" diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index b56766c723..dd1b342eb7 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -6,9 +6,17 @@ PYBIND11_MAKE_OPAQUE(std::vector>); PYBIND11_MAKE_OPAQUE(std::vector); #endif PYBIND11_MAKE_OPAQUE(std::vector >); +PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); +PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector + +// TODO(fan): This is to fix the Argument-dependent lookup (ADL) of std::pair. We should find a way to NOT do this. +namespace std { + using gtsam::operator<<; +} diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 98143160ee..c7f45fc0f9 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -17,3 +17,4 @@ py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector > >(m_, "CameraSetCal3_S2"); py::bind_vector > >(m_, "CameraSetCal3Bundler"); +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py new file mode 100644 index 0000000000..2ad7b929d4 --- /dev/null +++ b/python/gtsam/tests/test_custom_factor.py @@ -0,0 +1,86 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +CustomFactor unit tests. +Author: Fan Jiang +""" +from typing import List +import unittest +from gtsam import Values, Pose2, CustomFactor + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestCustomFactor(GtsamTestCase): + + def test_new(self): + 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_call(self): + + expected_pose = Pose2(1, 1, 0) + + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: + key0 = this.keys()[0] + error = -v.atPose2(key0).localCoordinates(expected_pose) + return error + + noise_model = gtsam.noiseModel.Unit.Create(3) + cf = CustomFactor(noise_model, gtsam.KeyVector([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) + 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)) + + if len(H) > 0: + 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) + gf_b = bf.linearize(v) + + J_cf, b_cf = gf.jacobian() + J_bf, b_bf = gf_b.jacobian() + np.testing.assert_allclose(J_cf, J_bf) + np.testing.assert_allclose(b_cf, b_bf) + +if __name__ == "__main__": + unittest.main() From 3638b3745fb3cc831d81ffa7465bce31f115c362 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 14:22:57 -0400 Subject: [PATCH 02/15] Change to using nullptr --- gtsam/nonlinear/CustomFactor.cpp | 27 ++++++++++++- python/gtsam/tests/test_custom_factor.py | 49 +++++++++++++++++++++--- 2 files changed, 69 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index 0f2c542bcf..f9f7be7b00 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 2ad7b929d4..e8f06b27aa 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -17,10 +17,9 @@ 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): 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 @@ def test_jacobian(self): 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): 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() From 66e397cb38c97443520fa8c1b5a70dd97383fa7a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 14:36:14 -0400 Subject: [PATCH 03/15] Allow KeyVector to just be lists --- gtsam/gtsam.i | 1 + python/gtsam/specializations.h | 3 +++ python/gtsam/tests/test_custom_factor.py | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 295d5237f6..b962724b9d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2168,6 +2168,7 @@ 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. CustomFactor(); CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction); }; diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index c7f45fc0f9..c6efd2f710 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -2,9 +2,12 @@ // These are required to save one copy operation on Python calls #ifdef GTSAM_ALLOCATOR_TBB py::bind_vector > >(m_, "KeyVector"); +py::implicitly_convertible > >(); #else py::bind_vector >(m_, "KeyVector"); +py::implicitly_convertible >(); #endif + py::bind_vector > >(m_, "Point2Vector"); py::bind_vector >(m_, "Point3Pairs"); py::bind_vector >(m_, "Pose3Pairs"); diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index e8f06b27aa..32ae505905 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -36,7 +36,7 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.n return error noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) + cf = CustomFactor(noise_model, [0], error_func) v = Values() v.insert(0, Pose2(1, 0, 0)) e = cf.error(v) From 4708d7ad0e502e62cc88c38f1c1049c52dad1c35 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 14:40:17 -0400 Subject: [PATCH 04/15] Add comment on functor signature --- gtsam/nonlinear/CustomFactor.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 9d9db9eda5..34cb5ad518 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -27,6 +27,14 @@ typedef std::vector JacobianVector; class CustomFactor; +/* + * 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. + */ typedef std::function CustomErrorFunction; /** From 3c327ff568127603678d2339550979b8ef795678 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 14:43:32 -0400 Subject: [PATCH 05/15] Add comment in gtsam.i --- gtsam/gtsam.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b962724b9d..077285bb0d 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2170,6 +2170,21 @@ virtual class NoiseModelFactor: gtsam::NonlinearFactor { virtual class CustomFactor: gtsam::NoiseModelFactor { // Note CustomFactor will not be wrapped for MATLAB, as there is no supporting machinery there. CustomFactor(); + /* + * Example: + * ``` + * def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + * + * if not H is None: + * + * H[0] = J1 # 2-d numpy array for a Jacobian block + * H[1] = J2 + * ... + * return error # 1-d numpy array + * + * cf = CustomFactor(noise_model, keys, error_func) + * ``` + */ CustomFactor(const gtsam::SharedNoiseModel& noiseModel, const gtsam::KeyVector& keys, const gtsam::CustomErrorFunction& errorFunction); }; From 5d1fd83a2c8786abda9af53764b096227d9e01ed Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 19:19:20 -0400 Subject: [PATCH 06/15] 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 077285bb0d..c4c601f2e6 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 34cb5ad518..41de338f31 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 @@ class CustomFactor: public NoiseModelFactor { ~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 32ae505905..b41eec2ec0 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -17,15 +17,26 @@ 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.n 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): 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 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): e_bf = bf.error(v) np.testing.assert_allclose(e_cf, e_bf) + if __name__ == "__main__": unittest.main() From 615a932f300897e2614649a6fda69b2497f9c942 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 May 2021 20:11:17 -0400 Subject: [PATCH 07/15] Remove unnecessary comment --- python/gtsam/tests/test_custom_factor.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index b41eec2ec0..e1ebfcd69c 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -126,8 +126,6 @@ def test_no_jacobian(self): 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) From a8ed71abbcdfde30e706905500a878c934c8b148 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 21 May 2021 13:38:03 -0400 Subject: [PATCH 08/15] Add documentation --- python/FACTORS.md | 78 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 python/FACTORS.md diff --git a/python/FACTORS.md b/python/FACTORS.md new file mode 100644 index 0000000000..36fd5f8f14 --- /dev/null +++ b/python/FACTORS.md @@ -0,0 +1,78 @@ +# GTSAM Python-based factors + +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: + +```python +import gtsam +import numpy as np +from typing import List + +def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + ... +``` + +`this` is a reference to the `CustomFactor` object. This is required because one can reuse the same +`error_func` for multiple factors. `v` is a reference to the current set of values, and `H` is a list of +**references** to the list of required Jacobians (see the corresponding C++ documentation). + +If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error` +method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`, +each entry of `H` can be assigned a `numpy` array, as the Jacobian for the corresponding variable. + +After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM: + +```python +noise_model = gtsam.noiseModel.Unit.Create(3) +# constructor(, , ) +cf = gtsam.CustomFactor(noise_model, [X(0), X(1)], error_func) +``` + +## Example + +The following is a simple `BetweenFactor` implemented in Python. + +```python +import gtsam +import numpy as np +from typing import List + +def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + # Get the variable values from `v` + key0 = this.keys()[0] + key1 = this.keys()[1] + + # Calculate non-linear error + gT1, gT2 = v.atPose2(key0), v.atPose2(key1) + error = gtsam.Pose2(0, 0, 0).localCoordinates(gT1.between(gT2)) + + # If we need Jacobian + if H is not None: + # Fill the Jacobian arrays + # Note we have two vars, so two entries + result = gT1.between(gT2) + H[0] = -result.inverse().AdjointMap() + H[1] = np.eye(3) + + # Return the error + return error + +noise_model = gtsam.noiseModel.Unit.Create(3) +cf = gtsam.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) +``` + +In general, the Python-based factor works just like their C++ counterparts. + +## Known Issues + +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. From 7de3714d54b5bb22cdb47846aabbdb67be7379b3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 21 May 2021 16:11:53 -0400 Subject: [PATCH 09/15] Address Frank's comments --- gtsam/gtsam.i | 5 ++- gtsam/nonlinear/CustomFactor.cpp | 21 ++++++++++-- gtsam/nonlinear/CustomFactor.h | 27 ++++----------- python/{FACTORS.md => CustomFactors.md} | 42 +++++++++++++++++++++--- python/gtsam/tests/test_custom_factor.py | 17 +++++++--- 5 files changed, 78 insertions(+), 34 deletions(-) rename python/{FACTORS.md => CustomFactors.md} (69%) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index c4c601f2e6..fa0a5c499a 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 f9f7be7b00..a6d6f1f7bd 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 41de338f31..7ee5f1f77c 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 @@ class CustomFactor: public NoiseModelFactor { /** 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 36fd5f8f14..abba9e00bc 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 e1ebfcd69c..d574965378 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -23,6 +23,7 @@ def test_new(self): """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 @@ def test_new_keylist(self): """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 @@ def test_call(self): 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 @@ def test_printing(self): 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 @@ def test_no_jacobian(self): 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) From 0e44261b1ec9108ab13eccbc2f59ae13e32dbef4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 21 May 2021 19:27:46 -0400 Subject: [PATCH 10/15] Add more comments --- python/gtsam/preamble.h | 12 ++++++++++-- python/gtsam/specializations.h | 16 ++++++++++++++-- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index dd1b342eb7..7294a6ac8e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -1,5 +1,13 @@ -// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html -// These are required to save one copy operation on Python calls +/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, + * such that the raw objects can be accessed in Python. Without this they will be automatically + * converted to a Python object, and all mutations on Python side will not be reflected on C++. + */ #ifdef GTSAM_ALLOCATOR_TBB PYBIND11_MAKE_OPAQUE(std::vector>); #else diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index c6efd2f710..be8eb8a6cf 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -1,5 +1,17 @@ -// Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html -// These are required to save one copy operation on Python calls +/* Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 automatic STL binding, + * such that the raw objects can be accessed in Python. Without this they will be automatically + * converted to a Python object, and all mutations on Python side will not be reflected on C++. + * + * `py::bind_vector` and similar machinery gives the std container a Python-like interface, but + * without the `` copying mechanism. Combined with `PYBIND11_MAKE_OPAQUE` this + * allows the types to be modified with Python, and saves one copy operation. + */ #ifdef GTSAM_ALLOCATOR_TBB py::bind_vector > >(m_, "KeyVector"); py::implicitly_convertible > >(); From 880d5b57af73e3d6d59eacb89d2284cfc20dd955 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 5 Jun 2021 00:18:45 -0400 Subject: [PATCH 11/15] Fixed Python factor for TBB --- gtsam/nonlinear/CustomFactor.cpp | 3 +- gtsam/nonlinear/CustomFactor.h | 18 +++-- gtsam/nonlinear/NonlinearFactor.h | 10 ++- gtsam/nonlinear/NonlinearFactorGraph.cpp | 12 +++- python/gtsam/examples/CustomFactorExample.py | 75 ++++++++++++++++++++ 5 files changed, 109 insertions(+), 9 deletions(-) create mode 100644 python/gtsam/examples/CustomFactorExample.py diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index a6d6f1f7bd..e33caed6fc 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -24,6 +24,7 @@ namespace gtsam { */ 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. @@ -45,7 +46,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optionalerror_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. + * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. * Users can check for `None` in their callback to determine if the Jacobian is requested. */ return this->error_function_(*this, x, nullptr); diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 7ee5f1f77c..dbaf318989 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -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. */ -using CustomErrorFunction = std::function; +using CustomErrorFunction = std::function; /** * @brief Custom factor that takes a std::function as the error @@ -46,7 +46,7 @@ using CustomErrorFunction = std::functionerror_function_ = errorFunction; } @@ -80,16 +80,22 @@ class CustomFactor: public NoiseModelFactor { 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; + void print(const std::string &s, + const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override; + /** + * Mark not sendable + */ + bool sendable() const override { + return false; + } private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { ar & boost::serialization::make_nvp("CustomFactor", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 21c05dc2c4..5c61bf7dcb 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -95,7 +95,7 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { /** * Checks whether a factor should be used based on a set of values. - * This is primarily used to implment inequality constraints that + * This is primarily used to implement inequality constraints that * require a variable active set. For all others, the default implementation * returning true solves this problem. * @@ -134,6 +134,14 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { */ shared_ptr rekey(const KeyVector& new_keys) const; + /** + * Should the factor be evaluated in the same thread as the caller + * This is to enable factors that has shared states (like the Python GIL lock) + */ + virtual bool sendable() const { + return true; + } + }; // \class NonlinearFactor /// traits diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 6a9e0cd0a4..42fe5ae57d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -325,7 +325,7 @@ class _LinearizeOneFactor { // Operator that linearizes a given range of the factors void operator()(const tbb::blocked_range& blocked_range) const { for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) { - if (nonlinearGraph_[i]) + if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable()) result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_); else result_[i] = GaussianFactor::shared_ptr(); @@ -348,9 +348,19 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li linearFG->resize(size()); TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + + // First linearize all sendable factors tbb::parallel_for(tbb::blocked_range(0, size()), _LinearizeOneFactor(*this, linearizationPoint, *linearFG)); + // Linearize all non-sendable factors + for(int i = 0; i < size(); i++) { + auto& factor = (*this)[i]; + if(factor && !(factor->sendable())) { + (*linearFG)[i] = factor->linearize(linearizationPoint); + } + } + #else linearFG->reserve(size()); diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py new file mode 100644 index 0000000000..562951ae53 --- /dev/null +++ b/python/gtsam/examples/CustomFactorExample.py @@ -0,0 +1,75 @@ +import gtsam +import numpy as np + +from typing import List, Optional +from functools import partial + +# Simulate a car for one second +x0 = 0 +dt = 0.25 # 4 Hz, typical GPS +v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast +x = [x0 + v * dt * i for i in range(5)] +print(x) + +# %% +add_noise = True # set this to False to run with "perfect" measurements + +# GPS measurements +sigma_gps = 3.0 # assume GPS is +/- 3m +g = [x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0) + for k in range(5)] + +# Odometry measurements +sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz +o = [x[k + 1] - x[k] + (np.random.normal(scale=sigma_odo) if add_noise else 0) + for k in range(4)] + +# Landmark measurements: +sigma_lm = 1 # assume landmark measurement is accurate up to 1m + +# Assume first landmark is at x=5, we measure it at time k=0 +lm_0 = 5.0 +z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + +# Assume other landmark is at x=28, we measure it at time k=3 +lm_3 = 28.0 +z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0) + +unknown = [gtsam.symbol('x', k) for k in range(5)] + +print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown))) + +# We now can use nonlinear factor graphs +factor_graph = gtsam.NonlinearFactorGraph() + +# Add factors for GPS measurements +I = np.eye(1) +gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) + + +def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): + key = this.keys()[0] + estimate = values.atVector(key) + error = measurement - estimate + if jacobians is not None: + jacobians[0] = -I + + return error + + +for k in range(5): + factor_graph.add(gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, measurement=np.array([g[k]])))) + +v = gtsam.Values() + +for i in range(5): + v.insert(unknown[i], np.array([0.0])) + +linearized: gtsam.GaussianFactorGraph = factor_graph.linearize(v) +print(linearized.at(1)) + +params = gtsam.GaussNewtonParams() +optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + +result = optimizer.optimize() +print(result) From 22ddab79215867fae216343502c0001d9b28960f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 5 Jun 2021 00:37:55 -0400 Subject: [PATCH 12/15] Trajectory Estimation example --- python/gtsam/examples/CustomFactorExample.py | 89 ++++++++++++++++++-- 1 file changed, 84 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index 562951ae53..24766d3dff 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -48,11 +48,18 @@ def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): + """GPS Factor error function + :param this: gtsam.CustomFactor handle + :param values: gtsam.Values + :param jacobians: Optional list of Jacobians + :param measurement: GPS measurement, to be filled with `partial` + :return: the unwhitened error + """ key = this.keys()[0] estimate = values.atVector(key) - error = measurement - estimate + error = estimate - measurement if jacobians is not None: - jacobians[0] = -I + jacobians[0] = I return error @@ -65,11 +72,83 @@ def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndar for i in range(5): v.insert(unknown[i], np.array([0.0])) -linearized: gtsam.GaussianFactorGraph = factor_graph.linearize(v) -print(linearized.at(1)) +params = gtsam.GaussNewtonParams() +optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + +result = optimizer.optimize() + +error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + +print("Result with only GPS") +print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") + +# Adding odometry will improve things a lot +odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) + + +def error_odom(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): + """Odometry Factor error function + :param this: gtsam.CustomFactor handle + :param values: gtsam.Values + :param jacobians: Optional list of Jacobians + :param measurement: Odometry measurement, to be filled with `partial` + :return: the unwhitened error + """ + key1 = this.keys()[0] + key2 = this.keys()[1] + pos1, pos2 = values.atVector(key1), values.atVector(key2) + error = measurement - (pos1 - pos2) + if jacobians is not None: + jacobians[0] = I + jacobians[1] = -I + + return error + + +for k in range(4): + factor_graph.add( + gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, measurement=np.array([o[k]])))) + +params = gtsam.GaussNewtonParams() +optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) + +result = optimizer.optimize() + +error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + +print("Result with GPS+Odometry") +print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") + +# This is great, but GPS noise is still apparent, so now we add the two landmarks +lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) + + +def error_lm(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): + """Landmark Factor error function + :param this: gtsam.CustomFactor handle + :param values: gtsam.Values + :param jacobians: Optional list of Jacobians + :param measurement: Landmark measurement, to be filled with `partial` + :return: the unwhitened error + """ + key = this.keys()[0] + pos = values.atVector(key) + error = pos - measurement + if jacobians is not None: + jacobians[0] = I + + return error + + +factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, measurement=np.array([lm_0 + z_0])))) +factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, measurement=np.array([lm_3 + z_3])))) params = gtsam.GaussNewtonParams() optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) result = optimizer.optimize() -print(result) + +error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) + +print("Result with GPS+Odometry+Landmark") +print(result, np.round(error, 2), f"\nJ(X)={0.5 * np.sum(np.square(error))}") From 56faf3c4a8e4d9d4e37531bcc5b42cc9cd927e79 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 5 Jun 2021 01:18:02 -0400 Subject: [PATCH 13/15] Add unit test for optimization a factor graph --- python/gtsam/tests/test_custom_factor.py | 52 ++++++++++++++++++++++-- 1 file changed, 48 insertions(+), 4 deletions(-) diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index d574965378..4f0f333619 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -75,7 +75,7 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: 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)) + error = expected.localCoordinates(gT1.between(gT2)) if H is not None: result = gT1.between(gT2) @@ -89,7 +89,7 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): v.insert(0, gT1) v.insert(1, gT2) - bf = gtsam.BetweenFactorPose2(0, 1, Pose2(0, 0, 0), noise_model) + bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model) gf = cf.linearize(v) gf_b = bf.linearize(v) @@ -136,7 +136,7 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: 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)) + error = expected.localCoordinates(gT1.between(gT2)) self.assertTrue(H is None) # Should be true if we only request the error @@ -152,12 +152,56 @@ def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): v.insert(0, gT1) v.insert(1, gT2) - bf = gtsam.BetweenFactorPose2(0, 1, Pose2(0, 0, 0), noise_model) + bf = gtsam.BetweenFactorPose2(0, 1, expected, noise_model) e_cf = cf.error(v) e_bf = bf.error(v) np.testing.assert_allclose(e_cf, e_bf) + def test_optimization(self): + """Tests if a factor graph with a CustomFactor can be properly optimized""" + 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]): + """ + 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) + error = expected.localCoordinates(gT1.between(gT2)) + + 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, [0, 1], error_func) + + fg = gtsam.NonlinearFactorGraph() + fg.add(cf) + fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model)) + + v = Values() + v.insert(0, Pose2(0, 0, 0)) + v.insert(1, Pose2(0, 0, 0)) + + params = gtsam.LevenbergMarquardtParams() + optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params) + result = optimizer.optimize() + + self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5) + self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5) + if __name__ == "__main__": unittest.main() From 93ebc9d5e9cf5abb962b88b7cf9e8cec441f9c89 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 5 Jun 2021 19:32:00 -0400 Subject: [PATCH 14/15] Address Frank's comments --- python/gtsam/examples/CustomFactorExample.py | 59 ++++++++++++++------ 1 file changed, 42 insertions(+), 17 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index 24766d3dff..c7fe1e202c 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -1,15 +1,33 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +CustomFactor demo that simulates a 1-D sensor fusion task. +Author: Fan Jiang, Frank Dellaert +""" + import gtsam import numpy as np from typing import List, Optional from functools import partial -# Simulate a car for one second -x0 = 0 -dt = 0.25 # 4 Hz, typical GPS -v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast -x = [x0 + v * dt * i for i in range(5)] -print(x) + +def simulate_car(): + # Simulate a car for one second + x0 = 0 + dt = 0.25 # 4 Hz, typical GPS + v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast + x = [x0 + v * dt * i for i in range(5)] + + return x + + +x = simulate_car() +print(f"Simulated car trajectory: {x}") # %% add_noise = True # set this to False to run with "perfect" measurements @@ -47,12 +65,12 @@ gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps) -def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): +def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): """GPS Factor error function + :param measurement: GPS measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle :param values: gtsam.Values :param jacobians: Optional list of Jacobians - :param measurement: GPS measurement, to be filled with `partial` :return: the unwhitened error """ key = this.keys()[0] @@ -64,19 +82,26 @@ def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndar return error +# Add the GPS factors for k in range(5): - factor_graph.add(gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, measurement=np.array([g[k]])))) + gf = gtsam.CustomFactor(gps_model, [unknown[k]], partial(error_gps, np.array([g[k]]))) + factor_graph.add(gf) +# New Values container v = gtsam.Values() +# Add initial estimates to the Values container for i in range(5): v.insert(unknown[i], np.array([0.0])) +# Initialize optimizer params = gtsam.GaussNewtonParams() optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) +# Optimize the factor graph result = optimizer.optimize() +# calculate the error from ground truth error = np.array([(result.atVector(unknown[k]) - x[k])[0] for k in range(5)]) print("Result with only GPS") @@ -86,12 +111,12 @@ def error_gps(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndar odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo) -def error_odom(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): +def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): """Odometry Factor error function + :param measurement: Odometry measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle :param values: gtsam.Values :param jacobians: Optional list of Jacobians - :param measurement: Odometry measurement, to be filled with `partial` :return: the unwhitened error """ key1 = this.keys()[0] @@ -106,8 +131,8 @@ def error_odom(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.nda for k in range(4): - factor_graph.add( - gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, measurement=np.array([o[k]])))) + odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]], partial(error_odom, np.array([o[k]]))) + factor_graph.add(odof) params = gtsam.GaussNewtonParams() optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) @@ -123,12 +148,12 @@ def error_odom(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.nda lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm) -def error_lm(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]], measurement: np.ndarray): +def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarray]]): """Landmark Factor error function + :param measurement: Landmark measurement, to be filled with `partial` :param this: gtsam.CustomFactor handle :param values: gtsam.Values :param jacobians: Optional list of Jacobians - :param measurement: Landmark measurement, to be filled with `partial` :return: the unwhitened error """ key = this.keys()[0] @@ -140,8 +165,8 @@ def error_lm(this: gtsam.CustomFactor, values, jacobians: Optional[List[np.ndarr return error -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, measurement=np.array([lm_0 + z_0])))) -factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, measurement=np.array([lm_3 + z_3])))) +factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[0]], partial(error_lm, np.array([lm_0 + z_0])))) +factor_graph.add(gtsam.CustomFactor(lm_model, [unknown[3]], partial(error_lm, np.array([lm_3 + z_3])))) params = gtsam.GaussNewtonParams() optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params) From 1ebf67520154603279ea05f9ebcbb20326670d23 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 5 Jun 2021 19:35:32 -0400 Subject: [PATCH 15/15] Fix example in docs --- python/CustomFactors.md | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/python/CustomFactors.md b/python/CustomFactors.md index abba9e00bc..a6ffa2f360 100644 --- a/python/CustomFactors.md +++ b/python/CustomFactors.md @@ -40,24 +40,25 @@ import gtsam import numpy as np from typing import List -def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]): - # Get the variable values from `v` +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] - - # Calculate non-linear error gT1, gT2 = v.atPose2(key0), v.atPose2(key1) - error = gtsam.Pose2(0, 0, 0).localCoordinates(gT1.between(gT2)) + error = expected.localCoordinates(gT1.between(gT2)) - # If we need Jacobian if H is not None: - # Fill the Jacobian arrays - # Note we have two vars, so two entries result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - - # Return the error return error noise_model = gtsam.noiseModel.Unit.Create(3)