127 lines
4.3 KiB
C++
127 lines
4.3 KiB
C++
/**
|
|
* @file BayesTree.cpp
|
|
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include "BayesTree.h"
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace std;
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
Front<Conditional>::Front(string key, cond_ptr conditional) {
|
|
add(key, conditional);
|
|
separator_ = conditional->parents();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
void Front<Conditional>::print(const string& s) const {
|
|
cout << s;
|
|
BOOST_FOREACH(string key, keys_) cout << " " << key;
|
|
if (!separator_.empty()) {
|
|
cout << " :";
|
|
BOOST_FOREACH(string key, separator_)
|
|
cout << " " << key;
|
|
}
|
|
cout << endl;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
bool Front<Conditional>::equals(const Front<Conditional>& other, double tol) const {
|
|
return (keys_ == other.keys_) &&
|
|
equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star<Conditional>);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
void Front<Conditional>::add(string key, cond_ptr conditional) {
|
|
keys_.push_front(key);
|
|
conditionals_.push_front(conditional);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
BayesTree<Conditional>::BayesTree() {
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// TODO: traversal is O(n*log(n)) but could be O(n) with better bayesChain
|
|
template<class Conditional>
|
|
BayesTree<Conditional>::BayesTree(BayesNet<Conditional>& bayesChain, bool verbose) {
|
|
list<string> reverseOrdering = bayesChain.keys();
|
|
BOOST_FOREACH(string key, reverseOrdering)
|
|
insert(key,bayesChain[key],verbose);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
void BayesTree<Conditional>::print(const string& s) const {
|
|
cout << s << ": size == " << nodes_.size() << endl;
|
|
if (nodes_.empty()) return;
|
|
nodes_[0]->printTree("");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
bool BayesTree<Conditional>::equals(const BayesTree<Conditional>& other,
|
|
double tol) const {
|
|
return size()==other.size() &&
|
|
equal(nodeMap_.begin(),nodeMap_.end(),other.nodeMap_.begin()) &&
|
|
equal(nodes_.begin(),nodes_.end(),other.nodes_.begin(),equals_star<Node>);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
template<class Conditional>
|
|
void BayesTree<Conditional>::insert(string key, conditional_ptr conditional, bool verbose) {
|
|
|
|
if (verbose) cout << "Inserting " << key << "| ";
|
|
|
|
// get parents
|
|
list<string> parents = conditional->parents();
|
|
if (verbose) BOOST_FOREACH(string p, parents) cout << p << " ";
|
|
if (verbose) cout << endl;
|
|
|
|
// if no parents, start a new root clique
|
|
if (parents.empty()) {
|
|
if (verbose) cout << "Creating root clique" << endl;
|
|
node_ptr root(new Node(key, conditional));
|
|
nodes_.push_back(root);
|
|
nodeMap_.insert(make_pair(key, 0));
|
|
return;
|
|
}
|
|
|
|
// otherwise, find the parent clique
|
|
string parent = parents.front();
|
|
NodeMap::const_iterator it = nodeMap_.find(parent);
|
|
if (it == nodeMap_.end()) throw(invalid_argument(
|
|
"BayesTree::insert('"+key+"'): parent '" + parent + "' was not yet inserted"));
|
|
int index = it->second;
|
|
node_ptr parent_clique = nodes_[index];
|
|
|
|
// if the parents and parent clique have the same size, add to parent clique
|
|
if (parent_clique->size() == parents.size()) {
|
|
if (verbose) cout << "Adding to clique " << index << endl;
|
|
nodeMap_.insert(make_pair(key, index));
|
|
parent_clique->add(key, conditional);
|
|
return;
|
|
}
|
|
|
|
// otherwise, start a new clique and add it to the tree
|
|
if (verbose) cout << "Starting new clique" << endl;
|
|
node_ptr new_clique(new Node(key, conditional));
|
|
new_clique->parent_ = parent_clique;
|
|
parent_clique->children_.push_back(new_clique);
|
|
nodeMap_.insert(make_pair(key, nodes_.size()));
|
|
nodes_.push_back(new_clique);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
} /// namespace gtsam
|