From e4e43d5d16b9acb071cf970a82d3c40d4b135614 Mon Sep 17 00:00:00 2001
From: Kartik Arcot <kartik@verdantrobotics.com>
Date: Sat, 21 Jan 2023 12:32:25 -0800
Subject: [PATCH] fix defaultorderingfunc

---
 gtsam/discrete/DiscreteFactorGraph.h            | 4 ++--
 gtsam/hybrid/HybridGaussianFactorGraph.h        | 3 ++-
 gtsam/inference/EliminateableFactorGraph-inst.h | 4 ++--
 gtsam/linear/GaussianFactorGraph.h              | 4 ++--
 gtsam/symbolic/SymbolicFactorGraph.h            | 4 ++--
 5 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h
index fdfc2bc0d..4441b0df4 100644
--- a/gtsam/discrete/DiscreteFactorGraph.h
+++ b/gtsam/discrete/DiscreteFactorGraph.h
@@ -70,8 +70,8 @@ template<> struct EliminationTraits<DiscreteFactorGraph>
   /// The default ordering generation function
   static Ordering DefaultOrderingFunc(
       const FactorGraphType& graph,
-      const VariableIndex& variableIndex) {
-    return Ordering::Colamd(variableIndex);
+      std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
+    return Ordering::Colamd((*variableIndex).get());
   }
 };
 
diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h
index 432bf7c07..b516d0adc 100644
--- a/gtsam/hybrid/HybridGaussianFactorGraph.h
+++ b/gtsam/hybrid/HybridGaussianFactorGraph.h
@@ -27,6 +27,7 @@
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 
+#include <functional>
 #include <optional>
 
 namespace gtsam {
@@ -87,7 +88,7 @@ struct EliminationTraits<HybridGaussianFactorGraph> {
   /// The default ordering generation function
   static Ordering DefaultOrderingFunc(
       const FactorGraphType& graph,
-      const VariableIndex&) {
+      std::optional<std::reference_wrapper<const VariableIndex>>) {
     return HybridOrdering(graph);
   }
 };
diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h
index b7aa52bab..eb6d57067 100644
--- a/gtsam/inference/EliminateableFactorGraph-inst.h
+++ b/gtsam/inference/EliminateableFactorGraph-inst.h
@@ -52,7 +52,7 @@ namespace gtsam {
         return eliminateSequential(computedOrdering, function, variableIndex);
       } else {
         Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
-            asDerived(), (*variableIndex).get());
+            asDerived(), *variableIndex);
         return eliminateSequential(computedOrdering, function, variableIndex);
       }
     }
@@ -115,7 +115,7 @@ namespace gtsam {
         return eliminateMultifrontal(computedOrdering, function, variableIndex);
       } else {
         Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
-            asDerived(), (*variableIndex).get());
+            asDerived(), *variableIndex);
         return eliminateMultifrontal(computedOrdering, function, variableIndex);
       }
     }
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 65c22cd04..cacb95119 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -58,8 +58,8 @@ namespace gtsam {
     /// The default ordering generation function
     static Ordering DefaultOrderingFunc(
         const FactorGraphType& graph,
-        const VariableIndex& variableIndex) {
-      return Ordering::Colamd(variableIndex);
+        std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
+      return Ordering::Colamd((*variableIndex).get());
     }
   };
 
diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h
index ecb7c222b..219ba7bf5 100644
--- a/gtsam/symbolic/SymbolicFactorGraph.h
+++ b/gtsam/symbolic/SymbolicFactorGraph.h
@@ -49,8 +49,8 @@ namespace gtsam {
     /// The default ordering generation function
     static Ordering DefaultOrderingFunc(
         const FactorGraphType& graph,
-        const VariableIndex& variableIndex) {
-      return Ordering::Colamd(variableIndex);
+        std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
+      return Ordering::Colamd((*variableIndex).get());
     }
   };