diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 5673379cd..bbf46d08a 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -235,7 +235,7 @@ namespace gtsam { template std::shared_ptr::BayesNetType> EliminateableFactorGraph::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(&variables) != nullptr); - const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered - ? get_pointer(std::get(variables)) - : get_pointer(std::get(variables)); - + constexpr bool forceOrder = true; 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 std::shared_ptr::BayesNetType> EliminateableFactorGraph::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 + std::shared_ptr::BayesNetType> + EliminateableFactorGraph::marginalMultifrontalBayesNet( + const Ordering& variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -282,18 +306,33 @@ namespace gtsam { const auto [bayesTree, factorGraph] = eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); - if(std::get_if(&variables)) - { - const Ordering* varsAsOrdering = get_pointer(std::get(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); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(Ordering::COLAMD, function); - } + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateSequential(variables, function); + } + } + + /* ************************************************************************* */ + template + std::shared_ptr::BayesNetType> + EliminateableFactorGraph::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); } } @@ -301,7 +340,7 @@ namespace gtsam { template std::shared_ptr::BayesTreeType> EliminateableFactorGraph::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(&variables) != 0); - const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered - ? get_pointer(std::get(variables)) - : get_pointer(std::get(variables)); - + constexpr bool forceOrder = true; 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 std::shared_ptr::BayesTreeType> EliminateableFactorGraph::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 + std::shared_ptr::BayesTreeType> + EliminateableFactorGraph::marginalMultifrontalBayesTree( + const Ordering& variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -348,18 +411,33 @@ namespace gtsam { const auto [bayesTree, factorGraph] = eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); - if(std::get_if(&variables)) - { - const Ordering* varsAsOrdering = get_pointer(std::get(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); - } - else - { - // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function); - } + // An ordering was also provided for the unmarginalized variables, so we can also + // eliminate them in the order requested. + return factorGraph->eliminateMultifrontal(variables, function); + } + } + + /* ************************************************************************* */ + template + std::shared_ptr::BayesTreeType> + EliminateableFactorGraph::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); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 777c0f505..858bbb61f 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -22,20 +22,11 @@ #include #include #include -#include #include #include namespace gtsam { - // Creating an alias for the variant type since it is verbose - template - using ref_wrap = std::reference_wrapper; - using OrderingConstRef = std::reference_wrapper; - using KeyVectorConstRef = std::reference_wrapper; - using OrderingKeyVectorVariant = - std::variant; - /// 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 - * Ordering they will be ordered in the returned BayesNet as specified, and if provided - * 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 variables Determines the *ordered* variables whose marginal to compute, + * will be ordered in the returned BayesNet. + * @param function Optional dense elimination function. * @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 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 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 - * Ordering they will be ordered in the returned BayesNet as specified, and if provided - * as a KeyVector they will be ordered using constrained COLAMD. + * @param variables Determines the *ordered* variables whose marginal to compute, + * will be ordered in the returned BayesNet. * @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 function Optional dense elimination function. * @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 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 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 - * Ordering they will be ordered in the returned BayesNet as specified, and if provided - * as a KeyVector they will be ordered using constrained COLAMD. + * @param variables Determines the *ordered* variables whose marginal to compute, + * will be ordered in the returned BayesNet. * @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 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 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 - * Ordering they will be ordered in the returned BayesNet as specified, and if provided - * as a KeyVector they will be ordered using constrained COLAMD. + * @param variables Determines the *ordered* variables whose marginal to compute, + * will be ordered in the returned BayesNet. * @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 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 marginalMultifrontalBayesTree( + const KeyVector& variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 30a7bb943..e1294157b 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -124,15 +124,40 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { } /* ************************************************************************* */ -TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { - auto expectedBayesNet = - SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional( - 1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3)); +TEST(SymbolicFactorGraph, MarginalMultifrontalBayesNetOrdering) { + auto expectedBayesNet = SymbolicBayesNet({0, 1, 2})({1, 2, 3})({2, 3})({3}); - auto ordering = Ordering{0,1,2,3}; - SymbolicBayesNet actual1 = - *simpleTestGraph2.marginalMultifrontalBayesNet(std::cref(ordering)); - EXPECT(assert_equal(expectedBayesNet, actual1)); + SymbolicBayesNet actual = + *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3}); + EXPECT(assert_equal(expectedBayesNet, actual)); +} + +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)); } /* ************************************************************************* */