Expose GNC params to python

gtsam/nonlinear/GncParams.h

@@ -161,7 +161,7 @@ class GTSAM_EXPORT GncParams {
       std::cout << "knownInliers: " << knownInliers[i] << "\n";
     for (size_t i = 0; i < knownOutliers.size(); i++)
       std::cout << "knownOutliers: " << knownOutliers[i] << "\n";
-    baseOptimizerParams.print(str);
+    baseOptimizerParams.print("Base optimizer params: ");
   }
 };
 

gtsam/nonlinear/nonlinear.i

@@ -544,12 +544,34 @@ virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
 };
 
 #include <gtsam/nonlinear/GncParams.h>
+enum GncLossType {
+  GM /*Geman McClure*/,
+  TLS /*Truncated least squares*/
+};
+
 template<PARAMS>
 virtual class GncParams {
   GncParams(const PARAMS& baseOptimizerParams);
   GncParams();
+  BaseOptimizerParameters baseOptimizerParams;
+  GncLossType lossType;
+  size_t maxIterations;
+  double muStep;
+  double relativeCostTol;
+  double weightsTol;
+  Verbosity verbosity;
+  std::vector<size_t> knownInliers;
+  std::vector<size_t> knownOutliers;
+
+  void setLossType(const GncLossType type);
+  void setMaxIterations(const size_t maxIter);
+  void setMuStep(const double step);
+  void setRelativeCostTol(double value);
+  void setWeightsTol(double value);
   void setVerbosityGNC(const This::Verbosity value);
-  void print(const string& str) const;
+  void setKnownInliers(const std::vector<size_t>& knownIn);
+  void setKnownOutliers(const std::vector<size_t>& knownOut);
+  void print(const string& str = "GncParams: ") const;
 
   enum Verbosity {
     SILENT,
@@ -597,6 +619,11 @@ virtual class GncOptimizer {
   GncOptimizer(const gtsam::NonlinearFactorGraph& graph,
                const gtsam::Values& initialValues,
                const PARAMS& params);
+  void setInlierCostThresholds(const double inth);
+  const Vector& getInlierCostThresholds();
+  void setInlierCostThresholdsAtProbability(const double alpha);
+  void setWeights(const Vector w);
+  const Vector& getWeights();
   gtsam::Values optimize();
 };
 

python/gtsam/preamble/nonlinear.h

@@ -9,4 +9,5 @@
  * automatic STL binding, such that the raw objects can be accessed in Python.
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
- */
+ */
+#include <pybind11/stl.h>

python/gtsam/tests/test_NonlinearOptimizer.py

@@ -15,10 +15,12 @@
 import unittest
 
 import gtsam
-from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
-                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
-                   Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
+from gtsam import (
+    DoglegOptimizer, DoglegParams, DummyPreconditionerParameters, GaussNewtonOptimizer,
+    GaussNewtonParams, GncLMParams, GncLossType, GncLMOptimizer, LevenbergMarquardtOptimizer,
+    LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
+    PriorFactorPoint2, Values
+)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
@@ -27,7 +29,6 @@
 
 class TestScenario(GtsamTestCase):
     """Do trivial test with three optimizer variants."""
-
     def setUp(self):
         """Set up the optimization problem and ordering"""
         # create graph
@@ -83,16 +84,83 @@ def test_graduated_non_convexity(self):
         actual = GncLMOptimizer(self.fg, self.initial_values, gncParams).optimize()
         self.assertAlmostEqual(0, self.fg.error(actual))
 
+    def test_gnc_params(self):
+        base_params = LevenbergMarquardtParams()
+        # Test base params
+        for base_max_iters in (50, 100):
+            base_params.setMaxIterations(base_max_iters)
+            params = GncLMParams(base_params)
+            self.assertEqual(params.baseOptimizerParams.getMaxIterations(), base_max_iters)
+
+        # Test printing
+        params_str = str(params)
+        for s in (
+            "lossType",
+            "maxIterations",
+            "muStep",
+            "relativeCostTol",
+            "weightsTol",
+            "verbosity",
+        ):
+            self.assertTrue(s in params_str)
+
+        # Test each parameter
+        for loss_type in (GncLossType.TLS, GncLossType.GM):
+            params.setLossType(loss_type)  # Default is TLS
+            self.assertEqual(params.lossType, loss_type)
+        for max_iter in (1, 10, 100):
+            params.setMaxIterations(max_iter)
+            self.assertEqual(params.maxIterations, max_iter)
+        for mu_step in (1.1, 1.2, 1.5):
+            params.setMuStep(mu_step)
+            self.assertEqual(params.muStep, mu_step)
+        for rel_cost_tol in (1e-5, 1e-6, 1e-7):
+            params.setRelativeCostTol(rel_cost_tol)
+            self.assertEqual(params.relativeCostTol, rel_cost_tol)
+        for weights_tol in (1e-4, 1e-3, 1e-2):
+            params.setWeightsTol(weights_tol)
+            self.assertEqual(params.weightsTol, weights_tol)
+        for i in (0, 1, 2):
+            verb = GncLMParams.Verbosity(i)
+            params.setVerbosityGNC(verb)
+            self.assertEqual(params.verbosity, verb)
+        for inl in ([], [10], [0, 100]):
+            params.setKnownInliers(inl)
+            self.assertEqual(params.knownInliers, inl)
+            params.knownInliers = []
+        for out in ([], [1], [0, 10]):
+            params.setKnownInliers(out)
+            self.assertEqual(params.knownInliers, out)
+            params.knownInliers = []
+
+        # Test optimizer params
+        optimizer = GncLMOptimizer(self.fg, self.initial_values, params)
+        for ict_factor in (0.9, 1.1):
+            new_ict = ict_factor * optimizer.getInlierCostThresholds()
+            optimizer.setInlierCostThresholds(new_ict)
+            self.assertAlmostEqual(optimizer.getInlierCostThresholds(), new_ict)
+        for w_factor in (0.8, 0.9):
+            new_weights = w_factor * optimizer.getWeights()
+            optimizer.setWeights(new_weights)
+            self.assertAlmostEqual(optimizer.getWeights(), new_weights)
+        optimizer.setInlierCostThresholdsAtProbability(0.9)
+        w1 = optimizer.getInlierCostThresholds()
+        optimizer.setInlierCostThresholdsAtProbability(0.8)
+        w2 = optimizer.getInlierCostThresholds()
+        self.assertLess(w2, w1)
+
     def test_iteration_hook(self):
         # set up iteration hook to track some testable values
         iteration_count = 0
         final_error = 0
         final_values = None
+
         def iteration_hook(iter, error_before, error_after):
             nonlocal iteration_count, final_error, final_values
             iteration_count = iter
             final_error = error_after
             final_values = optimizer.values()
+
         # optimize
         params = LevenbergMarquardtParams.CeresDefaults()
         params.setOrdering(self.ordering)
@@ -104,5 +172,6 @@ def iteration_hook(iter, error_before, error_after):
         self.assertEqual(self.fg.error(actual), final_error)
         self.assertEqual(optimizer.iterations(), iteration_count)
 
+
 if __name__ == "__main__":
     unittest.main()