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); 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 diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 671dbe34d..55dfd4561 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 { + // Check if either is null. If both are null, then true + iterative_params_equal = !iterativeParams && !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; +} + /* ************************************************************************* */ std::string NonlinearOptimizerParams::linearSolverTranslator( LinearSolverType linearSolverType) const { diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index cafb235bc..cef7e85e3 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -114,16 +114,7 @@ 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 - } + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const; inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY)