add set/get interface for LMParameters in c++ and matlab
							parent
							
								
									188478e4ed
								
							
						
					
					
						commit
						5acc52bbae
					
				
							
								
								
									
										9
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										9
									
								
								gtsam.h
								
								
								
								
							|  | @ -961,6 +961,15 @@ class LevenbergMarquardtParams { | |||
|   LevenbergMarquardtParams(); | ||||
|   LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose); | ||||
|   void print(string s) const; | ||||
|   double getlambdaInitial() const ; | ||||
|   double getlambdaFactor() const ; | ||||
|   double getlambdaUpperBound() const; | ||||
|   string getVerbosityLM() const ; | ||||
| 
 | ||||
|   void setlambdaInitial(double value); | ||||
|   void setlambdaFactor(double value); | ||||
|   void setlambdaUpperBound(double value); | ||||
|   void setVerbosityLM(string s); | ||||
| }; | ||||
| 
 | ||||
| }///\namespace gtsam
 | ||||
|  |  | |||
|  | @ -25,11 +25,52 @@ | |||
| #include <gtsam/linear/GaussianJunctionTree.h> | ||||
| #include <gtsam/linear/SimpleSPCGSolver.h> | ||||
| #include <gtsam/linear/SubgraphSolver.h> | ||||
| #include <boost/algorithm/string.hpp> | ||||
| #include <string> | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(const std::string &src) const { | ||||
|   std::string s = src;  boost::algorithm::to_upper(s); | ||||
|   if (s == "SILENT") return LevenbergMarquardtParams::SILENT; | ||||
|   if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; | ||||
|   if (s == "TRYLAMBDA") return LevenbergMarquardtParams::TRYLAMBDA; | ||||
|   if (s == "TRYCONFIG") return LevenbergMarquardtParams::TRYCONFIG; | ||||
|   if (s == "TRYDELTA") return LevenbergMarquardtParams::TRYDELTA; | ||||
|   if (s == "DAMPED") return LevenbergMarquardtParams::DAMPED; | ||||
| 
 | ||||
|   /* default is silent */ | ||||
|   return LevenbergMarquardtParams::SILENT; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| std::string LevenbergMarquardtParams::verbosityLMTranslator(VerbosityLM value) const { | ||||
|   std::string s; | ||||
|   switch (value) { | ||||
|   case LevenbergMarquardtParams::SILENT:    s = "SILENT" ;     break; | ||||
|   case LevenbergMarquardtParams::LAMBDA:    s = "LAMBDA" ;     break; | ||||
|   case LevenbergMarquardtParams::TRYLAMBDA: s = "TRYLAMBDA" ;  break; | ||||
|   case LevenbergMarquardtParams::TRYCONFIG: s = "TRYCONFIG" ;  break; | ||||
|   case LevenbergMarquardtParams::TRYDELTA:  s = "TRYDELTA" ;   break; | ||||
|   case LevenbergMarquardtParams::DAMPED:    s = "DAMPED" ;     break; | ||||
|   default:                                  s = "UNDEFINED" ;  break; | ||||
|   } | ||||
|   return s; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void LevenbergMarquardtParams::print(const std::string& str) const { | ||||
|   SuccessiveLinearizationParams::print(str); | ||||
|   std::cout << "              lambdaInitial: " << lambdaInitial << "\n"; | ||||
|   std::cout << "               lambdaFactor: " << lambdaFactor << "\n"; | ||||
|   std::cout << "           lambdaUpperBound: " << lambdaUpperBound << "\n"; | ||||
|   std::cout << "                verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; | ||||
|   std::cout.flush(); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void LevenbergMarquardtOptimizer::iterate() { | ||||
| 
 | ||||
|  |  | |||
|  | @ -53,15 +53,20 @@ public: | |||
|     lambdaInitial(initial), lambdaFactor(factor), lambdaUpperBound(bound), verbosityLM(VerbosityLM(verbose)) {} | ||||
| 
 | ||||
|   virtual ~LevenbergMarquardtParams() {} | ||||
|   virtual void print(const std::string& str = "") const; | ||||
| 
 | ||||
|   virtual void print(const std::string& str = "") const { | ||||
|     SuccessiveLinearizationParams::print(str); | ||||
|     std::cout << "              lambdaInitial: " << lambdaInitial << "\n"; | ||||
|     std::cout << "               lambdaFactor: " << lambdaFactor << "\n"; | ||||
|     std::cout << "           lambdaUpperBound: " << lambdaUpperBound << "\n"; | ||||
|     std::cout << "                verbosityLM: " << verbosityLM << "\n"; | ||||
|     std::cout.flush(); | ||||
|   } | ||||
|   inline double getlambdaInitial() const { return lambdaInitial; } | ||||
|   inline double getlambdaFactor() const { return lambdaFactor; } | ||||
|   inline double getlambdaUpperBound() const { return lambdaUpperBound; } | ||||
|   inline std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM); } | ||||
| 
 | ||||
|   inline void setlambdaInitial(double value) { lambdaInitial = value; } | ||||
|   inline void setlambdaFactor(double value) { lambdaFactor = value; } | ||||
|   inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } | ||||
|   inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } | ||||
| 
 | ||||
|   VerbosityLM verbosityLMTranslator(const std::string &s) const; | ||||
|   std::string verbosityLMTranslator(VerbosityLM value) const; | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  |  | |||
|  | @ -19,11 +19,49 @@ | |||
| #include <iostream> | ||||
| #include <iomanip> | ||||
| #include <gtsam/nonlinear/NonlinearOptimizer.h> | ||||
| 
 | ||||
| #include <boost/algorithm/string.hpp> | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator(const std::string &src) const { | ||||
|   std::string s = src;  boost::algorithm::to_upper(s); | ||||
|   if (s == "SILENT") return NonlinearOptimizerParams::SILENT; | ||||
|   if (s == "ERROR") return NonlinearOptimizerParams::ERROR; | ||||
|   if (s == "VALUES") return NonlinearOptimizerParams::VALUES; | ||||
|   if (s == "DELTA") return NonlinearOptimizerParams::DELTA; | ||||
|   if (s == "LINEAR") return NonlinearOptimizerParams::LINEAR; | ||||
| 
 | ||||
|   /* default is silent */ | ||||
|   return NonlinearOptimizerParams::SILENT; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| std::string NonlinearOptimizerParams::verbosityTranslator(Verbosity value) const { | ||||
|   std::string s; | ||||
|   switch (value) { | ||||
|   case NonlinearOptimizerParams::SILENT:  s = "SILENT"; break; | ||||
|   case NonlinearOptimizerParams::ERROR:   s = "ERROR"; break; | ||||
|   case NonlinearOptimizerParams::VALUES:  s = "VALUES"; break; | ||||
|   case NonlinearOptimizerParams::DELTA:   s = "DELTA"; break; | ||||
|   case NonlinearOptimizerParams::LINEAR:  s = "LINEAR"; break; | ||||
|   default:                                s = "UNDEFINED"; break; | ||||
|   } | ||||
|   return s; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void NonlinearOptimizerParams::print(const std::string& str) const { | ||||
|   std::cout << str << "\n"; | ||||
|   std::cout << "relative decrease threshold: " << relativeErrorTol << "\n"; | ||||
|   std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n"; | ||||
|   std::cout << "      total error threshold: " << errorTol << "\n"; | ||||
|   std::cout << "         maximum iterations: " << maxIterations << "\n"; | ||||
|   std::cout << "                  verbosity: " << verbosityTranslator(verbosity) << "\n"; | ||||
|   std::cout.flush(); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void NonlinearOptimizer::defaultOptimize() { | ||||
| 
 | ||||
|  |  | |||
|  | @ -48,18 +48,24 @@ public: | |||
|     maxIterations(100.0), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), | ||||
|     errorTol(0.0), verbosity(SILENT) {} | ||||
| 
 | ||||
|   virtual void print(const std::string& str = "") const { | ||||
|     std::cout << str << "\n"; | ||||
|     std::cout << "relative decrease threshold: " << relativeErrorTol << "\n"; | ||||
|     std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n"; | ||||
|     std::cout << "      total error threshold: " << errorTol << "\n"; | ||||
|     std::cout << "         maximum iterations: " << maxIterations << "\n"; | ||||
|     std::cout << "            verbosity level: " << verbosity << std::endl; | ||||
|   } | ||||
| 
 | ||||
|   virtual ~NonlinearOptimizerParams() {} | ||||
| }; | ||||
|   virtual void print(const std::string& str = "") const ; | ||||
| 
 | ||||
|   inline double getMaxIterations() const { return maxIterations; } | ||||
|   inline double getRelativeErrorTol() const { return relativeErrorTol; } | ||||
|   inline double getAbsoluteErrorTol() const { return absoluteErrorTol; } | ||||
|   inline double getErrorTol() const { return errorTol; } | ||||
|   inline std::string getVerbosity() const { return verbosityTranslator(verbosity); } | ||||
| 
 | ||||
|   inline void setMaxIterations(double value) { maxIterations = value; } | ||||
|   inline void setRelativeErrorTol(double value) { relativeErrorTol = value; } | ||||
|   inline void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } | ||||
|   inline void setErrorTol(double value) { errorTol  = value ; } | ||||
|   inline void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); } | ||||
| 
 | ||||
|   Verbosity verbosityTranslator(const std::string &s) const; | ||||
|   std::string verbosityTranslator(Verbosity value) const; | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * Base class for a nonlinear optimization state, including the current estimate | ||||
|  |  | |||
|  | @ -61,14 +61,14 @@ for j=1:size(truth.points,2) | |||
| end | ||||
| initialEstimate.print(sprintf('\nInitial estimate:\n  ')); | ||||
| 
 | ||||
| %% One-shot Optimize using Levenberg-Marquardt optimization with an ordering from colamd | ||||
| %result = graph.optimize(initialEstimate,1); | ||||
| %result.print(sprintf('\nFinal result:\n  ')); | ||||
| 
 | ||||
| %% Fine grain optimization, allowing user to iterate step by step | ||||
| 
 | ||||
| parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 2); | ||||
| parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0); | ||||
| parameters.setlambdaInitial(1.0); | ||||
| parameters.setVerbosityLM('trylambda'); | ||||
| 
 | ||||
| optimizer = graph.optimizer(initialEstimate, parameters); | ||||
| 
 | ||||
| for i=1:5 | ||||
|     optimizer.iterate(); | ||||
| end | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue