From aee494a24f967f652aae6783df95601cf7bab2c2 Mon Sep 17 00:00:00 2001
From: Amado Antonini <amado.antonini@openspace.ai>
Date: Tue, 14 Jun 2022 17:07:29 -0400
Subject: [PATCH] Expose GNC params to python

---
 gtsam/nonlinear/GncOptimizer.h    |  8 ++++----
 gtsam/nonlinear/nonlinear.i       | 17 +++++++++++++++++
 python/gtsam/preamble/nonlinear.h |  3 ++-
 3 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index cc3fdaf34..69067a915 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -246,7 +246,9 @@ class GTSAM_EXPORT GncOptimizer {
       prev_cost = cost;
 
       // display info
-      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+      if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
+        std::cout << "iter: " << iter << std::endl;
+        std::cout << "mu: " << mu << std::endl;
         std::cout << "previous cost: " << prev_cost << std::endl;
         std::cout << "current cost: " << cost << std::endl;
       }
@@ -255,9 +257,7 @@ class GTSAM_EXPORT GncOptimizer {
     if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
       std::cout << "final iterations: " << iter << std::endl;
       std::cout << "final mu: " << mu << std::endl;
-      std::cout << "final weights: " << weights_ << std::endl;
-      std::cout << "previous cost: " << prev_cost << std::endl;
-      std::cout << "current cost: " << cost << std::endl;
+      std::cout << "final cost: " << cost << std::endl;
     }
     return result;
   }
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 033e5ced2..21470060c 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -544,11 +544,23 @@ 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();
+  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 setKnownInliers(const std::vector<size_t>& knownIn);
+  void setKnownOutliers(const std::vector<size_t>& knownOut);
   void print(const string& str) const;
   
   enum Verbosity {
@@ -597,6 +609,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();
 };
 
diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h
index a34e73058..34b864027 100644
--- a/python/gtsam/preamble/nonlinear.h
+++ b/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++.
- */
\ No newline at end of file
+ */
+#include <pybind11/stl.h>
\ No newline at end of file