diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index c4c2afedd..c208310b0 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -45,14 +45,14 @@ namespace gtsam { /* ************************************************************************* */ bool DecisionTreeFactor::equals(const This& other, double tol) const { - return IndexFactorOrdered::equals(other, tol) && Potentials::equals(other, tol); + return Factor::equals(other, tol) && Potentials::equals(other, tol); } /* ************************************************************************* */ void DecisionTreeFactor::print(const string& s, const IndexFormatter& formatter) const { cout << s; - IndexFactorOrdered::print("IndexFactor:",formatter); + Base::print("Factor:",formatter); Potentials::print("Potentials:",formatter); } diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index d072c89af..aff160cab 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -128,26 +128,6 @@ namespace gtsam { */ shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; - /** - * @brief Permutes the keys in Potentials and DiscreteFactor - * - * This re-implements the permuteWithInverse() in both Potentials - * and DiscreteFactor by doing both of them together. - */ - - void permuteWithInverse(const Permutation& inversePermutation){ - DiscreteFactor::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - DiscreteFactor::reduceWithInverse(inverseReduction); - Potentials::reduceWithInverse(inverseReduction); - } - /// @} }; // DecisionTreeFactor diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index f0a32ed64..0453bb95f 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -18,47 +18,41 @@ #include #include -#include +#include + #include +#include namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class BayesNetOrdered ; - /* ************************************************************************* */ - void add_front(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_front(boost::make_shared(s)); + void DiscreteBayesNet::add(const Signature& s) { + push_back(boost::make_shared(s)); } /* ************************************************************************* */ - void add(DiscreteBayesNet& bayesNet, const Signature& s) { - bayesNet.push_back(boost::make_shared(s)); - } - - /* ************************************************************************* */ - double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values) { + double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) { // evaluate all conditionals and multiply double result = 1.0; - BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, *this) result *= (*conditional)(values); return result; } /* ************************************************************************* */ - DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn) { + DiscreteFactor::sharedValues DiscreteBayesNet::optimize() { // solve each node in turn in topological sort order (parents first) DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, bn) + BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, *this) conditional->solveInPlace(*result); return result; } /* ************************************************************************* */ - DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn) { + DiscreteFactor::sharedValues DiscreteBayesNet::sample() { // sample each node in turn in topological sort order (parents first) DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); - BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, bn) + BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, *this) conditional->sampleInPlace(*result); return result; } diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index aa7692142..1b1f7ed4c 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -17,30 +17,50 @@ #pragma once +#include +#include + #include #include #include -#include -#include namespace gtsam { - typedef BayesNetOrdered DiscreteBayesNet; + class GTSAM_EXPORT DiscreteBayesNet : public FactorGraph + { + public: + /// @name Standard Constructors + /// @{ + + /** Construct empty factor graph */ + DiscreteBayesNet() {} + + /** Construct from iterator over conditionals */ + template + DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit DiscreteBayesNet(const CONTAINER& conditionals) : Base(conditionals) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + DiscreteBayesNet(const FactorGraph& graph) : Base(graph) {} + + /// @} - /** Add a DiscreteCondtional */ - GTSAM_EXPORT void add(DiscreteBayesNet&, const Signature& s); + /** Add a DiscreteCondtional */ + void add(const Signature& s); - /** Add a DiscreteCondtional in front, when listing parents first*/ - GTSAM_EXPORT void add_front(DiscreteBayesNet&, const Signature& s); + //** evaluate for given Values */ + double evaluate(const DiscreteConditional::Values& values); - //** evaluate for given Values */ - GTSAM_EXPORT double evaluate(const DiscreteBayesNet& bn, const DiscreteConditional::Values & values); + /** Optimize function for back-substitution. */ + DiscreteFactor::sharedValues optimize(); - /** Optimize function for back-substitution. */ - GTSAM_EXPORT DiscreteFactor::sharedValues optimize(const DiscreteBayesNet& bn); - - /** Do ancestral sampling */ - GTSAM_EXPORT DiscreteFactor::sharedValues sample(const DiscreteBayesNet& bn); + /** Do ancestral sampling */ + DiscreteFactor::sharedValues sample(); + }; } // namespace diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index e0afd6354..04957fdc1 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -35,38 +35,34 @@ using namespace std; namespace gtsam { /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const size_t nrFrontals, - const DecisionTreeFactor& f) : - IndexConditionalOrdered(f.keys(), nrFrontals), Potentials( - f / (*f.sum(nrFrontals))) { - } + DiscreteConditional::DiscreteConditional(const size_t nrFrontals, const DecisionTreeFactor& f) : + BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {} /* ******************************************************************************** */ - DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal) : - IndexConditionalOrdered(joint.keys(), joint.size() - marginal.size()), Potentials( - ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal) { + DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint, const DecisionTreeFactor& marginal) : + BaseFactor(ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), + BaseConditional(joint.size() - marginal.size()) + { if (ISDEBUG("DiscreteConditional::DiscreteConditional")) cout << (firstFrontalKey()) << endl; //TODO Print all keys } /* ******************************************************************************** */ DiscreteConditional::DiscreteConditional(const Signature& signature) : - IndexConditionalOrdered(signature.indices(), 1), Potentials( - signature.discreteKeysParentsFirst(), signature.cpt()) { - } + BaseFactor(signature.discreteKeysParentsFirst(), signature.cpt()), + BaseConditional(1) {} /* ******************************************************************************** */ - void DiscreteConditional::print(const std::string& s, const IndexFormatter& formatter) const { - std::cout << s << std::endl; - IndexConditionalOrdered::print(s, formatter); + void DiscreteConditional::print(const std::string& s, const KeyFormatter& formatter) const { + BaseConditional::print(s, formatter); Potentials::print(s); } /* ******************************************************************************** */ bool DiscreteConditional::equals(const DiscreteConditional& other, double tol) const { - return IndexConditionalOrdered::equals(other, tol) - && Potentials::equals(other, tol); + return BaseFactor::equals(other, tol) + && BaseConditional::equals(other, tol) + && Potentials::equals(other, tol); } /* ******************************************************************************** */ @@ -195,13 +191,6 @@ namespace gtsam { return 0; } - /* ******************************************************************************** */ - void DiscreteConditional::permuteWithInverse(const Permutation& inversePermutation){ - IndexConditionalOrdered::permuteWithInverse(inversePermutation); - Potentials::permuteWithInverse(inversePermutation); - } - - /* ******************************************************************************** */ } // namespace diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 3b46542ef..ccc728c6e 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -30,13 +30,16 @@ namespace gtsam { * Discrete Conditional Density * Derives from DecisionTreeFactor */ - class GTSAM_EXPORT DiscreteConditional: public IndexConditionalOrdered, public Potentials { - + class GTSAM_EXPORT DiscreteConditional : + public DecisionTreeFactor, + public Conditional + { public: // typedefs needed to play nice with gtsam - typedef DiscreteFactor FactorType; - typedef boost::shared_ptr shared_ptr; - typedef IndexConditionalOrdered Base; + typedef DiscreteConditional This; + typedef DecisionTreeFactor BaseFactor; + typedef Conditional BaseConditional; + typedef boost::shared_ptr shared_ptr; /** A map from keys to values */ typedef Assignment Values; @@ -56,8 +59,7 @@ namespace gtsam { DiscreteConditional(const Signature& signature); /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */ - DiscreteConditional(const DecisionTreeFactor& joint, - const DecisionTreeFactor& marginal); + DiscreteConditional(const DecisionTreeFactor& joint, const DecisionTreeFactor& marginal); /** * Combine several conditional into a single one. @@ -75,7 +77,7 @@ namespace gtsam { /// GTSAM-style print void print(const std::string& s = "Discrete Conditional: ", - const IndexFormatter& formatter = DefaultIndexFormatter) const; + const KeyFormatter& formatter = DefaultKeyFormatter) const; /// GTSAM-style equals bool equals(const DiscreteConditional& other, double tol = 1e-9) const; @@ -89,11 +91,6 @@ namespace gtsam { return Potentials::operator()(values); } - /** Convert to a factor */ - DecisionTreeFactor::shared_ptr toFactor() const { - return DecisionTreeFactor::shared_ptr(new DecisionTreeFactor(*this)); - } - /** Restrict to given parent values, returns AlgebraicDecisionDiagram */ ADT choose(const Assignment& parentsValues) const; @@ -121,11 +118,6 @@ namespace gtsam { /// sample in place, stores result in partial solution void sampleInPlace(Values& parentsValues) const; - /** - * Permutes both IndexConditional and Potentials. - */ - void permuteWithInverse(const Permutation& inversePermutation); - /// @} }; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index a9a1ac0cf..f8d2ba695 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -19,7 +19,9 @@ #pragma once #include -#include +#include + +#include namespace gtsam { @@ -30,12 +32,13 @@ namespace gtsam { * Base class for discrete probabilistic factors * The most general one is the derived DecisionTreeFactor */ - class GTSAM_EXPORT DiscreteFactor: public IndexFactorOrdered { + class GTSAM_EXPORT DiscreteFactor : public Factor { public: // typedefs needed to play nice with gtsam typedef DiscreteFactor This; + typedef Factor Base; typedef DiscreteConditional ConditionalType; typedef boost::shared_ptr shared_ptr; @@ -47,23 +50,23 @@ namespace gtsam { /// Construct n-way factor DiscreteFactor(const std::vector& js) : - IndexFactorOrdered(js) { + Base(js) { } /// Construct unary factor DiscreteFactor(Index j) : - IndexFactorOrdered(j) { + Base(boost::assign::cref_list_of<1>(j)) { } /// Construct binary factor DiscreteFactor(Index j1, Index j2) : - IndexFactorOrdered(j1, j2) { + Base(boost::assign::cref_list_of<2>(j1)(j2)) { } /// construct from container template DiscreteFactor(KeyIterator beginKey, KeyIterator endKey) : - IndexFactorOrdered(beginKey, endKey) { + Base(beginKey, endKey) { } public: @@ -83,9 +86,8 @@ namespace gtsam { // print virtual void print(const std::string& s = "DiscreteFactor\n", - const IndexFormatter& formatter - =DefaultIndexFormatter) const { - IndexFactorOrdered::print(s,formatter); + const KeyFormatter& formatter = DefaultKeyFormatter) const { + Base::print(s,formatter); } /// @} @@ -100,21 +102,6 @@ namespace gtsam { virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; - /** - * Permutes the factor, but for efficiency requires the permutation - * to already be inverted. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation){ - IndexFactorOrdered::permuteWithInverse(inversePermutation); - } - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - virtual void reduceWithInverse(const internal::Reduction& inverseReduction) { - IndexFactorOrdered::reduceWithInverse(inverseReduction); - } - /// @} }; // DiscreteFactor diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 642a87eca..c4e2582d3 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -16,28 +16,15 @@ * @author Frank Dellaert */ -//#define ENABLE_TIMING +#include #include #include -#include +#include +#include #include namespace gtsam { - // Explicitly instantiate so we don't have to include everywhere - template class FactorGraphOrdered ; - template class EliminationTreeOrdered ; - - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph() { - } - - /* ************************************************************************* */ - DiscreteFactorGraph::DiscreteFactorGraph( - const BayesNetOrdered& bayesNet) : - FactorGraphOrdered(bayesNet) { - } - /* ************************************************************************* */ FastSet DiscreteFactorGraph::keys() const { FastSet keys; @@ -75,27 +62,9 @@ namespace gtsam { } } - /* ************************************************************************* */ - void DiscreteFactorGraph::permuteWithInverse( - const Permutation& inversePermutation) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->permuteWithInverse(inversePermutation); - } - } - - /* ************************************************************************* */ - void DiscreteFactorGraph::reduceWithInverse( - const internal::Reduction& inverseReduction) { - BOOST_FOREACH(const sharedFactor& factor, factors_) { - if(factor) - factor->reduceWithInverse(inverseReduction); - } - } - /* ************************************************************************* */ std::pair // - EliminateDiscrete(const FactorGraphOrdered& factors, size_t num) { + EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys) { // PRODUCT: multiply all factors gttic(product); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index b2da2d519..1afc339c6 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -20,31 +20,37 @@ #include #include -#include +#include #include #include namespace gtsam { -class DiscreteFactorGraph: public FactorGraphOrdered { + // Forward declarations + class Ordering; + +class GTSAM_EXPORT DiscreteFactorGraph: public FactorGraph { public: /** A map from keys to values */ typedef std::vector Indices; typedef Assignment Values; typedef boost::shared_ptr sharedValues; - - /** Construct empty factor graph */ - GTSAM_EXPORT DiscreteFactorGraph(); - - /** Constructor from a factor graph of GaussianFactor or a derived type */ - template - DiscreteFactorGraph(const FactorGraphOrdered& fg) { - push_back(fg); - } - - /** construct from a BayesNet */ - GTSAM_EXPORT DiscreteFactorGraph(const BayesNetOrdered& bayesNet); + + /** Default constructor */ + DiscreteFactorGraph() {} + + /** Construct from iterator over factors */ + template + DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {} + + /** Construct from container of factors (shared_ptr or plain objects) */ + template + explicit DiscreteFactorGraph(const CONTAINER& factors) : Base(factors) {} + + /** Implicit copy/downcast constructor to override explicit template container constructor */ + template + DiscreteFactorGraph(const FactorGraph& graph) : Base(graph) {} template void add(const DiscreteKey& j, SOURCE table) { @@ -68,29 +74,22 @@ public: } /** Return the set of variables involved in the factors (set union) */ - GTSAM_EXPORT FastSet keys() const; + FastSet keys() const; /** return product of all factors as a single factor */ - GTSAM_EXPORT DecisionTreeFactor product() const; + DecisionTreeFactor product() const; /** Evaluates the factor graph given values, returns the joint probability of the factor graph given specific instantiation of values*/ - GTSAM_EXPORT double operator()(const DiscreteFactor::Values & values) const; + double operator()(const DiscreteFactor::Values & values) const; /// print - GTSAM_EXPORT void print(const std::string& s = "DiscreteFactorGraph", - const IndexFormatter& formatter =DefaultIndexFormatter) const; - - /** Permute the variables in the factors */ - GTSAM_EXPORT void permuteWithInverse(const Permutation& inversePermutation); - - /** Apply a reduction, which is a remapping of variable indices. */ - GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); + void print(const std::string& s = "DiscreteFactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const; }; // DiscreteFactorGraph /** Main elimination function for DiscreteFactorGraph */ GTSAM_EXPORT std::pair, DecisionTreeFactor::shared_ptr> -EliminateDiscrete(const FactorGraphOrdered& factors, - size_t nrFrontals = 1); +EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys); } // namespace gtsam diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index c8fb3a11d..2d5e1634b 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -61,38 +61,5 @@ namespace gtsam { } /* ************************************************************************* */ - template - void Potentials::remapIndices(const P& remapping) { - // Permute the _cardinalities (TODO: Inefficient Consider Improving) - DiscreteKeys keys; - map ordering; - - // Get the original keys from cardinalities_ - BOOST_FOREACH(const DiscreteKey& key, cardinalities_) - keys & key; - - // Perform Permutation - BOOST_FOREACH(DiscreteKey& key, keys) { - ordering[key.first] = remapping[key.first]; - key.first = ordering[key.first]; - } - - // Change *this - AlgebraicDecisionTree permuted((*this), ordering); - *this = permuted; - cardinalities_ = keys.cardinalities(); - } - - /* ************************************************************************* */ - void Potentials::permuteWithInverse(const Permutation& inversePermutation) { - remapIndices(inversePermutation); - } - - /* ************************************************************************* */ - void Potentials::reduceWithInverse(const internal::Reduction& inverseReduction) { - remapIndices(inverseReduction); - } - - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 1f190da82..91683516e 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -19,7 +19,6 @@ #include #include -#include #include #include @@ -48,10 +47,6 @@ namespace gtsam { // Safe division for probabilities GTSAM_EXPORT static double safe_div(const double& a, const double& b); - // Apply either a permutation or a reduction - template - void remapIndices(const P& remapping); - public: /** Default constructor for I/O */ @@ -73,20 +68,6 @@ namespace gtsam { size_t cardinality(Index j) const { return cardinalities_.at(j);} - /** - * @brief Permutes the keys in Potentials - * - * This permutes the Indices and performs necessary re-ordering of ADD. - * This is virtual so that derived types e.g. DecisionTreeFactor can - * re-implement it. - */ - GTSAM_EXPORT virtual void permuteWithInverse(const Permutation& inversePermutation); - - /** - * Apply a reduction, which is a remapping of variable indices. - */ - GTSAM_EXPORT virtual void reduceWithInverse(const internal::Reduction& inverseReduction); - }; // Potentials } // namespace gtsam