diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 47f314ed7..1c005e446 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -33,8 +33,8 @@ namespace gtsam { { if(ordering && variableIndex) { // Do elimination - std::pair, boost::shared_ptr > result = - ELIMINATIONTREE(asDerived(), *variableIndex, *ordering).eliminate(function); + std::pair, boost::shared_ptr > result + = ELIMINATIONTREE(asDerived(), *variableIndex, *ordering).eliminate(function); // If any factors are remaining, the ordering was incomplete if(!result.second->empty()) throw InconsistentEliminationRequested(); @@ -58,16 +58,14 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr - EliminateableFactorGraph:: - eliminateMultifrontal( - const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const + boost::shared_ptr + EliminateableFactorGraph + ::eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const { if(ordering && variableIndex) { - // Do elimination - std::pair, boost::shared_ptr > result; // Do elimination with given ordering - result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, *ordering)).eliminate(function); + std::pair, boost::shared_ptr > result + = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, *ordering)).eliminate(function); // If any factors are remaining, the ordering was incomplete if(!result.second->empty()) throw InconsistentEliminationRequested(); @@ -88,4 +86,39 @@ namespace gtsam { } } + /* ************************************************************************* */ + template + std::pair, boost::shared_ptr > + EliminateableFactorGraph + ::eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering, + OptionalVariableIndex variableIndex = boost::none) const + { + if(variableIndex) { + // Do elimination + return ELIMINATIONTREE(asDerived(), *variableIndex, ordering).eliminate(function); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialSequential(function, ordering, VariableIndexUnordered(asDerived())); + } + } + + /* ************************************************************************* */ + template + std::pair, boost::shared_ptr > + EliminateableFactorGraph + ::eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering, + OptionalVariableIndex variableIndex = boost::none) const + { + if(variableIndex) { + // Do elimination + return JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, ordering)).eliminate(function); + } else { + // If no variable index is provided, compute one and call this function again + return eliminatePartialMultifrontal(function, ordering, VariableIndexUnordered(asDerived())); + } + } + + } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index b37f4c2da..88176ad14 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -96,7 +96,7 @@ namespace gtsam { * B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering, - OptionalVariableIndex variableIndex = boost::none); + OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) @@ -104,7 +104,7 @@ namespace gtsam { * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialSequential(const Eliminate& function, const std::vector& variables, - OptionalVariableIndex variableIndex = boost::none); + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) @@ -112,7 +112,7 @@ namespace gtsam { * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering, - OptionalVariableIndex variableIndex = boost::none); + OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) @@ -120,7 +120,7 @@ namespace gtsam { * \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialMultifrontal(const Eliminate& function, const std::vector& variables, - OptionalVariableIndex variableIndex = boost::none); + OptionalVariableIndex variableIndex = boost::none) const; private: