From d0c31d3eb0450e398ec945540fbc7a5223e7082b Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 11 Aug 2021 04:46:32 -0400 Subject: [PATCH 01/13] start wrapping GNC --- gtsam/nonlinear/nonlinear.i | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 8071e8bc79..dceb0c3083 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -522,6 +522,14 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { void setVerbosityDL(string verbosityDL) const; }; +#include +template +class GncParams { + GncParams(const BaseOptimizerParameters& baseOptimizerParams); + GncParams(); + void print(const std::string& str) const; +}; + #include virtual class NonlinearOptimizer { gtsam::Values optimize(); @@ -551,6 +559,15 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { const gtsam::DoglegParams& params); double getDelta() const; }; + +#include +template +class GncOptimizer { + GncOptimizer(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::GncParameters& params); + gtsam::Values optimize(); +}; #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { From 6bd6651ea29f61f5c46b73274b098e380fd1c8c1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 11 Aug 2021 22:17:08 +0900 Subject: [PATCH 02/13] Remove std prefix in .i fn signatures --- gtsam/nonlinear/nonlinear.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index dceb0c3083..9f19141e1c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -527,7 +527,7 @@ template class GncParams { GncParams(const BaseOptimizerParameters& baseOptimizerParams); GncParams(); - void print(const std::string& str) const; + void print(const string& str) const; }; #include From d0f25ec7b98f3e0f194b3482a99848cac7270768 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 05:07:33 +0900 Subject: [PATCH 03/13] =?UTF-8?q?Remove=20=E2=80=98class=E2=80=99=20from?= =?UTF-8?q?=20template=20specifications?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- gtsam/nonlinear/nonlinear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 9f19141e1c..704d1d2579 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,7 +523,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template class GncParams { GncParams(const BaseOptimizerParameters& baseOptimizerParams); GncParams(); @@ -561,7 +561,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template +template class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, From 651815724b793cbb006f38e900339492e9baae57 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 11 Aug 2021 21:33:10 -0400 Subject: [PATCH 04/13] try imports --- python/gtsam/tests/test_NonlinearOptimizer.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index e9234a43b1..5df7fcc0a3 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -17,8 +17,9 @@ import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, LevenbergMarquardtOptimizer, - LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + GaussNewtonParams, GncParams, GncOptimizer, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase From 6364e34013985942db1ed9f77e839cc0449ade79 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 03:15:52 -0400 Subject: [PATCH 05/13] provide template list of typedef --- gtsam/nonlinear/nonlinear.i | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 704d1d2579..97b8d2c237 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,9 +523,9 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template class GncParams { - GncParams(const BaseOptimizerParameters& baseOptimizerParams); + GncParams(const PARAMS& baseOptimizerParams); GncParams(); void print(const string& str) const; }; @@ -561,11 +561,11 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template +template class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, - const gtsam::GncParameters& params); + const PARAMS& params); gtsam::Values optimize(); }; From c9bcb1430cfb350e3db3b28a8f2fca0f9999d28a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 03:26:53 -0400 Subject: [PATCH 06/13] test GNC along with other non-linear optimizers in python unit tests --- python/gtsam/tests/test_NonlinearOptimizer.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 5df7fcc0a3..66339b41b1 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -78,6 +78,12 @@ def test_optimize(self): dlParams.setOrdering(ordering) actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() self.assertAlmostEqual(0, fg.error(actual3)) + + # Graduated Non-Convexity (GNC) + gncParams = GncParams() + actual4 = GncOptimizer(fg, initial_values, gncParams).optimize() + self.assertAlmostEqual(0, fg.error(actual4)) + if __name__ == "__main__": From 9b05390ccba3f17c12c9f738bd8766b07e92ed28 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 06:44:01 -0400 Subject: [PATCH 07/13] remove Dogleg from GNC-supported base-optimizers, and use nested templates --- gtsam/nonlinear/nonlinear.i | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 97b8d2c237..7c81f04f20 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,7 +523,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); @@ -561,7 +561,8 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template +template, + gtsam::GncParams}> class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, From e9465128fda2cfdcdac0d1fffaab443c555b2d87 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:19:58 -0400 Subject: [PATCH 08/13] add virtual to all classes in the .i file --- gtsam/nonlinear/nonlinear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 7c81f04f20..a22b310237 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -524,7 +524,7 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { #include template -class GncParams { +virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); void print(const string& str) const; @@ -563,7 +563,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { #include template, gtsam::GncParams}> -class GncOptimizer { +virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const PARAMS& params); From 7bf640e1f5135fa0d6bd157f0f872725063abd8c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:22:10 -0400 Subject: [PATCH 09/13] add GTSAM_EXPORT to GncParams.h --- gtsam/nonlinear/GncParams.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index c1bf7a0352..086f08acc7 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -39,7 +39,7 @@ enum GncLossType { }; template -class GncParams { +class GTSAM_EXPORT GncParams { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename BaseOptimizerParameters::OptimizerType OptimizerType; From 6e788faf34a1fa63e10c57a1b6d2cb3c783d69f2 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 07:22:37 -0400 Subject: [PATCH 10/13] add GTSAM_EXPORT to GncOptimizer.h --- gtsam/nonlinear/GncOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index eb353c53f9..3ddaf4820e 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -42,7 +42,7 @@ static double Chi2inv(const double alpha, const size_t dofs) { /* ************************************************************************* */ template -class GncOptimizer { +class GTSAM_EXPORT GncOptimizer { public: /// For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimizer. typedef typename GncParameters::OptimizerType BaseOptimizer; From 30d028ef74d86dbdfa9f3e18118edf5ee24562a0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 17:30:20 -0400 Subject: [PATCH 11/13] use custom typedefs for GNC + GaussNewton and GNC + LM --- gtsam/nonlinear/nonlinear.i | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index a22b310237..d2047b7987 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -523,12 +523,15 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams { }; #include -template +template virtual class GncParams { GncParams(const PARAMS& baseOptimizerParams); GncParams(); void print(const string& str) const; }; + +typedef GncParams GncGaussNewtonParams; +typedef GncParams GncLMParams; #include virtual class NonlinearOptimizer { @@ -561,8 +564,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { }; #include -template, - gtsam::GncParams}> +template virtual class GncOptimizer { GncOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, @@ -570,6 +572,9 @@ virtual class GncOptimizer { gtsam::Values optimize(); }; +typedef gtsam::GncOptimizer > GncGaussNewtonOptimizer; +typedef gtsam::GncOptimizer > GncLMOptimizer; + #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, From db9b98030b8ffc0d485bdae0a21453bf476dc898 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 18:31:24 -0400 Subject: [PATCH 12/13] import GncLMParams, GncLMOptimizer to prevent pybind's automatic long names from name concat --- python/gtsam/tests/test_NonlinearOptimizer.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py index 66339b41b1..e2561ae52a 100644 --- a/python/gtsam/tests/test_NonlinearOptimizer.py +++ b/python/gtsam/tests/test_NonlinearOptimizer.py @@ -17,7 +17,7 @@ import gtsam from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer, - GaussNewtonParams, GncParams, GncOptimizer, + GaussNewtonParams, GncLMParams, GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values) @@ -80,8 +80,8 @@ def test_optimize(self): self.assertAlmostEqual(0, fg.error(actual3)) # Graduated Non-Convexity (GNC) - gncParams = GncParams() - actual4 = GncOptimizer(fg, initial_values, gncParams).optimize() + gncParams = GncLMParams() + actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize() self.assertAlmostEqual(0, fg.error(actual4)) From b7eccdab7ba31e9f81da575a0456afd56d6e871c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Thu, 12 Aug 2021 18:35:39 -0400 Subject: [PATCH 13/13] add missing gtsam prefix --- gtsam/nonlinear/nonlinear.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d2047b7987..73eaef1250 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -530,8 +530,8 @@ virtual class GncParams { void print(const string& str) const; }; -typedef GncParams GncGaussNewtonParams; -typedef GncParams GncLMParams; +typedef gtsam::GncParams GncGaussNewtonParams; +typedef gtsam::GncParams GncLMParams; #include virtual class NonlinearOptimizer {