From 11fcd5a69d8bc7bd8e5605b6cc1d5aa803ddd14b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 5 Nov 2009 05:29:47 +0000 Subject: [PATCH] KISS: just a map of pointers in BayesTree now --- cpp/BayesTree-inl.h | 43 ++++++++++++++++++++++++------------------- cpp/BayesTree.h | 13 ++++++------- cpp/testBayesTree.cpp | 10 +++++----- 3 files changed, 35 insertions(+), 31 deletions(-) diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 003de7989..e3e1447c1 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -60,29 +60,29 @@ namespace gtsam { void BayesTree::print(const string& s) const { cout << s << ": size == " << nodes_.size() << endl; if (nodes_.empty()) return; - nodes_[0]->printTree(""); + root_->printTree(""); } /* ************************************************************************* */ template bool BayesTree::equals(const BayesTree& 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(tol)); + return size()==other.size(); + //&& equal(nodes_.begin(),nodes_.end(),other.nodes_.begin(),equals_star(tol)); } /* ************************************************************************* */ template - void BayesTree::addClique + boost::shared_ptr::Node> BayesTree::addClique (const boost::shared_ptr& conditional, node_ptr parent_clique) { node_ptr new_clique(new Node(conditional)); - nodeMap_.insert(make_pair(conditional->key(), nodes_.size())); - nodes_.push_back(new_clique); - if (parent_clique==NULL) return; - new_clique->parent_ = parent_clique; - parent_clique->children_.push_back(new_clique); + nodes_.insert(make_pair(conditional->key(), new_clique)); + if (parent_clique!=NULL) { + new_clique->parent_ = parent_clique; + parent_clique->children_.push_back(new_clique); + } + return new_clique; } /* ************************************************************************* */ @@ -96,21 +96,20 @@ namespace gtsam { // if no parents, start a new root clique if (parents.empty()) { - addClique(conditional); + root_ = addClique(conditional); return; } // otherwise, find the parent clique string parent = parents.front(); - NodeMap::const_iterator it = nodeMap_.find(parent); - if (it == nodeMap_.end()) throw(invalid_argument( + typename Nodes::const_iterator it = nodes_.find(parent); + if (it == nodes_.end()) throw(invalid_argument( "BayesTree::insert('"+key+"'): parent '" + parent + "' not yet inserted")); - int parent_index = it->second; - node_ptr parent_clique = nodes_[parent_index]; + node_ptr parent_clique = it->second; // if the parents and parent clique have the same size, add to parent clique if (parent_clique->size() == parents.size()) { - nodeMap_.insert(make_pair(key, parent_index)); + nodes_.insert(make_pair(key, parent_clique)); parent_clique->push_front(conditional); return; } @@ -124,12 +123,18 @@ namespace gtsam { boost::shared_ptr BayesTree::marginal(const string& key) const { // find the clique to which key belongs - NodeMap::const_iterator it = nodeMap_.find(key); - if (it == nodeMap_.end()) throw(invalid_argument( + typename Nodes::const_iterator it = nodes_.find(key); + if (it == nodes_.end()) throw(invalid_argument( "BayesTree::marginal('"+key+"'): key not found")); - // find all cliques on the path to the root + // find all cliques on the path to the root and turn into factor graph // FactorGraph + node_ptr node = it->second; + int i=0; + while (node!=NULL) { + //node->print("node"); + node = node->parent_; + } boost::shared_ptr result(new Conditional); return result; diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 1ab94fe71..e9bd2c3b9 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -56,17 +56,16 @@ namespace gtsam { void printTree(const std::string& indent) const; }; - /** vector of Nodes */ + /** Map from keys to Node */ typedef boost::shared_ptr node_ptr; - typedef std::vector Nodes; + typedef std::map Nodes; Nodes nodes_; - /** Map from keys to Node index */ - typedef std::map NodeMap; - NodeMap nodeMap_; + /** Roor clique */ + node_ptr root_; /** add a clique */ - void addClique(const conditional_ptr& conditional, node_ptr parent_clique=node_ptr()); + node_ptr addClique(const conditional_ptr& conditional, node_ptr parent_clique=node_ptr()); public: @@ -92,7 +91,7 @@ namespace gtsam { inline size_t size() const { return nodes_.size();} /** return root clique */ - const BayesNet& root() const {return *(nodes_[0]);} + boost::shared_ptr > root() const {return root_;} /** return marginal on any variable */ boost::shared_ptr marginal(const std::string& key) const; diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 8b76306b7..74520b5bc 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -50,15 +50,15 @@ TEST( BayesTree, constructor ) bayesTree.insert(X); // Check Size - LONGS_EQUAL(4,bayesTree.size()); + LONGS_EQUAL(6,bayesTree.size()); // Check root BayesNet expected_root; expected_root.push_back(E); expected_root.push_back(L); expected_root.push_back(B); - BayesNet actual_root = bayesTree.root(); - CHECK(assert_equal(expected_root,actual_root)); + boost::shared_ptr > actual_root = bayesTree.root(); + CHECK(assert_equal(expected_root,*actual_root)); // Create from symbolic Bayes chain in which we want to discover cliques SymbolicBayesNet ASIA; @@ -97,7 +97,7 @@ TEST( BayesTree, smoother ) // Create the Bayes tree BayesTree bayesTree(*chordalBayesNet); - LONGS_EQUAL(6,bayesTree.size()); + LONGS_EQUAL(7,bayesTree.size()); } /* ************************************************************************* * @@ -119,7 +119,7 @@ TEST( BayesTree, balanced_smoother_marginals ) // Create the Bayes tree BayesTree bayesTree(*chordalBayesNet); - LONGS_EQUAL(4,bayesTree.size()); + LONGS_EQUAL(7,bayesTree.size()); // Check root clique //BayesNet expected_root;