diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index c7561e01a..38b9c2205 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -101,6 +101,26 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + std::pair::BayesNetType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialSequential( + const std::vector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const + { + if(variableIndex) { + gttic(eliminatePartialSequential); + // Compute full ordering + Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables); + + // Split off the part of the ordering for the variables being eliminated + Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); + return eliminatePartialSequential(ordering, function, variableIndex); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialSequential(variables, function, VariableIndex(asDerived())); + } + } + /* ************************************************************************* */ template std::pair::BayesTreeType>, boost::shared_ptr > @@ -231,4 +251,31 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + boost::shared_ptr + EliminateableFactorGraph::marginal( + const std::vector& variables, + const Eliminate& function = EliminationTraits::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const + { + if(variableIndex) + { + // Compute a total ordering for all variables + Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables); + + // Split out the part for the marginalized variables + Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); + + // Eliminate and return the remaining factor graph + return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second; + } + else + { + // If no variable index is provided, compute one and call this function again + return marginal(variables, function, VariableIndex(asDerived())); + } + } + + } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index b3f1bf7f0..37ae6a27c 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -217,6 +217,12 @@ namespace gtsam { const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; + /** Compute the marginal factor graph of the requested variables. */ + boost::shared_ptr marginal( + const std::vector& variables, + const Eliminate& function = EliminationTraits::DefaultEliminate, + OptionalVariableIndex variableIndex = boost::none) const; + private: // Access the derived factor graph class