diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index 1857d81fa..e308907ac 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -32,7 +32,6 @@ using namespace boost::assign; namespace lam = boost::lambda; -#include #include #include @@ -41,20 +40,20 @@ namespace gtsam { using namespace std; /* ************************************************************************* */ - template - BayesTree::Clique::Clique() {} + template + BayesTree::Clique::Clique() {} /* ************************************************************************* */ - template - BayesTree::Clique::Clique(const sharedConditional& conditional) { + template + BayesTree::Clique::Clique(const sharedConditional& conditional) { separator_.assign(conditional->parents().begin(), conditional->parents().end()); this->push_back(conditional); } /* ************************************************************************* */ - template - BayesTree::Clique::Clique(const BayesNet& bayesNet) - : BayesNet(bayesNet) { + template + BayesTree::Clique::Clique(const BayesNet& bayesNet) + : BayesNet(bayesNet) { if(bayesNet.size() > 0) { #ifndef NDEBUG // Debug check that each parent variable is either a frontal variable in @@ -63,13 +62,13 @@ namespace gtsam { // is "enough like a clique". todo: this should really check that the // fragment *is* a clique. if(bayesNet.size() > 1) { - typename BayesNet::const_reverse_iterator cond = bayesNet.rbegin(); + typename BayesNet::const_reverse_iterator cond = bayesNet.rbegin(); ++ cond; - typename Conditional::shared_ptr lastConditional = *cond; + typename CONDITIONAL::shared_ptr lastConditional = *cond; for( ; cond != bayesNet.rend(); ++cond) - for(typename Conditional::const_iterator parent=(*cond)->beginParents(); parent!=(*cond)->endParents(); ++parent) { + for(typename CONDITIONAL::const_iterator parent=(*cond)->beginParents(); parent!=(*cond)->endParents(); ++parent) { bool infragment = false; - typename BayesNet::const_reverse_iterator parentCond = cond; + typename BayesNet::const_reverse_iterator parentCond = cond; do { if(*parent == (*parentCond)->key()) infragment = true; @@ -86,8 +85,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - vector BayesTree::Clique::keys() const { + template + vector BayesTree::Clique::keys() const { vector keys; keys.reserve(this->size() + separator_.size()); BOOST_FOREACH(const sharedConditional conditional, *this) { @@ -98,8 +97,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::Clique::print(const string& s) const { + template + void BayesTree::Clique::print(const string& s) const { cout << s << "Clique "; BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { cout << conditional->key() << " "; } cout << "| "; @@ -118,8 +117,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - size_t BayesTree::Clique::treeSize() const { + template + size_t BayesTree::Clique::treeSize() const { size_t size = 1; BOOST_FOREACH(const shared_ptr& child, children_) size += child->treeSize(); @@ -127,17 +126,17 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::Clique::printTree(const string& indent) const { + template + void BayesTree::Clique::printTree(const string& indent) const { print(indent); BOOST_FOREACH(const shared_ptr& child, children_) child->printTree(indent+" "); } /* ************************************************************************* */ - template - void BayesTree::Clique::permuteWithInverse(const Permutation& inversePermutation) { - BayesNet::permuteWithInverse(inversePermutation); + template + void BayesTree::Clique::permuteWithInverse(const Permutation& inversePermutation) { + BayesNet::permuteWithInverse(inversePermutation); BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; } if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); BOOST_FOREACH(const shared_ptr& child, children_) { @@ -146,9 +145,9 @@ namespace gtsam { } /* ************************************************************************* */ - template - bool BayesTree::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { - bool changed = BayesNet::permuteSeparatorWithInverse(inversePermutation); + template + bool BayesTree::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { + bool changed = BayesNet::permuteSeparatorWithInverse(inversePermutation); #ifndef NDEBUG if(!changed) { BOOST_FOREACH(Index& separatorKey, separator_) { assert(separatorKey == inversePermutation[separatorKey]); } @@ -168,17 +167,17 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::CliqueData - BayesTree::getCliqueData() const { + template + typename BayesTree::CliqueData + BayesTree::getCliqueData() const { CliqueData data; getCliqueData(data, root_); return data; } - template - void BayesTree::getCliqueData(CliqueData& data, - BayesTree::sharedClique clique) const { + template + void BayesTree::getCliqueData(CliqueData& data, + BayesTree::sharedClique clique) const { data.conditionalSizes.push_back(clique->conditionals_.size()); data.separatorSizes.push_back(clique->separator_.size()); BOOST_FOREACH(sharedClique c, clique->children_) { @@ -187,8 +186,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::saveGraph(const std::string &s) const { + template + void BayesTree::saveGraph(const std::string &s) const { if (!root_.get()) throw invalid_argument("the root of bayes tree has not been initialized!"); ofstream of(s.c_str()); of<< "digraph G{\n"; @@ -197,9 +196,9 @@ namespace gtsam { of.close(); } - template - void BayesTree::saveGraph(ostream &s, - BayesTree::sharedClique clique, + template + void BayesTree::saveGraph(ostream &s, + BayesTree::sharedClique clique, int parentnum) const { static int num = 0; bool first = true; @@ -208,7 +207,7 @@ namespace gtsam { string parent = out.str(); parent += "[label=\""; - BOOST_FOREACH(boost::shared_ptr c, clique->conditionals_) { + BOOST_FOREACH(boost::shared_ptr c, clique->conditionals_) { if(!first) parent += ","; first = false; parent += (string(c->key())).c_str(); } @@ -234,9 +233,9 @@ namespace gtsam { } - template - typename BayesTree::CliqueStats - BayesTree::CliqueData::getStats() const { + template + typename BayesTree::CliqueStats + BayesTree::CliqueData::getStats() const { CliqueStats stats; double sum = 0.0; @@ -266,21 +265,21 @@ namespace gtsam { // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p // TODO, why do we actually return a shared pointer, why does eliminate? /* ************************************************************************* */ - template + template template - BayesNet - BayesTree::Clique::shortcut(shared_ptr R) { + BayesNet + BayesTree::Clique::shortcut(shared_ptr R) { // A first base case is when this clique or its parent is the root, // in which case we return an empty Bayes net. sharedClique parent(parent_.lock()); if (R.get()==this || parent==R) { - BayesNet empty; + BayesNet empty; return empty; } - // The parent clique has a Conditional for each frontal node in Fp + // The parent clique has a CONDITIONAL for each frontal node in Fp // so we can obtain P(Fp|Sp) in factor graph form FactorGraph p_Fp_Sp(*parent); @@ -327,7 +326,7 @@ namespace gtsam { factor->permuteWithInverse(*toFrontInverse); } varIndex.permute(toFront); - BayesNet p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); + BayesNet p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); // remove all integrands for(Index j=0; j + template template FactorGraph - BayesTree::Clique::marginal(shared_ptr R) { + BayesTree::Clique::marginal(shared_ptr R) { // If we are the root, just return this root if (R.get()==this) return *R; // Combine P(F|S), P(S|R), and P(R) - BayesNet p_FSR = this->shortcut(R); + BayesNet p_FSR = this->shortcut(R); p_FSR.push_front(*this); p_FSR.push_back(*R); @@ -365,14 +364,14 @@ namespace gtsam { // /* ************************************************************************* */ // // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R) // /* ************************************************************************* */ -// template +// template // template // pair, Ordering> -// BayesTree::Clique::joint(shared_ptr C2, shared_ptr R) { +// BayesTree::Clique::joint(shared_ptr C2, shared_ptr R) { // // For now, assume neither is the root // // // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R) -// sharedBayesNet bn(new BayesNet); +// sharedBayesNet bn(new BayesNet); // if (!isRoot()) bn->push_back(*this); // P(F1|S1) // if (!isRoot()) bn->push_back(shortcut(R)); // P(S1|R) // if (!C2->isRoot()) bn->push_back(*C2); // P(F2|S2) @@ -385,26 +384,26 @@ namespace gtsam { // keys12.unique(); // // // Calculate the marginal -// return make_pair(marginalize(*bn,keys12), keys12); +// return make_pair(marginalize(*bn,keys12), keys12); // } /* ************************************************************************* */ - template - void BayesTree::Cliques::print(const std::string& s) const { + template + void BayesTree::Cliques::print(const std::string& s) const { cout << s << ":\n"; BOOST_FOREACH(sharedClique clique, *this) clique->printTree(); } /* ************************************************************************* */ - template - bool BayesTree::Cliques::equals(const Cliques& other, double tol) const { + template + bool BayesTree::Cliques::equals(const Cliques& other, double tol) const { return other == *this; } /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( + template + typename BayesTree::sharedClique BayesTree::addClique( const sharedConditional& conditional, sharedClique parent_clique) { sharedClique new_clique(new Clique(conditional)); Index key = conditional->key(); @@ -418,8 +417,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( + template + typename BayesTree::sharedClique BayesTree::addClique( const sharedConditional& conditional, list& child_cliques) { sharedClique new_clique(new Clique(conditional)); Index key = conditional->key(); @@ -432,8 +431,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - inline void BayesTree::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { + template + inline void BayesTree::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { static const bool debug = false; #ifndef NDEBUG // Debug check to make sure the conditional variable is ordered lower than @@ -465,8 +464,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::removeClique(sharedClique clique) { + template + void BayesTree::removeClique(sharedClique clique) { if (clique->isRoot()) root_.reset(); @@ -475,7 +474,7 @@ namespace gtsam { // orphan my children BOOST_FOREACH(sharedClique child, clique->children_) - child->parent_ = typename BayesTree::Clique::weak_ptr(); + child->parent_ = typename BayesTree::Clique::weak_ptr(); BOOST_FOREACH(Index key, clique->separator_) { nodes_[key].reset(); @@ -487,34 +486,34 @@ namespace gtsam { } /* ************************************************************************* */ - template - BayesTree::BayesTree() { + template + BayesTree::BayesTree() { } /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet) { - typename BayesNet::const_reverse_iterator rit; + template + BayesTree::BayesTree(const BayesNet& bayesNet) { + typename BayesNet::const_reverse_iterator rit; for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) insert(*rit); } /* ************************************************************************* */ - template - BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { + template + BayesTree::BayesTree(const BayesNet& bayesNet, std::list > subtrees) { if (bayesNet.size() == 0) throw invalid_argument("BayesTree::insert: empty bayes net!"); // get the roots of child subtrees and merge their nodes_ list childRoots; - BOOST_FOREACH(const BayesTree& subtree, subtrees) { + BOOST_FOREACH(const BayesTree& subtree, subtrees) { nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end()); childRoots.push_back(subtree.root()); } // create a new clique and add all the conditionals to the clique sharedClique new_clique; - typename BayesNet::sharedConditional conditional; + typename BayesNet::sharedConditional conditional; BOOST_REVERSE_FOREACH(conditional, bayesNet) { if (!new_clique.get()) new_clique = addClique(conditional,childRoots); @@ -526,8 +525,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::print(const string& s) const { + template + void BayesTree::print(const string& s) const { if (root_.use_count() == 0) { printf("WARNING: BayesTree.print encountered a forest...\n"); return; @@ -539,26 +538,26 @@ namespace gtsam { /* ************************************************************************* */ // binary predicate to test equality of a pair for use in equals - template + template bool check_sharedCliques( - const typename BayesTree::sharedClique& v1, - const typename BayesTree::sharedClique& v2 + const typename BayesTree::sharedClique& v1, + const typename BayesTree::sharedClique& v2 ) { return v1->equals(*v2); } /* ************************************************************************* */ - template - bool BayesTree::equals(const BayesTree& other, + template + bool BayesTree::equals(const BayesTree& other, double tol) const { return size()==other.size() && - std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); + std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques); } /* ************************************************************************* */ - template + template template - inline Index BayesTree::findParentClique(const Container& parents) const { + inline Index BayesTree::findParentClique(const Container& parents) const { typename Container::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); assert(lowestOrderedParent != parents.end()); return *lowestOrderedParent; @@ -578,13 +577,13 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::insert(const sharedConditional& conditional) + template + void BayesTree::insert(const sharedConditional& conditional) { static const bool debug = false; // get key and parents - typename Conditional::Parents parents = conditional->parents(); // todo: const reference? + typename CONDITIONAL::Parents parents = conditional->parents(); // todo: const reference? if(debug) conditional->print("Adding conditional "); @@ -627,9 +626,9 @@ namespace gtsam { /* ************************************************************************* */ //TODO: remove this function after removing TSAM.cpp - template - typename BayesTree::sharedClique BayesTree::insert( - const BayesNet& bayesNet, list& children, bool isRootClique) + template + typename BayesTree::sharedClique BayesTree::insert( + const BayesNet& bayesNet, list& children, bool isRootClique) { static const bool debug = false; @@ -638,7 +637,7 @@ namespace gtsam { // create a new clique and add all the conditionals to the clique sharedClique new_clique; - typename BayesNet::sharedConditional conditional; + typename BayesNet::sharedConditional conditional; BOOST_REVERSE_FOREACH(conditional, bayesNet) { if(debug) conditional->print("Inserting conditional: "); if (!new_clique.get()) @@ -653,18 +652,18 @@ namespace gtsam { } /* ************************************************************************* */ - template - void BayesTree::fillNodesIndex(const sharedClique& subtree) { + template + void BayesTree::fillNodesIndex(const sharedClique& subtree) { // Add each frontal variable of this root node - BOOST_FOREACH(const typename Conditional::shared_ptr& cond, *subtree) { nodes_[cond->key()] = subtree; } + BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, *subtree) { nodes_[cond->key()] = subtree; } // Fill index for each child - BOOST_FOREACH(const typename BayesTree::sharedClique& child, subtree->children_) { + BOOST_FOREACH(const typename BayesTree::sharedClique& child, subtree->children_) { fillNodesIndex(child); } } /* ************************************************************************* */ - template - void BayesTree::insert(const sharedClique& subtree) { + template + void BayesTree::insert(const sharedClique& subtree) { if(subtree) { // Find the parent clique of the new subtree. By the running intersection // property, those separator variables in the subtree that are ordered @@ -690,10 +689,10 @@ namespace gtsam { /* ************************************************************************* */ // First finds clique marginal then marginalizes that /* ************************************************************************* */ - template + template template FactorGraph - BayesTree::marginal(Index key) const { + BayesTree::marginal(Index key) const { // get clique containing key sharedClique clique = (*this)[key]; @@ -721,10 +720,10 @@ namespace gtsam { } /* ************************************************************************* */ - template + template template - BayesNet - BayesTree::marginalBayesNet(Index key) const { + BayesNet + BayesTree::marginalBayesNet(Index key) const { // calculate marginal as a factor graph FactorGraph fg = this->marginal(key); @@ -736,10 +735,10 @@ namespace gtsam { // /* ************************************************************************* */ // // Find two cliques, their joint, then marginalizes // /* ************************************************************************* */ -// template +// template // template // FactorGraph -// BayesTree::joint(Index key1, Index key2) const { +// BayesTree::joint(Index key1, Index key2) const { // // // get clique C1 and C2 // sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; @@ -755,15 +754,15 @@ namespace gtsam { // // // partially eliminate, remaining factor graph is requested joint // // TODO, make eliminate functional -// eliminate(p_C1C2,ord); +// eliminate(p_C1C2,ord); // return p_C1C2; // } // /* ************************************************************************* */ -// template +// template // template -// BayesNet -// BayesTree::jointBayesNet(Index key1, Index key2) const { +// BayesNet +// BayesTree::jointBayesNet(Index key1, Index key2) const { // // // calculate marginal as a factor graph // FactorGraph fg = this->joint(key1,key2); @@ -771,21 +770,21 @@ namespace gtsam { // // eliminate further to Bayes net // Ordering ordering; // ordering += key1, key2; -// return eliminate(fg,ordering); +// return eliminate(fg,ordering); // } /* ************************************************************************* */ - template - void BayesTree::clear() { + template + void BayesTree::clear() { // Remove all nodes and clear the root pointer nodes_.clear(); root_.reset(); } /* ************************************************************************* */ - template - void BayesTree::removePath(sharedClique clique, - BayesNet& bn, typename BayesTree::Cliques& orphans) { + template + void BayesTree::removePath(sharedClique clique, + BayesNet& bn, typename BayesTree::Cliques& orphans) { // base case is NULL, if so we do nothing and return empties above if (clique!=NULL) { @@ -808,10 +807,10 @@ namespace gtsam { } /* ************************************************************************* */ - template + template template - void BayesTree::removeTop(const Container& keys, - BayesNet& bn, typename BayesTree::Cliques& orphans) { + void BayesTree::removeTop(const Container& keys, + BayesNet& bn, typename BayesTree::Cliques& orphans) { // process each key of the new factor BOOST_FOREACH(const Index& key, keys) { diff --git a/inference/BayesTree.h b/inference/BayesTree.h index 1c1a08b8b..2409cff36 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -35,38 +35,38 @@ namespace gtsam { /** * Bayes tree - * Templated on the Conditional class, the type of node in the underlying Bayes chain. + * Templated on the CONDITIONAL class, the type of node in the underlying Bayes chain. * This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional */ - template - class BayesTree: public Testable > { + template + class BayesTree: public Testable > { public: - typedef boost::shared_ptr sharedConditional; - typedef boost::shared_ptr > sharedBayesNet; + typedef boost::shared_ptr sharedConditional; + typedef boost::shared_ptr > sharedBayesNet; /** A Clique in the tree is an incomplete Bayes net: the variables * in the Bayes net are the frontal nodes, and the variables conditioned * on is the separator. We also have pointers up and down the tree. */ - struct Clique: public BayesNet { + struct Clique: public BayesNet { typedef typename boost::shared_ptr shared_ptr; typedef typename boost::weak_ptr weak_ptr; weak_ptr parent_; std::list children_; std::list separator_; /** separator keys */ - typename Conditional::Factor::shared_ptr cachedFactor_; + typename CONDITIONAL::Factor::shared_ptr cachedFactor_; - friend class BayesTree; + friend class BayesTree; //* Constructor */ Clique(); Clique(const sharedConditional& conditional); - Clique(const BayesNet& bayesNet); + Clique(const BayesNet& bayesNet); /** return keys in frontal:separator order */ std::vector keys() const; @@ -87,7 +87,7 @@ namespace gtsam { const std::list& children() const { return children_; } /** Reference the cached factor */ - typename Conditional::Factor::shared_ptr& cachedFactor() { return cachedFactor_; } + typename CONDITIONAL::Factor::shared_ptr& cachedFactor() { return cachedFactor_; } /** The size of subtree rooted at this clique, i.e., nr of Cliques */ size_t treeSize() const; @@ -108,7 +108,7 @@ namespace gtsam { /** return the conditional P(S|Root) on the separator given the root */ // TODO: create a cached version template - BayesNet shortcut(shared_ptr root); + BayesNet shortcut(shared_ptr root); /** return the marginal P(C) of the clique */ template @@ -187,13 +187,13 @@ namespace gtsam { BayesTree(); /** Create a Bayes Tree from a Bayes Net */ - BayesTree(const BayesNet& bayesNet); + BayesTree(const BayesNet& bayesNet); /** * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the * new root clique and the subtrees are connected to the root clique. */ - BayesTree(const BayesNet& bayesNet, std::list > subtrees); + BayesTree(const BayesNet& bayesNet, std::list > subtrees); /** Destructor */ virtual ~BayesTree() { @@ -210,7 +210,7 @@ namespace gtsam { * It is the caller's responsibility to decide whether the given Bayes net is a valid clique, * i.e. all the variables (frontal and separator) are connected */ - sharedClique insert(const BayesNet& bayesNet, + sharedClique insert(const BayesNet& bayesNet, std::list& children, bool isRootClique = false); /** @@ -227,7 +227,7 @@ namespace gtsam { */ /** check equality */ - bool equals(const BayesTree& other, double tol = 1e-9) const; + bool equals(const BayesTree& other, double tol = 1e-9) const; /** * Find parent clique of a conditional. It will look at all parents and @@ -262,7 +262,7 @@ namespace gtsam { /** return marginal on any variable, as a Bayes Net */ template - BayesNet marginalBayesNet(Index key) const; + BayesNet marginalBayesNet(Index key) const; // /** return joint on two variables */ // template @@ -270,7 +270,7 @@ namespace gtsam { // // /** return joint on two variables as a BayesNet */ // template -// BayesNet jointBayesNet(Index key1, Index key2) const; +// BayesNet jointBayesNet(Index key1, Index key2) const; /** * Read only with side effects @@ -293,14 +293,14 @@ namespace gtsam { * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. */ - void removePath(sharedClique clique, BayesNet& bn, Cliques& orphans); + void removePath(sharedClique clique, BayesNet& bn, Cliques& orphans); /** * Given a list of keys, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ template - void removeTop(const Container& keys, BayesNet& bn, Cliques& orphans); + void removeTop(const Container& keys, BayesNet& bn, Cliques& orphans); }; // BayesTree