From d11833317f38a96b8c0fa779c78bc4ffc2f23f2a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 15:36:38 +0000 Subject: [PATCH] Adding elimination algorithms to factor graph eliminateable base class --- .../inference/EliminateableFactorGraph-inst.h | 75 +++++++++++++ gtsam/inference/EliminateableFactorGraph.h | 104 ++++++++++++++---- gtsam/inference/inferenceExceptions.h | 42 +++++++ 3 files changed, 199 insertions(+), 22 deletions(-) create mode 100644 gtsam/inference/EliminateableFactorGraph-inst.h create mode 100644 gtsam/inference/inferenceExceptions.h diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h new file mode 100644 index 000000000..5d45a796c --- /dev/null +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EliminateableFactorGraph.h + * @brief Variable elimination algorithms for factor graphs + * @author Richard Roberts + * @date Apr 21, 2013 + */ + +#pragma once + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + boost::shared_ptr + EliminateableFactorGraph::eliminateSequential( + const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const + { + // Do elimination + std::pair, boost::shared_ptr > result; + if(ordering) { + // Do elimination with given ordering + result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function); + } else { + // Compute ordering + OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD(); + result = EliminationTreeType(*this, variableIndex, colamdOrdering).eliminate(function); + } + + // If any factors are remaining, the ordering was incomplete + if(!result.second->empty()) + throw InconsistentEliminationRequested(); + + // Return the Bayes net + return result.first; + } + + /* ************************************************************************* */ + template + boost::shared_ptr + EliminateableFactorGraph::eliminateMultifrontal( + const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const + { + // Do elimination + std::pair, boost::shared_ptr > result; + if(ordering) { + // Do elimination with given ordering + result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function); + } else { + // Compute ordering + OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD(); + result = JunctionTreeType(*this, variableIndex, colamdOrdering).eliminate(function); + } + + // If any factors are remaining, the ordering was incomplete + if(!result.second->empty()) + throw InconsistentEliminationRequested(); + + // Return the Bayes tree + return result.first; + } +} diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index ad58b6c45..ab28014af 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -13,7 +13,7 @@ * @file EliminateableFactorGraph.h * @brief Variable elimination algorithms for factor graphs * @author Richard Roberts - * @date Oct 21, 2010 + * @date Apr 21, 2013 */ #pragma once @@ -22,21 +22,24 @@ #include #include +#include namespace gtsam { /** EliminateableFactorGraph is a base class for factor graphs that contains elimination * algorithms. Any factor graph holding eliminateable factors can derive from this class to - * expose functions for computing marginals, conditional marginals, doing full multifrontal and + * expose functions for computing marginals, conditional marginals, doing multifrontal and * sequential elimination, etc. */ - template + template class EliminateableFactorGraph { public: - typedef EliminateableFactorGraph This; + typedef EliminateableFactorGraph This; typedef DERIVED FactorGraphType; - typedef FACTOR FactorType; - typedef BAYESNET BayesNetType; - typedef BAYESTREE BayesTreeType; + typedef ELIMINATIONTREE EliminationTreeType; + typedef JUNCTIONTREE JunctionTreeType; + typedef typename EliminationTreeType::FactorType FactorType; + typedef typename EliminationTreeType::BayesNetType BayesNetType; + typedef typename JunctionTreeType::BayesTreeType BayesTreeType; typedef typename BayesNetType::ConditionalType ConditionalType; typedef boost::shared_ptr sharedFactor; typedef boost::shared_ptr sharedConditional; @@ -46,28 +49,85 @@ namespace gtsam { typedef boost::optional OptionalOrdering; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not - * provided, the ordering provided by COLAMD will be used. */ + * provided, the ordering provided by COLAMD will be used. + * + * Example - Full Cholesky elimination in COLAMD order: + * \code + * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); + * \endcode + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, myOrdering); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); + * \endcode + * */ boost::shared_ptr - eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none) const; + eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none, + const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not - * provided, the ordering provided by COLAMD will be used. */ - boost::shared_ptr - eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none) const; + * provided, the ordering provided by COLAMD will be used. + * + * Example - Full Cholesky elimination in COLAMD order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); + * \endcode + * + * Example - Full QR elimination in specified order: + * \code + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); + * \endcode + * + * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: + * \code + * VariableIndex varIndex(graph); // Build variable index + * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex); + * \endcode + * */ + boost::shared_ptr + eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none, + const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; - /** Do sequential elimination of some variables to produce a Bayes net and a remaining factor - * graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, where \f$ A = \f$ \c - * variables, \f$ X \f$ is all the variables in the factor graph, and \f$ B = X\backslash A - * \f$. */ + /** Do sequential elimination of some variables in the given \c ordering to produce a Bayes net + * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, + * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ + * B = X\backslash A \f$. */ std::pair, boost::shared_ptr > - eliminatePartialSequential(const Eliminate& function, OptionalOrdering ordering = boost::none); + eliminatePartialSequential(const Eliminate& function, const Ordering& ordering, + const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); - /** Do multifrontal elimination of some variables to produce a Bayes tree and a remaining factor - * graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, where \f$ A = \f$ \c - * variables, \f$ X \f$ is all the variables in the factor graph, and \f$ B = X\backslash A - * \f$. */ + /** 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) + * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the + * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > - eliminatePartialMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none); + eliminatePartialSequential(const Eliminate& function, const std::vector& variables, + const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + + /** 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) + * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the + * factor graph, and \f$ B = X\backslash A \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal(const Eliminate& function, const Ordering& ordering, + const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + + /** 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) + * \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and + * \f$ B = X\backslash A \f$. */ + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal(const Eliminate& function, const std::vector& ordering, + const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); + }; } diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h new file mode 100644 index 000000000..edd0e0aa5 --- /dev/null +++ b/gtsam/inference/inferenceExceptions.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file inferenceExceptions.h + * @brief Exceptions that may be thrown by inference algorithms + * @author Richard Roberts + * @date Apr 25, 2013 + */ +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** An inference algorithm was called with inconsistent arguments. The factor graph, ordering, or + * variable index were inconsistent with each other, or a full elimination routine was called + * with an ordering that does not include all of the variables. */ + class InconsistentEliminationRequested : public std::exception { + public: + InconsistentEliminationRequested() throw() {} + virtual ~InconsistentEliminationRequested() throw() {} + virtual const char* what() const throw() { + return + "An inference algorithm was called with inconsistent arguments. The\n" + "factor graph, ordering, or variable index were inconsistent with each\n" + "other, or a full elimination routine was called with an ordering that\n" + "does not include all of the variables."; + } + }; + +}