From aee494a24f967f652aae6783df95601cf7bab2c2 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Tue, 14 Jun 2022 17:07:29 -0400 Subject: [PATCH 01/18] Expose GNC params to python --- gtsam/nonlinear/GncOptimizer.h | 8 ++++---- gtsam/nonlinear/nonlinear.i | 17 +++++++++++++++++ python/gtsam/preamble/nonlinear.h | 3 ++- 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index cc3fdaf34f..69067a915d 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -246,7 +246,9 @@ class GTSAM_EXPORT GncOptimizer { prev_cost = cost; // display info - if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { + std::cout << "iter: " << iter << std::endl; + std::cout << "mu: " << mu << std::endl; std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } @@ -255,9 +257,7 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; - std::cout << "previous cost: " << prev_cost << std::endl; - std::cout << "current cost: " << cost << std::endl; + std::cout << "final cost: " << cost << std::endl; } return result; } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 033e5ced25..21470060c7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -544,11 +544,23 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include +enum GncLossType { + GM /*Geman McClure*/, + TLS /*Truncated least squares*/ +}; + template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); + void setLossType(const GncLossType type); + void setMaxIterations(const size_t maxIter); + void setMuStep(const double step); + void setRelativeCostTol(double value); + void setWeightsTol(double value); void setVerbosityGNC(const This::Verbosity value); + void setKnownInliers(const std::vector& knownIn); + void setKnownOutliers(const std::vector& knownOut); void print(const string& str) const; enum Verbosity { @@ -597,6 +609,11 @@ virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const PARAMS& params); + void setInlierCostThresholds(const double inth); + const Vector& getInlierCostThresholds(); + void setInlierCostThresholdsAtProbability(const double alpha); + void setWeights(const Vector w); + const Vector& getWeights(); gtsam::Values optimize(); }; diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index a34e730580..34b864027f 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,4 +9,5 @@ * 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++. - */ \ No newline at end of file + */ +#include \ No newline at end of file From 4333f9a186f26aa7e181a7c0aced34fb42b5d681 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Thu, 7 Jul 2022 13:21:54 -0400 Subject: [PATCH 02/18] Revert changes to verbosity --- gtsam/nonlinear/GncOptimizer.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 69067a915d..cc3fdaf34f 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -246,9 +246,7 @@ class GTSAM_EXPORT GncOptimizer { prev_cost = cost; // display info - if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { - std::cout << "iter: " << iter << std::endl; - std::cout << "mu: " << mu << std::endl; + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } @@ -257,7 +255,9 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final cost: " << cost << std::endl; + std::cout << "final weights: " << weights_ << std::endl; + std::cout << "previous cost: " << prev_cost << std::endl; + std::cout << "current cost: " << cost << std::endl; } return result; } From 26dd94bdaf554c18c2f9027a6bb04d2a4448a744 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Fri, 8 Jul 2022 12:33:47 -0400 Subject: [PATCH 03/18] Expose remaining attributes, add tests --- gtsam/nonlinear/nonlinear.i | 12 +++- python/gtsam/tests/test_NonlinearOptimizer.py | 60 ++++++++++++++++++- 2 files changed, 68 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 21470060c7..913be1aed1 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -553,6 +553,16 @@ template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); + BaseOptimizerParameters baseOptimizerParams; + GncLossType lossType; + size_t maxIterations; + double muStep; + double relativeCostTol; + double weightsTol; + Verbosity verbosity; + std::vector knownInliers; + std::vector knownOutliers; + void setLossType(const GncLossType type); void setMaxIterations(const size_t maxIter); void setMuStep(const double step); @@ -561,7 +571,7 @@ virtual class GncParams { void setVerbosityGNC(const This::Verbosity value); void setKnownInliers(const std::vector& knownIn); void setKnownOutliers(const std::vector& knownOut); - void print(const string& str) const; + void print(const string& str = "") const; enum Verbosity { SILENT, diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 37afd9e1c0..47d41edb24 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -16,9 +16,10 @@ import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, - GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer, - LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, - Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) + GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLossType, + GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, + PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -83,6 +84,59 @@ def test_graduated_non_convexity(self): actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize() self.assertAlmostEqual(0, self.fg.error(actual)) + def test_gnc_params(self): + base_params = LevenbergMarquardtParams() + # Test base params + for base_max_iters in (50, 100): + base_params.setMaxIterations(base_max_iters) + params = GncLMParams(base_params) + self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters) + # Test printing + str(params) + # Test each parameter + for loss_type in (GncLossType.TLS, GncLossType.GM): + params.setLossType(loss_type) # Default is TLS + self.assertEqual(params.lossType, loss_type) + for max_iter in (1, 10, 100): + params.setMaxIterations(max_iter) + self.assertEqual(params.maxIterations, max_iter) + for mu_step in (1.1, 1.2, 1.5): + params.setMuStep(mu_step) + self.assertEqual(params.muStep, mu_step) + for rel_cost_tol in (1e-5, 1e-6, 1e-7): + params.setRelativeCostTol(rel_cost_tol) + self.assertEqual(params.relativeCostTol, rel_cost_tol) + for weights_tol in (1e-4, 1e-3, 1e-2): + params.setWeightsTol(weights_tol) + self.assertEqual(params.weightsTol, weights_tol) + for i in (0, 1, 2): + verb = GncLMParams.Verbosity(i) + params.setVerbosityGNC(verb) + self.assertEqual(params.verbosity, verb) + for inl in ([], [10], [0, 100]): + params.setKnownInliers(inl) + self.assertEqual(params.knownInliers, inl) + params.knownInliers = [] + for out in ([], [1], [0, 10]): + params.setKnownInliers(out) + self.assertEqual(params.knownInliers, out) + params.knownInliers = [] + # Test optimizer params + optimizer = GncLMOptimizer(self.fg, self.initial_values, params) + for ict_factor in (0.9, 1.1): + new_ict = ict_factor * optimizer.getInlierCostThresholds() + optimizer.setInlierCostThresholds(new_ict) + self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict) + for w_factor in (0.8, 0.9): + new_weights = w_factor * optimizer.getWeights() + optimizer.setWeights(new_weights) + self.assertAlmostEqual(optimizer.getWeights(), new_weights) + optimizer.setInlierCostThresholdsAtProbability(0.9) + w1 = optimizer.getInlierCostThresholds() + optimizer.setInlierCostThresholdsAtProbability(0.8) + w2 = optimizer.getInlierCostThresholds() + self.assertLess(w2, w1) + def test_iteration_hook(self): # set up iteration hook to track some testable values iteration_count = 0 From 9dd1f8ffaf0112a8ff053ad347157c1382010e47 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Fri, 8 Jul 2022 16:48:52 -0400 Subject: [PATCH 04/18] Adress review comments --- gtsam/nonlinear/GncParams.h | 2 +- gtsam/nonlinear/nonlinear.i | 2 +- python/gtsam/tests/test_NonlinearOptimizer.py | 14 +++++++++++++- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 086f08acc7..600a909766 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -161,7 +161,7 @@ class GTSAM_EXPORT GncParams { std::cout << "knownInliers: " << knownInliers[i] << "\n"; for (size_t i = 0; i < knownOutliers.size(); i++) std::cout << "knownOutliers: " << knownOutliers[i] << "\n"; - baseOptimizerParams.print(str); + baseOptimizerParams.print("Base optimizer params: "); } }; diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 913be1aed1..b6d0f31be4 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -571,7 +571,7 @@ virtual class GncParams { void setVerbosityGNC(const This::Verbosity value); void setKnownInliers(const std::vector& knownIn); void setKnownOutliers(const std::vector& knownOut); - void print(const string& str = "") const; + void print(const string& str = "GncParams: ") const; enum Verbosity { SILENT, diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 47d41edb24..3db2bb1961 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -91,8 +91,19 @@ def test_gnc_params(self): base_params.setMaxIterations(base_max_iters) params = GncLMParams(base_params) self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters) + # Test printing - str(params) + params_str = str(params) + for s in ( + "lossType", + "maxIterations", + "muStep", + "relativeCostTol", + "weightsTol", + "verbosity", + ): + self.assertTrue(s in params_str) + # Test each parameter for loss_type in (GncLossType.TLS, GncLossType.GM): params.setLossType(loss_type) # Default is TLS @@ -121,6 +132,7 @@ def test_gnc_params(self): params.setKnownInliers(out) self.assertEqual(params.knownInliers, out) params.knownInliers = [] + # Test optimizer params optimizer = GncLMOptimizer(self.fg, self.initial_values, params) for ict_factor in (0.9, 1.1): From b4fdda4740c39a78306f9189bab2c80ffedf8ffd Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Fri, 8 Jul 2022 16:49:04 -0400 Subject: [PATCH 05/18] Run yapf --- python/gtsam/tests/test_NonlinearOptimizer.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 3db2bb1961..a47c5ad620 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -15,11 +15,12 @@ import unittest import gtsam -from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, - GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLossType, - GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, - PriorFactorPoint2, Values) +from gtsam import ( + DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, + GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, + PriorFactorPoint2, Values +) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -28,7 +29,6 @@ class TestScenario(GtsamTestCase): """Do trivial test with three optimizer variants.""" - def setUp(self): """Set up the optimization problem and ordering""" # create graph @@ -154,11 +154,13 @@ def test_iteration_hook(self): iteration_count = 0 final_error = 0 final_values = None + def iteration_hook(iter, error_before, error_after): nonlocal iteration_count, final_error, final_values iteration_count = iter final_error = error_after final_values = optimizer.values() + # optimize params = LevenbergMarquardtParams.CeresDefaults() params.setOrdering(self.ordering) @@ -170,5 +172,6 @@ def iteration_hook(iter, error_before, error_after): self.assertEqual(self.fg.error(actual), final_error) self.assertEqual(optimizer.iterations(), iteration_count) + if __name__ == "__main__": unittest.main() From 01ea31633654a137ed1cc7f74ffac5c2bec37648 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Wed, 13 Jul 2022 08:33:09 -0400 Subject: [PATCH 06/18] Make vector opaque --- python/gtsam/preamble/nonlinear.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 34b864027f..b50059a2ed 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,4 +10,6 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include \ No newline at end of file +#include + +PYBIND11_MAKE_OPAQUE(std::vector); From e381a7c9d21d7e63d315ace33612cfe46cbd28e9 Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Wed, 13 Jul 2022 23:27:00 -0400 Subject: [PATCH 07/18] Revert pybind stl include --- python/gtsam/preamble/nonlinear.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index b50059a2ed..d07a75f6fb 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,6 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include - -PYBIND11_MAKE_OPAQUE(std::vector); From 3d3703441c60f49abe93314add486d35efeee623 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 14:08:31 -0400 Subject: [PATCH 08/18] wrap custom factor in a separate file --- gtsam/nonlinear/custom.i | 38 +++++++++++++++++++++++++++ matlab/CMakeLists.txt | 1 + python/CMakeLists.txt | 1 + python/gtsam/preamble/custom.h | 12 +++++++++ python/gtsam/preamble/nonlinear.h | 1 + python/gtsam/specializations/custom.h | 12 +++++++++ 6 files changed, 65 insertions(+) create mode 100644 gtsam/nonlinear/custom.i create mode 100644 python/gtsam/preamble/custom.h create mode 100644 python/gtsam/specializations/custom.h diff --git a/gtsam/nonlinear/custom.i b/gtsam/nonlinear/custom.i new file mode 100644 index 0000000000..0d195a91b6 --- /dev/null +++ b/gtsam/nonlinear/custom.i @@ -0,0 +1,38 @@ +//************************************************************************* +// Custom Factor wrapping +//************************************************************************* + +namespace gtsam { + +#include +virtual class CustomFactor : gtsam::NoiseModelFactor { + /* + * 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: + * ``` + * 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); + + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); +}; + +} \ No newline at end of file diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index a657c6be7e..b83bf1f0fe 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -73,6 +73,7 @@ set(interface_files ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/custom.i ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c14f02ddab..f798002964 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/geometry/geometry.i ${PROJECT_SOURCE_DIR}/gtsam/linear/linear.i ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i + ${PROJECT_SOURCE_DIR}/gtsam/nonlinear/custom.i ${PROJECT_SOURCE_DIR}/gtsam/symbolic/symbolic.i ${PROJECT_SOURCE_DIR}/gtsam/sam/sam.i ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h new file mode 100644 index 0000000000..d07a75f6fb --- /dev/null +++ b/python/gtsam/preamble/custom.h @@ -0,0 +1,12 @@ +/* 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++. + */ diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index d07a75f6fb..661215e599 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,3 +10,4 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ +#include diff --git a/python/gtsam/specializations/custom.h b/python/gtsam/specializations/custom.h new file mode 100644 index 0000000000..d46ccdc668 --- /dev/null +++ b/python/gtsam/specializations/custom.h @@ -0,0 +1,12 @@ +/* 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 + * ================= + * + * `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. + */ \ No newline at end of file From 31d174d640001c50d531ec5078017a8441d98624 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 14:09:39 -0400 Subject: [PATCH 09/18] add gtsam namespace --- gtsam/nonlinear/CustomFactor.h | 2 -- gtsam/nonlinear/nonlinear.i | 43 +++++----------------------------- 2 files changed, 6 insertions(+), 39 deletions(-) diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 615b5418e8..8580a49496 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -19,8 +19,6 @@ #include -using namespace gtsam; - namespace gtsam { using JacobianVector = std::vector; diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index b6d0f31be4..c793332368 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -99,11 +99,11 @@ class NonlinearFactorGraph { string dot( const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& writer = GraphvizFormatting()); + const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()); void saveGraph( const string& s, const gtsam::Values& values, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter, - const GraphvizFormatting& writer = GraphvizFormatting()) const; + const gtsam::GraphvizFormatting& writer = gtsam::GraphvizFormatting()) const; // enabling serialization functionality void serialize() const; @@ -135,37 +135,6 @@ virtual class NoiseModelFactor : gtsam::NonlinearFactor { Vector whitenedError(const gtsam::Values& x) const; }; -#include -virtual class CustomFactor : gtsam::NoiseModelFactor { - /* - * 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: - * ``` - * 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); - - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); -}; - #include class Values { Values(); @@ -554,7 +523,7 @@ virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); BaseOptimizerParameters baseOptimizerParams; - GncLossType lossType; + gtsam::GncLossType lossType; size_t maxIterations; double muStep; double relativeCostTol; @@ -563,12 +532,12 @@ virtual class GncParams { std::vector knownInliers; std::vector knownOutliers; - void setLossType(const GncLossType type); + void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); void setMuStep(const double step); void setRelativeCostTol(double value); void setWeightsTol(double value); - void setVerbosityGNC(const This::Verbosity value); + void setVerbosityGNC(const gtsam::This::Verbosity value); void setKnownInliers(const std::vector& knownIn); void setKnownOutliers(const std::vector& knownOut); void print(const string& str = "GncParams: ") const; @@ -900,7 +869,7 @@ template , gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { - NonlinearEquality2(Key key1, Key key2, double mu = 1e4); + NonlinearEquality2(gtsam::Key key1, gtsam::Key key2, double mu = 1e4); gtsam::Vector evaluateError(const T& x1, const T& x2); }; From 90723d933edf9bd68135444f1bc70ad678da1aaa Mon Sep 17 00:00:00 2001 From: Amado Antonini Date: Tue, 19 Jul 2022 19:32:21 -0400 Subject: [PATCH 10/18] Remove pybind/stl.h again --- python/gtsam/preamble/nonlinear.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 661215e599..a34e730580 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,5 +9,4 @@ * 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++. - */ -#include + */ \ No newline at end of file From 12b35b41424d1564162fde59b47b474b23d26678 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 18:59:58 -0400 Subject: [PATCH 11/18] remove pybind stl.h --- python/gtsam/preamble/nonlinear.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index a34e730580..d07a75f6fb 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,4 +9,4 @@ * 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++. - */ \ No newline at end of file + */ From 7eb9a95c5fbf0e60813d69443e2e9ab7dcd50c58 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:35:09 -0400 Subject: [PATCH 12/18] minor fixes --- gtsam/nonlinear/NonlinearFactor.cpp | 3 ++- python/gtsam/preamble/nonlinear.h | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 3d572e9702..debff54acd 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -167,8 +167,9 @@ boost::shared_ptr NoiseModelFactor::linearize( return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, boost::static_pointer_cast(noiseModel_)->unit())); - else + else { return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); + } } /* ************************************************************************* */ diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index d07a75f6fb..56a07cfdd1 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include From 27951b7b180f7cdfc3300fb9d8d163586fb9f305 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:17:48 -0400 Subject: [PATCH 13/18] use python 3.7 --- .github/scripts/python.sh | 2 +- .github/workflows/build-python.yml | 16 ++++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 0855dbc21b..b734f59191 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -59,7 +59,7 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin BUILD_PYBIND="ON" -sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt +$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f80b9362c9..d391e571c0 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -27,7 +27,7 @@ jobs: ] build_type: [Debug, Release] - python_version: [3] + python_version: [3.7] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 @@ -44,14 +44,6 @@ jobs: compiler: clang version: "9" - # NOTE temporarily added this as it is a required check. - - name: ubuntu-18.04-clang-9 - os: ubuntu-18.04 - compiler: clang - version: "9" - build_type: Debug - python_version: "3" - - name: macOS-11-xcode-13.4.1 os: macOS-11 compiler: xcode @@ -65,7 +57,11 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: '3.7' - name: Install (Linux) if: runner.os == 'Linux' run: | From 4edcb41ad3680a9d4f6ee2f829c8ffd49b2bdba8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:19:05 -0400 Subject: [PATCH 14/18] remove redundant KeyVector definition --- gtsam/gtsam.i | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d4e959c3dd..2671f0ef77 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -66,27 +66,6 @@ class KeySet { void serialize() const; }; -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - // Actually a FastMap class KeyGroupMap { KeyGroupMap(); From 42bab8f3e70eee07a2ae591786f3b2a297b6784a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:19:29 -0400 Subject: [PATCH 15/18] use KeyVector for GNC inliers & outliers --- gtsam/nonlinear/GncParams.h | 12 ++++++++---- gtsam/nonlinear/nonlinear.i | 8 ++++---- python/gtsam/preamble/nonlinear.h | 2 -- python/gtsam/specializations/nonlinear.h | 2 +- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index a7ccb88b76..08ae423366 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -72,8 +72,12 @@ class GTSAM_EXPORT GncParams { double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - std::vector knownInliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are inliers - std::vector knownOutliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers + + // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping + ///< Slots in the factor graph corresponding to measurements that we know are inliers + KeyVector knownInliers = KeyVector(); + ///< Slots in the factor graph corresponding to measurements that we know are outliers + KeyVector knownOutliers = KeyVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -114,7 +118,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const std::vector& knownIn) { + void setKnownInliers(const KeyVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -125,7 +129,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const std::vector& knownOut) { + void setKnownOutliers(const KeyVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c793332368..4bb80a0d62 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -529,8 +529,8 @@ virtual class GncParams { double relativeCostTol; double weightsTol; Verbosity verbosity; - std::vector knownInliers; - std::vector knownOutliers; + gtsam::KeyVector knownInliers; + gtsam::KeyVector knownOutliers; void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); @@ -538,8 +538,8 @@ virtual class GncParams { void setRelativeCostTol(double value); void setWeightsTol(double value); void setVerbosityGNC(const gtsam::This::Verbosity value); - void setKnownInliers(const std::vector& knownIn); - void setKnownOutliers(const std::vector& knownOut); + void setKnownInliers(const gtsam::KeyVector& knownIn); + void setKnownOutliers(const gtsam::KeyVector& knownOut); void print(const string& str = "GncParams: ") const; enum Verbosity { diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 56a07cfdd1..d07a75f6fb 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -#include diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h index d46ccdc668..da8842eaf4 100644 --- a/python/gtsam/specializations/nonlinear.h +++ b/python/gtsam/specializations/nonlinear.h @@ -9,4 +9,4 @@ * 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. - */ \ No newline at end of file + */ From 1d51c4e64692f0792685d4d50873ba00ef61d238 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Jul 2022 07:54:05 -0400 Subject: [PATCH 16/18] Use new GncParams::IndexVector --- gtsam/nonlinear/GncParams.h | 12 +++++++----- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 08ae423366..e069d5f3d9 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,11 +73,13 @@ class GTSAM_EXPORT GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping + //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. + /// Use IndexVector for inliers and outliers since it is fast + wrapping + using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - KeyVector knownInliers = KeyVector(); + IndexVector knownInliers = IndexVector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers - KeyVector knownOutliers = KeyVector(); + IndexVector knownOutliers = IndexVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -118,7 +120,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const KeyVector& knownIn) { + void setKnownInliers(const IndexVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -129,7 +131,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const KeyVector& knownOut) { + void setKnownOutliers(const IndexVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index c3335ce20c..15d660114f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption std::vector knownInliers; + // assumption GncParams::IndexVector knownInliers; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers); From a3aaaeb8a76f4fad1707ff834e3db0c06dbf3126 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Jul 2022 01:01:12 -0400 Subject: [PATCH 17/18] undo CI changes --- .github/workflows/build-python.yml | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 314e465454..5a2e554b1b 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -27,7 +27,7 @@ jobs: ] build_type: [Debug, Release] - python_version: [3.7] + python_version: [3] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 @@ -44,6 +44,14 @@ jobs: compiler: clang version: "9" + # NOTE temporarily added this as it is a required check. + - name: ubuntu-18.04-clang-9 + os: ubuntu-18.04 + compiler: clang + version: "9" + build_type: Debug + python_version: "3" + - name: macOS-11-xcode-13.4.1 os: macOS-11 compiler: xcode @@ -58,10 +66,6 @@ jobs: steps: - name: Checkout uses: actions/checkout@v3 - - name: Set up Python - uses: actions/setup-python@v4 - with: - python-version: '3.7' - name: Install (Linux) if: runner.os == 'Linux' run: | From 790403ca6989698f24f6ef2eada4cf0bfe1e3b36 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Jul 2022 10:53:19 -0400 Subject: [PATCH 18/18] revert checkout version --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 5a2e554b1b..1e1519da60 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -65,7 +65,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v2 - name: Install (Linux) if: runner.os == 'Linux' run: |