diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index 19e2d76d4..7d99d7c52 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -28,92 +28,142 @@ namespace gtsam { - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver( - const FactorGraph& factorGraph) : - factors_(new FactorGraph(factorGraph)), - structure_(new VariableIndex(factorGraph)), - eliminationTree_(EliminationTree::Create(*factors_, *structure_)) { - } - - /* ************************************************************************* */ - template - GenericSequentialSolver::GenericSequentialSolver( - const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex) : - factors_(factorGraph), structure_(variableIndex), - eliminationTree_(EliminationTree::Create(*factors_, *structure_)) { - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::print(const std::string& s) const { - this->factors_->print(s + " factors:"); - this->structure_->print(s + " structure:\n"); - this->eliminationTree_->print(s + " etree:"); - } - - /* ************************************************************************* */ - template - bool GenericSequentialSolver::equals( - const GenericSequentialSolver& expected, double tol) const { - if (!this->factors_->equals(*expected.factors_, tol)) return false; - if (!this->structure_->equals(*expected.structure_, tol)) return false; - if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) return false; - return true; - } - - /* ************************************************************************* */ - template - void GenericSequentialSolver::replaceFactors( - const sharedFactorGraph& factorGraph) { - // Reset this shared pointer first to deallocate if possible - for big - // problems there may not be enough memory to store two copies. - eliminationTree_.reset(); - factors_ = factorGraph; - eliminationTree_ = EliminationTree::Create(*factors_, *structure_); - } - - /* ************************************************************************* */ - template - typename boost::shared_ptr > // - GenericSequentialSolver::eliminate(Eliminate function) const { - return eliminationTree_->eliminate(function); - } + /* ************************************************************************* */ + template + GenericSequentialSolver::GenericSequentialSolver( + const FactorGraph& factorGraph) : + factors_(new FactorGraph(factorGraph)), structure_( + new VariableIndex(factorGraph)), eliminationTree_( + EliminationTree::Create(*factors_, *structure_)) { + } /* ************************************************************************* */ template - typename BayesNet::shared_ptr // - GenericSequentialSolver::jointBayesNet( - const std::vector& js, Eliminate function) const { + GenericSequentialSolver::GenericSequentialSolver( + const sharedFactorGraph& factorGraph, + const boost::shared_ptr& variableIndex) : + factors_(factorGraph), structure_(variableIndex), eliminationTree_( + EliminationTree::Create(*factors_, *structure_)) { + } - // Compute a COLAMD permutation with the marginal variables constrained to the end. - Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js)); - Permutation::shared_ptr permutationInverse(permutation->inverse()); + /* ************************************************************************* */ + template + void GenericSequentialSolver::print(const std::string& s) const { + this->factors_->print(s + " factors:"); + this->structure_->print(s + " structure:\n"); + this->eliminationTree_->print(s + " etree:"); + } + + /* ************************************************************************* */ + template + bool GenericSequentialSolver::equals( + const GenericSequentialSolver& expected, double tol) const { + if (!this->factors_->equals(*expected.factors_, tol)) + return false; + if (!this->structure_->equals(*expected.structure_, tol)) + return false; + if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) + return false; + return true; + } + + /* ************************************************************************* */ + template + void GenericSequentialSolver::replaceFactors( + const sharedFactorGraph& factorGraph) { + // Reset this shared pointer first to deallocate if possible - for big + // problems there may not be enough memory to store two copies. + eliminationTree_.reset(); + factors_ = factorGraph; + eliminationTree_ = EliminationTree::Create(*factors_, *structure_); + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::eliminate(Eliminate function) const { + return eliminationTree_->eliminate(function); + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::eliminate(const Permutation& permutation, + Eliminate function, boost::optional nrToEliminate) const { + + // Create inverse permutation + Permutation::shared_ptr permutationInverse(permutation.inverse()); // Permute the factors - NOTE that this permutes the original factors, not // copies. Other parts of the code may hold shared_ptr's to these factors so // we must undo the permutation before returning. BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) factor->permuteWithInverse(*permutationInverse); + if (factor) + factor->permuteWithInverse(*permutationInverse); - // Eliminate all variables - typename BayesNet::shared_ptr - bayesNet(EliminationTree::Create(*factors_)->eliminate(function)); + // Eliminate using elimination tree provided + typename EliminationTree::shared_ptr etree; + if (nrToEliminate) { + VariableIndex structure(*factors_, *nrToEliminate); + etree = EliminationTree::Create(*factors_, structure); + } else + etree = EliminationTree::Create(*factors_); + sharedBayesNet bayesNet = etree->eliminate(function); // Undo the permutation on the original factors and on the structure. BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_) - if (factor) factor->permuteWithInverse(*permutation); - - // Get rid of conditionals on variables that we want to marginalize out - size_t nrMarginalizedOut = bayesNet->size()-js.size(); - for(int i=0;ipop_front(); + if (factor) + factor->permuteWithInverse(permutation); // Undo the permutation on the conditionals BOOST_FOREACH(const boost::shared_ptr& c, *bayesNet) - c->permuteWithInverse(*permutation); + c->permuteWithInverse(permutation); + + return bayesNet; + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::conditionalBayesNet( + const std::vector& js, size_t nrFrontals, + Eliminate function) const { + + // Compute a COLAMD permutation with the marginal variables constrained to the end. + // TODO in case of nrFrontals, the order of js has to be respected here ! + Permutation::shared_ptr permutation( + inference::PermutationCOLAMD(*structure_, js)); + + // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S) + size_t nrVariables = factors_->keys().size(); // TODO expensive! + size_t nrMarginalized = nrVariables - js.size(); + size_t nrToEliminate = nrMarginalized + nrFrontals; + sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate); + + // Get rid of conditionals on variables that we want to marginalize out + for (int i = 0; i < nrMarginalized; i++) + bayesNet->pop_front(); + + return bayesNet; + } + + /* ************************************************************************* */ + template + typename GenericSequentialSolver::sharedBayesNet // + GenericSequentialSolver::jointBayesNet(const std::vector& js, + Eliminate function) const { + + // Compute a COLAMD permutation with the marginal variables constrained to the end. + Permutation::shared_ptr permutation( + inference::PermutationCOLAMD(*structure_, js)); + + // Eliminate all variables + sharedBayesNet bayesNet = eliminate(*permutation, function); + + // Get rid of conditionals on variables that we want to marginalize out + size_t nrMarginalizedOut = bayesNet->size() - js.size(); + for (int i = 0; i < nrMarginalizedOut; i++) + bayesNet->pop_front(); return bayesNet; } @@ -125,22 +175,23 @@ namespace gtsam { const std::vector& js, Eliminate function) const { // Eliminate all variables - typename BayesNet::shared_ptr - bayesNet = jointBayesNet(js,function); + typename BayesNet::shared_ptr bayesNet = jointBayesNet(js, + function); return boost::make_shared >(*bayesNet); } - /* ************************************************************************* */ - template - typename boost::shared_ptr // - GenericSequentialSolver::marginalFactor(Index j, Eliminate function) const { - // Create a container for the one variable index - std::vector js(1); - js[0] = j; + /* ************************************************************************* */ + template + typename boost::shared_ptr // + GenericSequentialSolver::marginalFactor(Index j, + Eliminate function) const { + // Create a container for the one variable index + std::vector js(1); + js[0] = j; - // Call joint and return the only factor in the factor graph it returns - return (*this->jointFactorGraph(js, function))[0]; - } + // Call joint and return the only factor in the factor graph it returns + return (*this->jointFactorGraph(js, function))[0]; + } } // namespace gtsam diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 5299b1afe..5b4cea73c 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -18,112 +18,141 @@ #pragma once -#include -#include -#include #include #include -namespace gtsam { class VariableIndex; } -namespace gtsam { template class EliminationTree; } -namespace gtsam { template class FactorGraph; } -namespace gtsam { template class BayesNet; } +#include +#include + +#include +#include + +namespace gtsam { + class VariableIndex; + class Permutation; +} +namespace gtsam { + template class EliminationTree; +} +namespace gtsam { + template class FactorGraph; +} +namespace gtsam { + template class BayesNet; +} namespace gtsam { - /** - * This solver implements sequential variable elimination for factor graphs. - * Underlying this is a column elimination tree, see Gilbert 2001 BIT. - * - * The elimination ordering is "baked in" to the variable indices at this - * stage, i.e. elimination proceeds in order from '0'. - * - * This is not the most efficient algorithm we provide, most efficient is the - * MultifrontalSolver, which examines and uses the clique structure. - * However, sequential variable elimination is easier to understand so this is a good - * starting point to learn about these algorithms and our implementation. - * Additionally, the first step of MFQR is symbolic sequential elimination. - * \nosubgrouping - */ - template - class GenericSequentialSolver { + /** + * This solver implements sequential variable elimination for factor graphs. + * Underlying this is a column elimination tree, see Gilbert 2001 BIT. + * + * The elimination ordering is "baked in" to the variable indices at this + * stage, i.e. elimination proceeds in order from '0'. + * + * This is not the most efficient algorithm we provide, most efficient is the + * MultifrontalSolver, which examines and uses the clique structure. + * However, sequential variable elimination is easier to understand so this is a good + * starting point to learn about these algorithms and our implementation. + * Additionally, the first step of MFQR is symbolic sequential elimination. + * \nosubgrouping + */ + template + class GenericSequentialSolver { - protected: + protected: - typedef boost::shared_ptr > sharedFactorGraph; - typedef typename FACTOR::ConditionalType Conditional; + typedef boost::shared_ptr > sharedFactorGraph; + typedef typename FACTOR::ConditionalType Conditional; + typedef typename boost::shared_ptr > sharedBayesNet; typedef std::pair, boost::shared_ptr > EliminationResult; - typedef boost::function&, size_t)> Eliminate; + typedef boost::function< + EliminationResult(const FactorGraph&, size_t)> Eliminate; - /** Store the original factors for computing marginals - * TODO Frank says: really? Marginals should be computed from result. - */ - sharedFactorGraph factors_; + /** Store the original factors for computing marginals + * TODO Frank says: really? Marginals should be computed from result. + */ + sharedFactorGraph factors_; - /** Store column structure of the factor graph. Why? */ - boost::shared_ptr structure_; + /** Store column structure of the factor graph. Why? */ + boost::shared_ptr structure_; - /** Elimination tree that performs elimination */ - boost::shared_ptr > eliminationTree_; + /** Elimination tree that performs elimination */ + boost::shared_ptr > eliminationTree_; - /** concept checks */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) -// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) + /** concept checks */ + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + // GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) - public: + /** + * Eliminate in a different order, given a permutation + * If given a number of variables to eliminate, will only eliminate that many + */ + sharedBayesNet + eliminate(const Permutation& permutation, Eliminate function, + boost::optional nrToEliminate = boost::none) const; - /// @name Standard Constructors - /// @{ + public: - /** - * Construct the solver for a factor graph. This builds the elimination - * tree, which already does some of the work of elimination. - */ - GenericSequentialSolver(const FactorGraph& factorGraph); + /// @name Standard Constructors + /// @{ - /** - * Construct the solver with a shared pointer to a factor graph and to a - * VariableIndex. The solver will store these pointers, so this constructor - * is the fastest. - */ - GenericSequentialSolver( - const sharedFactorGraph& factorGraph, - const boost::shared_ptr& variableIndex); + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the work of elimination. + */ + GenericSequentialSolver(const FactorGraph& factorGraph); - /// @} - /// @name Testable - /// @{ + /** + * Construct the solver with a shared pointer to a factor graph and to a + * VariableIndex. The solver will store these pointers, so this constructor + * is the fastest. + */ + GenericSequentialSolver(const sharedFactorGraph& factorGraph, + const boost::shared_ptr& variableIndex); - /** Print to cout */ - void print(const std::string& name = "GenericSequentialSolver: ") const; + /// @} + /// @name Testable + /// @{ - /** Test whether is equal to another */ - bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; + /** Print to cout */ + void print(const std::string& name = "GenericSequentialSolver: ") const; - /// @} - /// @name Standard Interface - /// @{ + /** Test whether is equal to another */ + bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; - /** - * Replace the factor graph with a new one having the same structure. The - * This function can be used if the numerical part of the factors changes, - * such as during relinearization or adjusting of noise models. - */ - void replaceFactors(const sharedFactorGraph& factorGraph); + /// @} + /// @name Standard Interface + /// @{ - /** - * Eliminate the factor graph sequentially. Uses a column elimination tree - * to recursively eliminate. - */ - typename boost::shared_ptr > - eliminate(Eliminate function) const; + /** + * Replace the factor graph with a new one having the same structure. The + * This function can be used if the numerical part of the factors changes, + * such as during relinearization or adjusting of noise models. + */ + void replaceFactors(const sharedFactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + sharedBayesNet eliminate(Eliminate function) const; + + /** + * Compute a conditional density P(F|S) while marginalizing out variables J + * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) + * Returns the result as a Bayes net. + */ + sharedBayesNet + conditionalBayesNet(const std::vector& js, size_t nrFrontals, + Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. Returns the result as a Bayes net */ - typename BayesNet::shared_ptr - jointBayesNet(const std::vector& js, Eliminate function) const; + sharedBayesNet + jointBayesNet(const std::vector& js, Eliminate function) const; /** * Compute the marginal joint over a set of variables, by integrating out @@ -132,17 +161,19 @@ namespace gtsam { typename FactorGraph::shared_ptr jointFactorGraph(const std::vector& js, Eliminate function) const; - /** - * Compute the marginal Gaussian density over a variable, by integrating out - * all of the other variables. This function returns the result as a factor. - */ - typename boost::shared_ptr - marginalFactor(Index j, Eliminate function) const; + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + typename boost::shared_ptr + marginalFactor(Index j, Eliminate function) const; - /// @} + /// @} - }; // GenericSequentialSolver + } + ; +// GenericSequentialSolver -} // namespace gtsam +}// namespace gtsam #include diff --git a/gtsam/inference/SymbolicSequentialSolver.h b/gtsam/inference/SymbolicSequentialSolver.h index f023acbaf..5bff88430 100644 --- a/gtsam/inference/SymbolicSequentialSolver.h +++ b/gtsam/inference/SymbolicSequentialSolver.h @@ -55,6 +55,14 @@ namespace gtsam { SymbolicBayesNet::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); }; + /** + * Compute a conditional density P(F|S) while marginalizing out variables J + * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S) + * Returns the result as a Bayes net. + */ + SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector& js, size_t nrFrontals) const + { return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); }; + /** * Compute the marginal joint over a set of variables, by integrating out * all of the other variables. Returns the result as a Bayes net.