From 26dd94bdaf554c18c2f9027a6bb04d2a4448a744 Mon Sep 17 00:00:00 2001
From: Amado Antonini <amado.antonini@openspace.ai>
Date: Fri, 8 Jul 2022 12:33:47 -0400
Subject: [PATCH] Expose remaining attributes, add tests

---
 gtsam/nonlinear/nonlinear.i                   | 12 +++-
 python/gtsam/tests/test_NonlinearOptimizer.py | 60 ++++++++++++++++++-
 2 files changed, 68 insertions(+), 4 deletions(-)

diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 21470060c..913be1aed 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -553,6 +553,16 @@ 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);
@@ -561,7 +571,7 @@ virtual class GncParams {
   void setVerbosityGNC(const This::Verbosity value);
   void setKnownInliers(const std::vector<size_t>& knownIn);
   void setKnownOutliers(const std::vector<size_t>& knownOut);
-  void print(const string& str) const;
+  void print(const string& str = "") const;
   
   enum Verbosity {
     SILENT,
diff --git a/python/gtsam/tests/test_NonlinearOptimizer.py b/python/gtsam/tests/test_NonlinearOptimizer.py
index 37afd9e1c..47d41edb2 100644
--- a/python/gtsam/tests/test_NonlinearOptimizer.py
+++ b/python/gtsam/tests/test_NonlinearOptimizer.py
@@ -16,9 +16,10 @@ import unittest
 
 import gtsam
 from gtsam import (DoglegOptimizer, DoglegParams, DummyPreconditionerParameters,
-                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLMOptimizer,
-                   LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph,
-                   Ordering, PCGSolverParameters, Point2, PriorFactorPoint2, Values)
+                   GaussNewtonOptimizer, GaussNewtonParams, GncLMParams, GncLossType,
+                   GncLMOptimizer, LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
+                   NonlinearFactorGraph, Ordering, PCGSolverParameters, Point2,
+                   PriorFactorPoint2, Values)
 from gtsam.utils.test_case import GtsamTestCase
 
 KEY1 = 1
@@ -83,6 +84,59 @@ class TestScenario(GtsamTestCase):
         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
+        str(params)
+        # 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