From 0d058100e57d187de3b7c4638addf21da1c8158b Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Wed, 3 Jan 2024 14:44:12 -0500
Subject: [PATCH] update wrapper for LM with Ordering parameter

---
 gtsam/hybrid/HybridGaussianFactorGraph.cpp |  2 ++
 gtsam/nonlinear/nonlinear.i                | 11 ++++++++---
 2 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
index d63f8b5ff..522dc626b 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp
@@ -333,6 +333,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
       const auto &factor = pair.second;
       if (!factor) return 1.0;  // TODO(dellaert): not loving this.
 
+      // Logspace version of:
       // exp(-factor->error(kEmpty)) / pair.first->normalizationConstant();
       return -factor->error(kEmpty) - pair.first->logNormalizationConstant();
     };
@@ -345,6 +346,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
     AlgebraicDecisionTree probabilities = DecisionTree<Key, double>(
         logProbabilities,
         [&max_log](const double x) { return exp(x - max_log); });
+    // probabilities.print("", DefaultKeyFormatter);
     probabilities = probabilities.normalize(probabilities.sum());
 
     return {
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
index 3f5fb1dd5..113f08e0f 100644
--- a/gtsam/nonlinear/nonlinear.i
+++ b/gtsam/nonlinear/nonlinear.i
@@ -378,17 +378,22 @@ typedef gtsam::GncOptimizer<gtsam::GncParams<gtsam::LevenbergMarquardtParams>> G
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
   LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph,
-                              const gtsam::Values& initialValues);
+                              const gtsam::Values& initialValues,
+                              const gtsam::LevenbergMarquardtParams& params =
+                                  gtsam::LevenbergMarquardtParams());
   LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph,
                               const gtsam::Values& initialValues,
-                              const gtsam::LevenbergMarquardtParams& params);
+                              const gtsam::Ordering& ordering,
+                              const gtsam::LevenbergMarquardtParams& params =
+                                  gtsam::LevenbergMarquardtParams());
+
   double lambda() const;
   void print(string s = "") const;
 };
 
 #include <gtsam/nonlinear/ISAM2.h>
 class ISAM2GaussNewtonParams {
-  ISAM2GaussNewtonParams();
+  ISAM2GaussNewtonParams(double _wildfireThreshold = 0.001);
 
   void print(string s = "") const;