diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 1c005e446..67299ae64 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -24,11 +24,9 @@ namespace gtsam { /* ************************************************************************* */ - template - boost::shared_ptr - EliminateableFactorGraph:: - eliminateSequential( + template + boost::shared_ptr::BayesNetType> + EliminateableFactorGraph::eliminateSequential( const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const { if(ordering && variableIndex) { @@ -56,11 +54,10 @@ namespace gtsam { } /* ************************************************************************* */ - template - boost::shared_ptr - EliminateableFactorGraph - ::eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const + template + boost::shared_ptr::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const { if(ordering && variableIndex) { // Do elimination with given ordering @@ -87,12 +84,10 @@ namespace gtsam { } /* ************************************************************************* */ - template - std::pair, boost::shared_ptr > - EliminateableFactorGraph - ::eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering, - OptionalVariableIndex variableIndex = boost::none) const + template + std::pair::BayesNetType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialSequential( + const Eliminate& function, const OrderingUnordered& ordering, OptionalVariableIndex variableIndex) const { if(variableIndex) { // Do elimination @@ -104,12 +99,10 @@ namespace gtsam { } /* ************************************************************************* */ - template - std::pair, boost::shared_ptr > - EliminateableFactorGraph - ::eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering, - OptionalVariableIndex variableIndex = boost::none) const + template + std::pair::BayesTreeType>, boost::shared_ptr > + EliminateableFactorGraph::eliminatePartialMultifrontal( + const Eliminate& function, const OrderingUnordered& ordering, OptionalVariableIndex variableIndex) const { if(variableIndex) { // Do elimination diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 0ed6a375b..c79317eed 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -33,11 +33,16 @@ namespace gtsam { class EliminationTraits { // Template for deriving: - // typedef MyFactor FactorType; // Type of factors in factor graph (e.g. GaussianFactor, SymbolicFactor) - // typedef MyBayesNet BayesNetType; // Type of Bayes net from sequential elimination (e.g. GaussianBayesNet) - // typedef MyEliminationTree EliminationTreeType; // Type of elimination tree (e.g. GaussianEliminationTree) - // typedef MyBayesTree BayesTreeType; // Type of Bayes tree (e.g. GaussianBayesTree) - // typedef MyJunctionTree JunctionTreeType; // Type of Junction tree (e.g. GaussianJunctionTree) + // typedef MyFactor FactorType; ///< Type of factors in factor graph (e.g. GaussianFactor) + // typedef MyConditional ConditionalType; ///< Type of conditionals from elimination (e.g. GaussianConditional) + // typedef MyBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination (e.g. GaussianBayesNet) + // typedef MyEliminationTree EliminationTreeType; ///< Type of elimination tree (e.g. GaussianEliminationTree) + // typedef MyBayesTree BayesTreeType; ///< Type of Bayes tree (e.g. GaussianBayesTree) + // typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree) + // static pair, shared_ptr + // DefaultEliminate( + // const std::vector >& factors, + // const std::vector& keys); ///< The default dense elimination function }; @@ -45,17 +50,44 @@ namespace gtsam { * algorithms. Any factor graph holding eliminateable factors can derive from this class to * expose functions for computing marginals, conditional marginals, doing multifrontal and * sequential elimination, etc. */ - template - class EliminateableFactorGraph { - public: - typedef EliminateableFactorGraph This; - typedef boost::optional OptionalOrdering; - typedef boost::optional OptionalVariableIndex; - typedef boost::function, boost::shared_ptr >( - std::vector >, std::vector)> - Eliminate; ///< Typedef for an eliminate subroutine + template + class EliminateableFactorGraph + { + private: + typedef EliminateableFactorGraph This; ///< Typedef to this class. + typedef FACTORGRAPH FactorGraphType; + public: + /// Base factor type stored in this graph + typedef typename EliminationTraits::FactorType FactorType; + + /// Conditional type stored in the Bayes net produced by elimination + typedef typename EliminationTraits::ConditionalType ConditionalType; + + /// Bayes net type produced by sequential elimination + typedef typename EliminationTraits::BayesNetType BayesNetType; + + /// Elimination tree type that can do sequential elimination of this graph + typedef typename EliminationTraits::EliminationTreeType EliminationTreeType; + + /// Bayes tree type produced by multifrontal elimination + typedef typename EliminationTraits::BayesTreeType BayesTreeType; + + /// Junction tree type that can do multifrontal elimination of this graph + typedef typename EliminationTraits::JunctionTreeType JunctionTreeType; + + /// The pair of conditional and remaining factor produced by a single dense elimination step on + /// a subgraph. + typedef std::pair, boost::shared_ptr > EliminationResult; + + /// The function type that does a single dense elimination step on a subgraph. + typedef boost::function >, std::vector)> Eliminate; + + /// Typedef for an optional ordering as an argument to elimination functions + typedef boost::optional OptionalOrdering; + + /// Typedef for an optional variable index as an argument to elimination functions + typedef boost::optional OptionalVariableIndex; /** 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. @@ -77,8 +109,9 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, boost::none, varIndex); * \endcode * */ - boost::shared_ptr - eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none, + boost::shared_ptr eliminateSequential( + const Eliminate& function = EliminationTraits::DefaultEliminate, + OptionalOrdering ordering = boost::none, OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not @@ -101,49 +134,58 @@ namespace gtsam { * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex); * \endcode * */ - boost::shared_ptr - eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none, + boost::shared_ptr eliminateMultifrontal( + const Eliminate& function = EliminationTraits::DefaultEliminate, + OptionalOrdering ordering = boost::none, OptionalVariableIndex variableIndex = boost::none) const; /** 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, const OrderingUnordered& ordering, + std::pair, boost::shared_ptr > + eliminatePartialSequential( + const OrderingUnordered& ordering, + const Eliminate& function = EliminationTraits::DefaultEliminate, 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) * = 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, const std::vector& variables, + std::pair, boost::shared_ptr > + eliminatePartialSequential( + const std::vector& variables, + const Eliminate& function = EliminationTraits::DefaultEliminate, 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) * = 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 OrderingUnordered& ordering, + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal( + const OrderingUnordered& ordering, + const Eliminate& function = EliminationTraits::DefaultEliminate, 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) * \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& variables, + std::pair, boost::shared_ptr > + eliminatePartialMultifrontal( + const std::vector& variables, + const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; private: // Access the derived factor graph class - const FACTORGRAPH& asDerived() const { return static_cast(*this); } + const FactorGraphType& asDerived() const { return static_cast(*this); } // Access the derived factor graph class - FACTORGRAPH& asDerived() { return static_cast(*this); } + FactorGraphType& asDerived() { return static_cast(*this); } }; } diff --git a/gtsam/symbolic/SymbolicFactorGraphUnordered.h b/gtsam/symbolic/SymbolicFactorGraphUnordered.h index ae1bd755b..d1b1c3b7a 100644 --- a/gtsam/symbolic/SymbolicFactorGraphUnordered.h +++ b/gtsam/symbolic/SymbolicFactorGraphUnordered.h @@ -24,39 +24,45 @@ #include // NOTE: Additional headers included at end of file for user convenience -namespace gtsam { class SymbolicConditionalUnordered; } -namespace gtsam { class SymbolicBayesNetUnordered; } -namespace gtsam { class SymbolicEliminationTreeUnordered; } -namespace gtsam { class SymbolicBayesTreeUnordered; } -namespace gtsam { class SymbolicJunctionTreeUnordered; } namespace gtsam { class SymbolicFactorGraphUnordered; + class SymbolicConditionalUnordered; + class SymbolicBayesNetUnordered; + class SymbolicEliminationTreeUnordered; + class SymbolicBayesTreeUnordered; + class SymbolicJunctionTreeUnordered; + /* ************************************************************************* */ template<> class EliminationTraits { - + typedef SymbolicFactorUnordered FactorType; ///< Type of factors in factor graph + typedef SymbolicConditionalUnordered ConditionalType; ///< Type of conditionals from elimination + typedef SymbolicBayesNetUnordered BayesNetType; ///< Type of Bayes net from sequential elimination + typedef SymbolicEliminationTreeUnordered EliminationTreeType; ///< Type of elimination tree + typedef SymbolicBayesTreeUnordered BayesTreeType; ///< Type of Bayes tree + typedef SymbolicJunctionTreeUnordered JunctionTreeType; ///< Type of Junction tree + /// The default dense elimination function + static std::pair, boost::shared_ptr > + DefaultEliminate(const std::vector >& factors, + const std::vector& keys) { return EliminateSymbolicUnordered(factors, keys); } }; + /* ************************************************************************* */ /** Symbolic Factor Graph * \nosubgrouping */ - class GTSAM_EXPORT SymbolicFactorGraphUnordered: + class GTSAM_EXPORT SymbolicFactorGraphUnordered : public FactorGraphUnordered, - public EliminateableFactorGraph< - SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered, - SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered> + public EliminateableFactorGraph { public: - typedef SymbolicFactorGraphUnordered This; - typedef FactorGraphUnordered Base; - typedef EliminateableFactorGraph< - SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered, - SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered> BaseEliminateable; - typedef boost::shared_ptr shared_ptr; - typedef BaseEliminateable::Eliminate Eliminate; + typedef SymbolicFactorGraphUnordered This; ///< Typedef to this class + typedef FactorGraphUnordered Base; ///< Typedef to base factor graph type + typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class /// @name Standard Constructors /// @{