From 205803a0eabd2c0b1fd4098bb37ee0e6b1b71a30 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 19 Mar 2019 00:11:45 -0400 Subject: [PATCH] Better optimization parameter wrapping, plus python test --- cython/gtsam/tests/test_NonlinearOptimizer.py | 71 +++++++++++++++++++ gtsam.h | 29 ++++++-- gtsam/nonlinear/LevenbergMarquardtParams.h | 6 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 47 +++--------- 4 files changed, 109 insertions(+), 44 deletions(-) create mode 100644 cython/gtsam/tests/test_NonlinearOptimizer.py diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py new file mode 100644 index 000000000..0d0318f40 --- /dev/null +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -0,0 +1,71 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, + GaussNewtonParams, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + Point2, PriorFactorPoint2, Values) + +KEY1 = 1 +KEY2 = 2 + + +class TestScenario(unittest.TestCase): + def test_optimize(self): + """Do trivial test with three optimizer variants.""" + fg = NonlinearFactorGraph() + model = gtsam.noiseModel_Unit.Create(2) + fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) + + # test error at minimum + xstar = Point2(0, 0) + optimal_values = Values() + optimal_values.insert(KEY1, xstar) + self.assertEqual(0.0, fg.error(optimal_values), 0.0) + + # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + x0 = Point2(3, 3) + initial_values = Values() + initial_values.insert(KEY1, x0) + self.assertEqual(9.0, fg.error(initial_values), 1e-3) + + # optimize parameters + ordering = Ordering() + ordering.push_back(KEY1) + + # Gauss-Newton + gnParams = GaussNewtonParams() + gnParams.setOrdering(ordering) + actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() + self.assertAlmostEqual(0, fg.error(actual1)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setOrdering(ordering) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Dogleg + dlParams = DoglegParams() + dlParams.setOrdering(ordering) + actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() + self.assertAlmostEqual(0, fg.error(actual3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index ed4bf8425..50d59f973 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2001,10 +2001,12 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); bool isMultifrontal() const; bool isSequential() const; @@ -2025,15 +2027,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { LevenbergMarquardtParams(); - double getlambdaInitial() const; + bool getDiagonalDamping() const; double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; string getVerbosityLM() const; - - void setlambdaInitial(double value); + + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); }; #include diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index abb8c3c22..491fbd233 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -122,22 +122,24 @@ public: virtual ~LevenbergMarquardtParams() {} void print(const std::string& str = "") const override; - /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @name Getters/Setters, mainly for wrappers. Use fields above in C++. /// @{ bool getDiagonalDamping() const { return diagonalDamping; } double getlambdaFactor() const { return lambdaFactor; } double getlambdaInitial() const { return lambdaInitial; } double getlambdaLowerBound() const { return lambdaLowerBound; } double getlambdaUpperBound() const { return lambdaUpperBound; } + bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } std::string getLogFile() const { return logFile; } std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } void setlambdaFactor(double value) { lambdaFactor = value; } void setlambdaInitial(double value) { lambdaInitial = value; } void setlambdaLowerBound(double value) { lambdaLowerBound = value; } void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - void setLogFile(const std::string& s) { logFile = s; } void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} }; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 88fc0f850..8e4678590 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -54,47 +54,24 @@ public: } virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { - return maxIterations; - } - double getRelativeErrorTol() const { - return relativeErrorTol; - } - double getAbsoluteErrorTol() const { - return absoluteErrorTol; - } - double getErrorTol() const { - return errorTol; - } - std::string getVerbosity() const { - return verbosityTranslator(verbosity); - } + size_t getMaxIterations() const { return maxIterations; } + double getRelativeErrorTol() const { return relativeErrorTol; } + double getAbsoluteErrorTol() const { return absoluteErrorTol; } + double getErrorTol() const { return errorTol; } + std::string getVerbosity() const { return verbosityTranslator(verbosity); } - void setMaxIterations(int value) { - maxIterations = value; - } - void setRelativeErrorTol(double value) { - relativeErrorTol = value; - } - void setAbsoluteErrorTol(double value) { - absoluteErrorTol = value; - } - void setErrorTol(double value) { - errorTol = value; - } - void setVerbosity(const std::string &src) { + void setMaxIterations(int value) { maxIterations = value; } + void setRelativeErrorTol(double value) { relativeErrorTol = value; } + void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } + void setErrorTol(double value) { errorTol = value; } + void setVerbosity(const std::string& src) { verbosity = verbosityTranslator(src); } static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - // Successive Linearization Parameters - -public: - /** See NonlinearOptimizerParams::linearSolverType */ - enum LinearSolverType { MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_QR, @@ -168,13 +145,9 @@ public: private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; - }; // For backward compatibility: