From c5576c534f3a29f0077c58858321829da859dd6c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 8 Nov 2020 11:57:47 +0100 Subject: [PATCH 1/3] Add iteration hook in non-linear optimizers --- .../NonlinearConjugateGradientOptimizer.h | 4 ++ gtsam/nonlinear/NonlinearOptimizer.cpp | 6 ++- gtsam/nonlinear/NonlinearOptimizer.h | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 30 ++++++----- tests/testNonlinearOptimizer.cpp | 52 +++++++++++++++++++ 5 files changed, 80 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index abf6b257aa..a85f274250 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -200,6 +200,10 @@ boost::tuple nonlinearConjugateGradient(const S &system, currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); + // User hook: + if (params.iterationHook) + params.iterationHook(iteration, prevError, currentError); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index fd99617427..0d7e9e17f2 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -97,7 +97,11 @@ void NonlinearOptimizer::defaultOptimize() { // Update newError for either printouts or conditional-end checks: newError = error(); - + + // User hook: + if (params.iterationHook) + params.iterationHook(iterations(), currentError, newError); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) values().print("newValues"); diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 9935f44cea..6fe369dd35 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -81,7 +81,7 @@ class GTSAM_EXPORT NonlinearOptimizer { public: /** A shared pointer to this class */ - typedef boost::shared_ptr shared_ptr; + using shared_ptr = boost::shared_ptr; /// @name Standard interface /// @{ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 65fdd1c92f..53805f5f04 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,22 +33,19 @@ namespace gtsam { */ class GTSAM_EXPORT NonlinearOptimizerParams { public: + NonlinearOptimizerParams() = default; + /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) - double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) - double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) - double errorTol; ///< The maximum total error to stop iterating (default 0.0) - Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) - - NonlinearOptimizerParams() : - maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), - linearSolverType(MULTIFRONTAL_CHOLESKY) {} + size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100) + double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5) + double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5) + double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0) + Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) + Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) virtual ~NonlinearOptimizerParams() { } @@ -71,6 +68,15 @@ class GTSAM_EXPORT NonlinearOptimizerParams { static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; + /** Type for an optional user-provided hook to be called after each + * internal optimizer iteration */ + using IterationHook = std::function< + void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; + + /** Optional user-provided iteration hook to be called after each + * optimization iteration (Default: empty) */ + IterationHook iterationHook; + /** See NonlinearOptimizerParams::linearSolverType */ enum LinearSolverType { MULTIFRONTAL_CHOLESKY, @@ -81,7 +87,7 @@ class GTSAM_EXPORT NonlinearOptimizerParams { CHOLMOD, /* Experimental Flag */ }; - LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index dc19801a2e..6415174d5b 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -566,6 +566,58 @@ TEST( NonlinearOptimizer, logfile ) // EXPECT(actual.str()==expected.str()); } +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterationHook_LM ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + LevenbergMarquardtParams lmParams; + size_t lastIterCalled = 0; + lmParams.iterationHook = [&](size_t iteration, double oldError, double newError) + { + // Tests: + lastIterCalled = iteration; + EXPECT(newError " << newError <<"\n"; + }; + LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); + + EXPECT(lastIterCalled>5); +} +/* ************************************************************************* */ +TEST( NonlinearOptimizer, iterationHook_CG ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + NonlinearConjugateGradientOptimizer::Parameters cgParams; + size_t lastIterCalled = 0; + cgParams.iterationHook = [&](size_t iteration, double oldError, double newError) + { + // Tests: + lastIterCalled = iteration; + EXPECT(newError " << newError <<"\n"; + }; + NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize(); + + EXPECT(lastIterCalled>5); +} + + /* ************************************************************************* */ //// Minimal traits example struct MyType : public Vector3 { From 91d275c9c74c955bb1ff86ea9b213e0344b796de Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 16 Nov 2020 07:25:30 +0100 Subject: [PATCH 2/3] Add docs, fix ctor placement --- gtsam/nonlinear/NonlinearOptimizerParams.h | 40 ++++++++++++++++------ 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 53805f5f04..92c800e0c7 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,8 +33,6 @@ namespace gtsam { */ class GTSAM_EXPORT NonlinearOptimizerParams { public: - NonlinearOptimizerParams() = default; - /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR @@ -47,10 +45,6 @@ class GTSAM_EXPORT NonlinearOptimizerParams { Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) - virtual ~NonlinearOptimizerParams() { - } - virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { return relativeErrorTol; } double getAbsoluteErrorTol() const { return absoluteErrorTol; } @@ -68,13 +62,33 @@ class GTSAM_EXPORT NonlinearOptimizerParams { static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - /** Type for an optional user-provided hook to be called after each - * internal optimizer iteration */ + /** Type for an optional user-provided hook to be called after each + * internal optimizer iteration. See iterationHook below. */ using IterationHook = std::function< void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; - /** Optional user-provided iteration hook to be called after each - * optimization iteration (Default: empty) */ + /** Optional user-provided iteration hook to be called after each + * optimization iteration (Default: none). + * Note that `IterationHook` is defined as a std::function<> with this + * signature: + * \code + * void(size_t iteration, double errorBefore, double errorAfter) + * \endcode + * which allows binding by means of a reference to a regular function: + * \code + * void foo(size_t iteration, double errorBefore, double errorAfter); + * // ... + * lmOpts.iterationHook = &foo; + * \endcode + * or to a C++11 lambda: + * \code + * lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) + * { + * // ... + * }; + * \endcode + * or to the result of a properly-formed `std::bind` call. + */ IterationHook iterationHook; /** See NonlinearOptimizerParams::linearSolverType */ @@ -91,6 +105,12 @@ class GTSAM_EXPORT NonlinearOptimizerParams { boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + NonlinearOptimizerParams() = default; + virtual ~NonlinearOptimizerParams() { + } + + virtual void print(const std::string& str = "") const; + inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); From 9a4bd10e222918cfd984d793a04fad24741b7c21 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 16 Nov 2020 07:32:03 +0100 Subject: [PATCH 3/3] further extended docs --- gtsam/nonlinear/NonlinearOptimizerParams.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 92c800e0c7..a7bc10a1f4 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -80,7 +80,9 @@ class GTSAM_EXPORT NonlinearOptimizerParams { * // ... * lmOpts.iterationHook = &foo; * \endcode - * or to a C++11 lambda: + * or to a C++11 lambda (preferred if you need to capture additional + * context variables, such that the optimizer object itself, the factor graph, + * etc.): * \code * lmOpts.iterationHook = [&](size_t iter, double oldError, double newError) * {