From cb226633c59236856a50ae962219656c8eb73e8b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 18:24:25 -0400 Subject: [PATCH 1/6] IterativeSolver equals method --- gtsam/linear/IterativeSolver.cpp | 6 ++++++ gtsam/linear/IterativeSolver.h | 16 +++++++++------- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index c5a25c4b8..e9b0a5f13 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -46,6 +46,12 @@ void IterativeOptimizationParameters::print(ostream &os) const { << verbosityTranslator(verbosity_) << endl; } +/*****************************************************************************/ +bool IterativeOptimizationParameters::equals( + const IterativeOptimizationParameters &other, double tol) const { + return verbosity_ == other.verbosity(); +} + /*****************************************************************************/ ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 0441cd9da..1a66708d4 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -41,15 +41,14 @@ class VectorValues; * parameters for iterative linear solvers */ class IterativeOptimizationParameters { - -public: - + public: typedef std::shared_ptr shared_ptr; - enum Verbosity { - SILENT = 0, COMPLEXITY, ERROR - } verbosity_; + enum Verbosity { SILENT = 0, COMPLEXITY, ERROR }; -public: + protected: + Verbosity verbosity_; + + public: IterativeOptimizationParameters(Verbosity v = SILENT) : verbosity_(v) { @@ -71,6 +70,9 @@ public: /* virtual print function */ GTSAM_EXPORT virtual void print(std::ostream &os) const; + GTSAM_EXPORT virtual bool equals(const IterativeOptimizationParameters &other, + double tol = 1e-9) const; + /* for serialization */ GTSAM_EXPORT friend std::ostream &operator<<( std::ostream &os, const IterativeOptimizationParameters &p); From 48b270efc57fdde5d0726a050c0689a151d996eb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 18:24:41 -0400 Subject: [PATCH 2/6] improved equals method for NonlinearOptimizerParams --- gtsam/nonlinear/NonlinearOptimizerParams.h | 24 ++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index cafb235bc..92e22e064 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -115,14 +115,22 @@ public: virtual void print(const std::string& str = "") const; bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const { - return maxIterations == other.getMaxIterations() - && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol - && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol - && std::abs(errorTol - other.getErrorTol()) <= tol - && verbosityTranslator(verbosity) == other.getVerbosity(); - // && orderingType.equals(other.getOrderingType()_; - // && linearSolverType == other.getLinearSolverType(); - // TODO: check ordering, iterativeParams, and iterationsHook + // Check for equality of shared ptrs + bool iterative_params_equal = false; + if (iterativeParams == other.iterativeParams) { + iterative_params_equal = true; + } + if (iterativeParams && other.iterativeParams) { + iterative_params_equal = iterativeParams->equals(*other.iterativeParams); + } + + return maxIterations == other.getMaxIterations() && + std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol && + std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol && + std::abs(errorTol - other.getErrorTol()) <= tol && + verbosityTranslator(verbosity) == other.getVerbosity() && + orderingType == other.orderingType && ordering == other.ordering && + linearSolverType == other.linearSolverType && iterative_params_equal; } inline bool isMultifrontal() const { From 19542053ccb9047085aa5282108efe8835492828 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 15 Nov 2023 10:43:33 -0500 Subject: [PATCH 3/6] better formatting for LM Optimizer summary --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index c49e3a65a..99682fb77 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -236,9 +236,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, if (currentState->iterations == 0) { cout << "iter cost cost_change lambda success iter_time" << endl; } - cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2) - << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4) - << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl; + cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2) + << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6) + << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl; } if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity From 5cf1e0d220534ddcf712cc6ca09308e9658ba20f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 06:19:00 -0400 Subject: [PATCH 4/6] move NonlinearOptimizerParams::equals definition to .cpp file and improve slightly --- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 22 ++++++++++++++++++++ gtsam/nonlinear/NonlinearOptimizerParams.h | 19 +---------------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 671dbe34d..800db02a0 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -123,6 +123,28 @@ void NonlinearOptimizerParams::print(const std::string& str) const { std::cout.flush(); } +/* ************************************************************************* */ +bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other, + double tol) const { + // Check for equality of shared ptrs + bool iterative_params_equal = iterativeParams == other.iterativeParams; + // Check equality of components + if (iterativeParams && other.iterativeParams) { + iterative_params_equal = iterativeParams->equals(*other.iterativeParams); + } else { + // If one or both shared pointers are null, we can't assume they are equal + iterative_params_equal = false; + } + + return maxIterations == other.getMaxIterations() && + std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol && + std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol && + std::abs(errorTol - other.getErrorTol()) <= tol && + verbosityTranslator(verbosity) == other.getVerbosity() && + orderingType == other.orderingType && ordering == other.ordering && + linearSolverType == other.linearSolverType && iterative_params_equal; +} + /* ************************************************************************* */ std::string NonlinearOptimizerParams::linearSolverTranslator( LinearSolverType linearSolverType) const { diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 92e22e064..0b4d8b2fd 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -114,24 +114,7 @@ public: virtual void print(const std::string& str = "") const; - bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const { - // Check for equality of shared ptrs - bool iterative_params_equal = false; - if (iterativeParams == other.iterativeParams) { - iterative_params_equal = true; - } - if (iterativeParams && other.iterativeParams) { - iterative_params_equal = iterativeParams->equals(*other.iterativeParams); - } - - return maxIterations == other.getMaxIterations() && - std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol && - std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol && - std::abs(errorTol - other.getErrorTol()) <= tol && - verbosityTranslator(verbosity) == other.getVerbosity() && - orderingType == other.orderingType && ordering == other.ordering && - linearSolverType == other.linearSolverType && iterative_params_equal; - } + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9); inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) From fa33bca0c7ffd240014cd878e2175662a22d4dd4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 06:35:17 -0400 Subject: [PATCH 5/6] make equals const-qualified --- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 0b4d8b2fd..cef7e85e3 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -114,7 +114,7 @@ public: virtual void print(const std::string& str = "") const; - bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9); + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const; inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) From c59f458facb10c224383995ebaefeb0fa6883a4a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 15:32:18 -0400 Subject: [PATCH 6/6] improved check --- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 800db02a0..55dfd4561 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -132,8 +132,8 @@ bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other, if (iterativeParams && other.iterativeParams) { iterative_params_equal = iterativeParams->equals(*other.iterativeParams); } else { - // If one or both shared pointers are null, we can't assume they are equal - iterative_params_equal = false; + // Check if either is null. If both are null, then true + iterative_params_equal = !iterativeParams && !other.iterativeParams; } return maxIterations == other.getMaxIterations() &&