Better optimization parameter wrapping, plus python test

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()

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 <gtsam/nonlinear/DoglegOptimizer.h>

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);}
   // @}
 };

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -54,47 +54,24 @@ public:
   }
   virtual void print(const std::string& str = "") const;
 
-  size_t getMaxIterations() const {
+  size_t getMaxIterations() const { return maxIterations; }
-    return maxIterations;
+  double getRelativeErrorTol() const { return relativeErrorTol; }
-  }
+  double getAbsoluteErrorTol() const { return absoluteErrorTol; }
-  double getRelativeErrorTol() const {
+  double getErrorTol() const { return errorTol; }
-    return relativeErrorTol;
+  std::string getVerbosity() const { return verbosityTranslator(verbosity); }
-  }
-  double getAbsoluteErrorTol() const {
-    return absoluteErrorTol;
-  }
-  double getErrorTol() const {
-    return errorTol;
-  }
-  std::string getVerbosity() const {
-    return verbosityTranslator(verbosity);
-  }
 
-  void setMaxIterations(int value) {
+  void setMaxIterations(int value) { maxIterations = value; }
-    maxIterations = value;
+  void setRelativeErrorTol(double value) { relativeErrorTol = value; }
-  }
+  void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
-  void setRelativeErrorTol(double value) {
+  void setErrorTol(double value) { errorTol = value; }
-    relativeErrorTol = value;
+  void setVerbosity(const std::string& src) {
-  }
-  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: