Removed variant as it did not play well with the python wrapper.

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -235,7 +235,7 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
-    OrderingKeyVectorVariant variables,
+    const Ordering& variables,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(!variableIndex) {
@@ -245,16 +245,12 @@ namespace gtsam {
     } else {
       // No ordering was provided for the marginalized variables, so order them using constrained
       // COLAMD.
-      bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != nullptr);
+      constexpr bool forceOrder = true;
-      const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
-                                                 ? get_pointer(std::get<const OrderingConstRef>(variables))
-                                                 : get_pointer(std::get<const KeyVectorConstRef>(variables));
-
       Ordering totalOrdering =
-        Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
+        Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
 
       // Split up ordering
-      const size_t nVars = variablesOrOrdering->size();
+      const size_t nVars = variables.size();
       Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
       Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
 
@@ -267,7 +263,35 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
-    OrderingKeyVectorVariant variables,
+    const KeyVector& variables,
+    const Eliminate& function, OptionalVariableIndex variableIndex) const
+  {
+    if(!variableIndex) {
+      // If no variable index is provided, compute one and call this function again
+      VariableIndex index(asDerived());
+      return marginalMultifrontalBayesNet(variables, function, std::cref(index));
+    } else {
+      // No ordering was provided for the marginalized variables, so order them using constrained
+      // COLAMD.
+      const constexpr bool forceOrder = false;
+      Ordering totalOrdering =
+        Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
+
+      // Split up ordering
+      const size_t nVars = variables.size();
+      Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
+      Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
+
+      // Call this function again with the computed orderings
+      return marginalMultifrontalBayesNet(std::cref(marginalVarsOrdering), marginalizationOrdering, function, variableIndex);
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
+    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
+    const Ordering& variables,
     const Ordering& marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
@@ -282,26 +306,41 @@ namespace gtsam {
       const auto [bayesTree, factorGraph] =
         eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
 
-      if(std::get_if<const OrderingConstRef>(&variables))
-      {
-        const Ordering* varsAsOrdering = get_pointer(std::get<const OrderingConstRef>(variables));
       // An ordering was also provided for the unmarginalized variables, so we can also
       // eliminate them in the order requested.
-        return factorGraph->eliminateSequential(*varsAsOrdering, function);
+      return factorGraph->eliminateSequential(variables, function);
     }
-      else
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
+    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
+    const KeyVector& variables,
+    const Ordering& marginalizedVariableOrdering,
+    const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
+    if(!variableIndex) {
+      // If no variable index is provided, compute one and call this function again
+      VariableIndex index(asDerived());
+      return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
+    } else {
+      gttic(marginalMultifrontalBayesNet);
+      // An ordering was provided for the marginalized variables, so we can first eliminate them
+      // in the order requested.
+      const auto [bayesTree, factorGraph] =
+        eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
+
       // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
       return factorGraph->eliminateSequential(Ordering::COLAMD, function);
     }
   }
-  }
 
   /* ************************************************************************* */
   template<class FACTORGRAPH>
   std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
-    OrderingKeyVectorVariant variables,
+    const Ordering& variables,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(!variableIndex) {
@@ -311,16 +350,12 @@ namespace gtsam {
     } else {
       // No ordering was provided for the marginalized variables, so order them using constrained
       // COLAMD.
-      bool unmarginalizedAreOrdered = (std::get_if<const OrderingConstRef>(&variables) != 0);
+      constexpr bool forceOrder = true;
-      const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered
-                                                 ? get_pointer(std::get<const OrderingConstRef>(variables))
-                                                 : get_pointer(std::get<const KeyVectorConstRef>(variables));
-
       Ordering totalOrdering =
-        Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered);
+        Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
 
       // Split up ordering
-      const size_t nVars = variablesOrOrdering->size();
+      const size_t nVars = variables.size();
       Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
       Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
 
@@ -333,7 +368,35 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
     EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
-    OrderingKeyVectorVariant variables,
+    const KeyVector& variables,
+    const Eliminate& function, OptionalVariableIndex variableIndex) const
+  {
+    if(!variableIndex) {
+      // If no variable index is provided, compute one and call this function again
+      VariableIndex computedVariableIndex(asDerived());
+      return marginalMultifrontalBayesTree(variables, function, std::cref(computedVariableIndex));
+    } else {
+      // No ordering was provided for the marginalized variables, so order them using constrained
+      // COLAMD.
+      constexpr bool forceOrder = false;
+      Ordering totalOrdering =
+        Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
+
+      // Split up ordering
+      const size_t nVars = variables.size();
+      Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
+      Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
+
+      // Call this function again with the computed orderings
+      return marginalMultifrontalBayesTree(std::cref(marginalVarsOrdering), marginalizationOrdering, function, variableIndex);
+    }
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
+    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
+    const Ordering& variables,
     const Ordering& marginalizedVariableOrdering,
     const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
@@ -348,20 +411,35 @@ namespace gtsam {
       const auto [bayesTree, factorGraph] =
         eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
 
-      if(std::get_if<const OrderingConstRef>(&variables))
-      {
-        const Ordering* varsAsOrdering = get_pointer(std::get<const OrderingConstRef>(variables));
       // An ordering was also provided for the unmarginalized variables, so we can also
       // eliminate them in the order requested.
-        return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
+      return factorGraph->eliminateMultifrontal(variables, function);
     }
-      else
+  }
+
+  /* ************************************************************************* */
+  template<class FACTORGRAPH>
+  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
+    EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
+    const KeyVector& variables,
+    const Ordering& marginalizedVariableOrdering,
+    const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
+    if(!variableIndex) {
+      // If no variable index is provided, compute one and call this function again
+      VariableIndex computedVariableIndex(asDerived());
+      return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, std::cref(computedVariableIndex));
+    } else {
+      gttic(marginalMultifrontalBayesTree);
+      // An ordering was provided for the marginalized variables, so we can first eliminate them
+      // in the order requested.
+      const auto [bayesTree, factorGraph] =
+        eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
+
       // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
       return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
     }
   }
-  }
 
   /* ************************************************************************* */
   template<class FACTORGRAPH>

gtsam/inference/EliminateableFactorGraph.h

@@ -22,20 +22,11 @@
 #include <cstddef>
 #include <functional>
 #include <optional>
-#include <variant>
 
 #include <gtsam/inference/Ordering.h>
 #include <gtsam/inference/VariableIndex.h>
 
 namespace gtsam {
-  // Creating an alias for the variant type since it is verbose
-  template <typename T>
-    using ref_wrap = std::reference_wrapper<T>;
-  using OrderingConstRef = std::reference_wrapper<const Ordering>;
-  using KeyVectorConstRef = std::reference_wrapper<const KeyVector>;
-  using OrderingKeyVectorVariant = 
-    std::variant<const OrderingConstRef, const KeyVectorConstRef>;
-
   /// Traits class for eliminateable factor graphs, specifies the types that result from
   /// elimination, etc.  This must be defined for each factor graph that inherits from
   /// EliminateableFactorGraph.
@@ -224,52 +215,89 @@ namespace gtsam {
 
     /** Compute the marginal of the requested variables and return the result as a Bayes net.  Uses
      *  COLAMD marginalization ordering by default
-     *  @param variables Determines the variables whose marginal to compute, if provided as an
+     *  @param variables Determines the *ordered* variables whose marginal to compute, 
-     *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
+     *         will be ordered in the returned BayesNet.
-     *         as a KeyVector they will be ordered using constrained COLAMD.
+     *  @param function Optional dense elimination function.
-     *  @param function Optional dense elimination function, if not provided the default will be
-     *         used.
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
-     *         provided one will be computed. */
+     *         provided one will be computed. 
+     */
     std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
-      OrderingKeyVectorVariant variables,
+      const Ordering& variables,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = {}) const;
+
+    /** Compute the marginal of the requested variables and return the result as a Bayes net.  Uses
+     *  COLAMD marginalization ordering by default
+     *  @param variables Determines the variables whose marginal to compute,
+     *         will be ordered using constrained COLAMD.
+     *  @param function Optional dense elimination function.
+     *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
+     *         provided one will be computed. 
+     */
+    std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = {}) const;
 
     /** Compute the marginal of the requested variables and return the result as a Bayes net.
-     *  @param variables Determines the variables whose marginal to compute, if provided as an
+     *  @param variables Determines the *ordered* variables whose marginal to compute, 
-     *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
+     *         will be ordered in the returned BayesNet.
-     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
      *         i.e. all variables not in \c variables.
-     *  @param function Optional dense elimination function, if not provided the default will be
+     *  @param function Optional dense elimination function.
-     *         used.
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
-     *         provided one will be computed. */
+     *         provided one will be computed.
+     */
     std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
-      OrderingKeyVectorVariant variables,
+      const Ordering& variables,
+      const Ordering& marginalizedVariableOrdering,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = {}) const;
+
+    /** Compute the marginal of the requested variables and return the result as a Bayes net.
+     *  @param variables Determines the variables whose marginal to compute,
+     *         will be ordered using constrained COLAMD.
+     *  @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
+     *         i.e. all variables not in \c variables.
+     *  @param function Optional dense elimination function.
+     *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
+     *         provided one will be computed.
+     */
+    std::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
+      const KeyVector& variables,
       const Ordering& marginalizedVariableOrdering,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = {}) const;
 
     /** Compute the marginal of the requested variables and return the result as a Bayes tree.  Uses
      *  COLAMD marginalization order by default
-     *  @param variables Determines the variables whose marginal to compute, if provided as an
+     *  @param variables Determines the *ordered* variables whose marginal to compute, 
-     *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
+     *         will be ordered in the returned BayesNet.
-     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param function Optional dense elimination function, if not provided the default will be
      *         used.
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
-      OrderingKeyVectorVariant variables,
+      const Ordering& variables,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = {}) const;
+
+    /** Compute the marginal of the requested variables and return the result as a Bayes tree.  Uses
+     *  COLAMD marginalization order by default
+     *  @param variables Determines the variables whose marginal to compute, 
+     *         will be ordered using constrained COLAMD.
+     *  @param function Optional dense elimination function, if not provided the default will be
+     *         used.
+     *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
+     *         provided one will be computed. */
+    std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
+      const KeyVector& variables,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = {}) const;
 
     /** Compute the marginal of the requested variables and return the result as a Bayes tree.
-     *  @param variables Determines the variables whose marginal to compute, if provided as an
+     *  @param variables Determines the *ordered* variables whose marginal to compute, 
-     *         Ordering they will be ordered in the returned BayesNet as specified, and if provided
+     *         will be ordered in the returned BayesNet.
-     *         as a KeyVector they will be ordered using constrained COLAMD.
      *  @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
      *         i.e. all variables not in \c variables.
      *  @param function Optional dense elimination function, if not provided the default will be
@@ -277,7 +305,22 @@ namespace gtsam {
      *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
      *         provided one will be computed. */
     std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
-      OrderingKeyVectorVariant variables,
+      const Ordering& variables,
+      const Ordering& marginalizedVariableOrdering,
+      const Eliminate& function = EliminationTraitsType::DefaultEliminate,
+      OptionalVariableIndex variableIndex = {}) const;
+
+    /** Compute the marginal of the requested variables and return the result as a Bayes tree.
+     *  @param variables Determines the variables whose marginal to compute, 
+     *         will be ordered using constrained COLAMD.
+     *  @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
+     *         i.e. all variables not in \c variables.
+     *  @param function Optional dense elimination function, if not provided the default will be
+     *         used.
+     *  @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
+     *         provided one will be computed. */
+    std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
+      const KeyVector& variables,
       const Ordering& marginalizedVariableOrdering,
       const Eliminate& function = EliminationTraitsType::DefaultEliminate,
       OptionalVariableIndex variableIndex = {}) const;

gtsam/symbolic/tests/testSymbolicFactorGraph.cpp

@@ -124,15 +124,40 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
 }
 
 /* ************************************************************************* */
-TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrdering) {
-  auto expectedBayesNet =
+  auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({1, 2, 3})({2, 3})({3});
-      SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional(
-          1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3));
 
-  auto ordering = Ordering{0,1,2,3};
+  SymbolicBayesNet actual =
-  SymbolicBayesNet actual1 =
+      *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3});
-      *simpleTestGraph2.marginalMultifrontalBayesNet(std::cref(ordering));
+  EXPECT(assert_equal(expectedBayesNet, actual));
-  EXPECT(assert_equal(expectedBayesNet, actual1));
+}
+
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVector) {
+  auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({2, 1, 3})({1, 3})({3});
+
+  SymbolicBayesNet actual =
+      *simpleTestGraph2.marginalMultifrontalBayesNet(KeyVector{0, 1, 2, 3});
+  EXPECT(assert_equal(expectedBayesNet, actual));
+}
+
+TEST_UNSAFE(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrderingPlus) {
+  auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional{0, 3})({3});
+
+  const Ordering orderedVariables{0, 3},
+      marginalizedVariableOrdering{1, 2, 4, 5};
+  SymbolicBayesNet actual = *simpleTestGraph2.marginalMultifrontalBayesNet(
+      orderedVariables, marginalizedVariableOrdering);
+  EXPECT(assert_equal(expectedBayesNet, actual));
+}
+
+TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetKeyVectorPlus) {
+  auto expectedBayesNet = SymbolicBayesNet({0, 1, 3})({3, 1})({1});
+
+  const KeyVector variables{0, 1, 3};
+  const Ordering marginalizedVariableOrdering{2, 4, 5};
+  SymbolicBayesNet actual = *simpleTestGraph2.marginalMultifrontalBayesNet(
+      variables, marginalizedVariableOrdering);
+  EXPECT(assert_equal(expectedBayesNet, actual));
 }
 
 /* ************************************************************************* */