Wrapped DoglegOptimizer and LevenbergMarquardtOptimizer, added class heirarchy to optimization parameters

gtsam.h

@@ -1130,32 +1130,46 @@ class Marginals {
 	Matrix marginalInformation(size_t variable) const;
 };
 
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+//*************************************************************************
-class LevenbergMarquardtParams {
+// Nonlinear optimizers
-  LevenbergMarquardtParams();
+//*************************************************************************
-  void print(string s) const;
 
-  size_t getMaxIterations() const;
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
-  double getRelativeErrorTol() const;
+virtual class NonlinearOptimizerParams {
-  double getAbsoluteErrorTol() const;
+	NonlinearOptimizerParams();
-  double getErrorTol() const;
+	void print(string s) const;
-  string getVerbosity() const;
 
-  void setMaxIterations(size_t value);
+	size_t getMaxIterations() const;
-  void setRelativeErrorTol(double value);
+	double getRelativeErrorTol() const;
-  void setAbsoluteErrorTol(double value);
+	double getAbsoluteErrorTol() const;
-  void setErrorTol(double value);
+	double getErrorTol() const;
-  void setVerbosity(string s);
+	string getVerbosity() const;
+
+	void setMaxIterations(size_t value);
+	void setRelativeErrorTol(double value);
+	void setAbsoluteErrorTol(double value);
+	void setErrorTol(double value);
+	void setVerbosity(string s);
+};
+
+#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
+virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams {
+  SuccessiveLinearizationParams();
 
   bool isMultifrontal() const;
   bool isSequential() const;
   bool isCholmod() const;
   bool isCG() const;
+};
 
-  double getlambdaInitial() const ;
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-  double getlambdaFactor() const ;
+virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams {
+	LevenbergMarquardtParams();
+
+  double getlambdaInitial() const;
+  double getlambdaFactor() const;
   double getlambdaUpperBound() const;
-  string getVerbosityLM() const ;
+  string getVerbosityLM() const;
 
   void setlambdaInitial(double value);
   void setlambdaFactor(double value);
@@ -1163,22 +1177,53 @@ class LevenbergMarquardtParams {
   void setVerbosityLM(string s);
 };
 
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+virtual class DoglegParams : gtsam::SuccessiveLinearizationParams {
+	DoglegParams();
+
+	double getDeltaInitial() const;
+	string getVerbosityDL() const;
+
+	void setDeltaInitial(double deltaInitial) const;
+	void setVerbosityDL(string verbosityDL) const;
+};
+
+virtual class NonlinearOptimizer {
+	gtsam::Values optimizeSafely();
+	double error() const;
+	int iterations() const;
+	gtsam::Values values() const;
+	void iterate() const;
+};
+
+virtual class DoglegOptimizer : gtsam::NonlinearOptimizer {
+	DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
+	DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params);
+	double getDelta() const;
+};
+
+virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
+	LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
+	LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params);
+	double lambda() const;
+};
+
 //*************************************************************************
 // Nonlinear factor types
 //*************************************************************************
-template<T = {gtsam::LieVector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
+template<T = {gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
 virtual class PriorFactor : gtsam::NonlinearFactor {
 	PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
 };
 
 
-template<T = {gtsam::LieVector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
+template<T = {gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
 virtual class BetweenFactor : gtsam::NonlinearFactor {
 	BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
 };
 
 
-template<T = {gtsam::LieVector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
+template<T = {gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera}>
 virtual class NonlinearEquality : gtsam::NonlinearFactor {
 	// Constructor - forces exact evaluation
 	NonlinearEquality(size_t j, const T& feasible);

gtsam/nonlinear/DoglegOptimizer.cpp

@@ -22,10 +22,33 @@
 #include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
+#include <boost/algorithm/string.hpp>
+
 using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+DoglegParams::VerbosityDL DoglegParams::verbosityDLTranslator(const std::string &verbosityDL) const {
+	std::string s = verbosityDL;  boost::algorithm::to_upper(s);
+	if (s == "SILENT") return DoglegParams::SILENT;
+	if (s == "VERBOSE") return DoglegParams::VERBOSE;
+
+	/* default is silent */
+	return DoglegParams::SILENT;
+}
+
+/* ************************************************************************* */
+std::string DoglegParams::verbosityDLTranslator(VerbosityDL verbosityDL) const {
+	std::string s;
+	switch (verbosityDL) {
+	case DoglegParams::SILENT:  s = "SILENT"; break;
+	case DoglegParams::VERBOSE: s = "VERBOSE"; break;
+	default:                    s = "UNDEFINED"; break;
+	}
+	return s;
+}
+
 /* ************************************************************************* */
 void DoglegOptimizer::iterate(void) {
 

gtsam/nonlinear/DoglegOptimizer.h

@@ -50,6 +50,15 @@ public:
     std::cout << "               deltaInitial: " << deltaInitial << "\n";
     std::cout.flush();
   }
+
+	double getDeltaInitial() const { return deltaInitial; }
+	std::string getVerbosityDL() const { return verbosityDLTranslator(verbosityDL); }
+
+	void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; }
+	void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); }
+
+	VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const;
+	std::string verbosityDLTranslator(VerbosityDL verbosityDL) const;
 };
 
 /**
@@ -128,7 +137,7 @@ public:
   const DoglegState& state() const { return state_; }
 
   /** Access the current trust region radius Delta */
-  double Delta() const { return state_.Delta; }
+  double getDelta() const { return state_.Delta; }
 
   /// @}