From eab038651e9541aabeadd73f6d9b55564e9a23c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 2 Nov 2009 05:17:44 +0000 Subject: [PATCH] Renamed BayesNet::insert -> push_back. BayesTree now uses Bayes nets as nodes. --- .cproject | 33 ++++++++++-------- cpp/BayesNet-inl.h | 2 +- cpp/BayesNet.h | 4 +-- cpp/BayesTree-inl.h | 45 ++++++++++++------------ cpp/BayesTree.h | 51 ++++++++++------------------ cpp/ConstrainedLinearFactorGraph.cpp | 4 +-- cpp/LinearFactorGraph.cpp | 2 +- cpp/SymbolicFactorGraph.cpp | 2 +- cpp/smallExample.cpp | 4 +-- cpp/testBayesTree.cpp | 35 +++++++++++-------- cpp/testLinearFactorGraph.cpp | 6 ++-- cpp/testSymbolicBayesNet.cpp | 6 ++-- cpp/testSymbolicFactorGraph.cpp | 6 ++-- 13 files changed, 94 insertions(+), 106 deletions(-) diff --git a/.cproject b/.cproject index 093b27c0a..aa9dd3b86 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + install true true @@ -307,6 +308,7 @@ make + check true true @@ -314,14 +316,15 @@ make - +-k check true -true +false true make + testSimpleCamera.run true true @@ -337,7 +340,6 @@ make - testVSLAMFactor.run true true @@ -345,6 +347,7 @@ make + testCalibratedCamera.run true true @@ -352,7 +355,6 @@ make - testConditionalGaussian.run true true @@ -360,6 +362,7 @@ make + testPose2.run true true @@ -375,6 +378,7 @@ make + testRot3.run true true @@ -382,7 +386,6 @@ make - testNonlinearOptimizer.run true true @@ -390,6 +393,7 @@ make + testLinearFactor.run true true @@ -397,6 +401,7 @@ make + testConstrainedNonlinearFactorGraph.run true true @@ -404,6 +409,7 @@ make + testLinearFactorGraph.run true true @@ -411,7 +417,6 @@ make - testNonlinearFactorGraph.run true true @@ -419,6 +424,7 @@ make + testPose3.run true true @@ -426,7 +432,6 @@ make - testConstrainedLinearFactorGraph.run true true @@ -434,7 +439,6 @@ make - testVectorConfig.run true true @@ -442,7 +446,6 @@ make - testPoint2.run true true @@ -450,6 +453,7 @@ make + testNonlinearFactor.run true true @@ -457,6 +461,7 @@ make + timeLinearFactor.run true true @@ -464,6 +469,7 @@ make + timeLinearFactorGraph.run true true @@ -471,6 +477,7 @@ make + testGaussianBayesNet.run true true @@ -478,7 +485,6 @@ make - testBayesTree.run true false @@ -486,7 +492,6 @@ make - testSymbolicBayesChain.run true false @@ -494,7 +499,6 @@ make - testSymbolicFactorGraph.run true false @@ -502,6 +506,7 @@ make + testVector.run true true @@ -509,6 +514,7 @@ make + testMatrix.run true true @@ -516,7 +522,6 @@ make - install true true @@ -524,7 +529,6 @@ make - clean true true @@ -532,7 +536,6 @@ make - check true true diff --git a/cpp/BayesNet-inl.h b/cpp/BayesNet-inl.h index ada9d0a08..321ef7b87 100644 --- a/cpp/BayesNet-inl.h +++ b/cpp/BayesNet-inl.h @@ -37,7 +37,7 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesNet::insert + void BayesNet::push_back (const boost::shared_ptr& conditional) { indices_.insert(make_pair(conditional->key(),conditionals_.size())); conditionals_.push_back(conditional); diff --git a/cpp/BayesNet.h b/cpp/BayesNet.h index 4a9afed33..d22526b7e 100644 --- a/cpp/BayesNet.h +++ b/cpp/BayesNet.h @@ -67,8 +67,8 @@ namespace gtsam { /** check equality */ bool equals(const BayesNet& other, double tol = 1e-9) const; - /** insert: use reverse topological sort (i.e. parents last / elimination order) */ - void insert(const boost::shared_ptr& conditional); + /** push_back: use reverse topological sort (i.e. parents last / elimination order) */ + void push_back(const boost::shared_ptr& conditional); /** size is the number of nodes */ inline size_t size() const { diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index f1897900c..472110179 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -13,36 +13,32 @@ namespace gtsam { /* ************************************************************************* */ template - Front::Front(const conditional_ptr& conditional) { - add(conditional); - separator_ = conditional->parents(); - } - - /* ************************************************************************* */ - template - void Front::print(const string& s) const { - cout << s; - BOOST_FOREACH(const conditional_ptr& conditional, conditionals_) - cout << " " << conditional->key(); - if (!separator_.empty()) { - cout << " :"; - BOOST_FOREACH(string key, separator_) - cout << " " << key; + BayesTree::Node::Node(const boost::shared_ptr& conditional) { + separator_ = conditional->parents(); + this->push_back(conditional); } - cout << endl; - } /* ************************************************************************* */ template - bool Front::equals(const Front& other, double tol) const { - return equal(conditionals_.begin(),conditionals_.end(),other.conditionals_.begin(),equals_star); - } + void BayesTree::Node::print(const string& s) const { + cout << s; + BOOST_REVERSE_FOREACH(const conditional_ptr& conditional, this->conditionals_) + cout << " " << conditional->key(); + if (!separator_.empty()) { + cout << " :"; + BOOST_FOREACH(string key, separator_) + cout << " " << key; + } + cout << endl; + } /* ************************************************************************* */ template - void Front::add(const conditional_ptr& conditional) { - conditionals_.push_front(conditional); - } + void BayesTree::Node::printTree(const string& indent) const { + print(indent); + BOOST_FOREACH(shared_ptr child, children_) + child->printTree(indent+" "); + } /* ************************************************************************* */ template @@ -104,12 +100,13 @@ namespace gtsam { "BayesTree::insert('"+key+"'): parent '" + parent + "' was not yet inserted")); int index = it->second; node_ptr parent_clique = nodes_[index]; + if (verbose) cout << "Parent clique " << index << " of size " << parent_clique->size() << endl; // 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(conditional); + parent_clique->push_back(conditional); return; } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 1c4aeb8a4..7d4e9ec22 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -13,36 +13,12 @@ #include #include #include + #include "Testable.h" #include "BayesNet.h" namespace gtsam { - /** A clique in a Bayes tree consisting of frontal nodes and conditionals */ - template - class Front: Testable > { - private: - typedef boost::shared_ptr conditional_ptr; - std::list conditionals_; /** conditionals */ - std::list separator_; /** separator keys */ - public: - - /** constructor */ - Front(const conditional_ptr& conditional); - - /** print */ - void print(const std::string& s = "") const; - - /** check equality */ - bool equals(const Front& other, double tol = 1e-9) const; - - /** add a frontal node */ - void add(const conditional_ptr& conditional); - - /** return size of the clique */ - inline size_t size() const {return conditionals_.size() + separator_.size();} - }; - /** * Bayes tree * Templated on the Conditional class, the type of node in the underlying Bayes chain. @@ -58,20 +34,27 @@ namespace gtsam { private: - /** A Node in the tree is a Front with tree connectivity */ - struct Node : public Front { + /** A Node 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 Node : public BayesNet { typedef boost::shared_ptr shared_ptr; shared_ptr parent_; + std::list separator_; /** separator keys */ std::list children_; - Node(const boost::shared_ptr& conditional):Front(conditional) {} + //* Constructor */ + Node(const boost::shared_ptr& conditional); + + /** The size *includes* the separator */ + size_t size() const { return this->conditionals_.size() + separator_.size(); } + + /** print this node */ + void print(const std::string& s="Bayes tree node") const; /** print this node and entire subtree below it*/ - void printTree(const std::string& indent) const { - print(indent); - BOOST_FOREACH(shared_ptr child, children_) - child->printTree(indent+" "); - } + void printTree(const std::string& indent) const; }; /** vector of Nodes */ @@ -107,7 +90,7 @@ namespace gtsam { inline size_t size() const { return nodes_.size();} /** return root clique */ - const Front& root() const {return *(nodes_[0]);} + const BayesNet& root() const {return *(nodes_[0]);} }; // BayesTree diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index cc20d9b17..13b2e8e65 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -79,12 +79,12 @@ GaussianBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Order if (is_constrained(key)) { ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key); - cbn->insert(ccg); + cbn->push_back(ccg); } else { ConditionalGaussian::shared_ptr cg = eliminateOne(key); - cbn->insert(cg); + cbn->push_back(cg); } } diff --git a/cpp/LinearFactorGraph.cpp b/cpp/LinearFactorGraph.cpp index 3a0e3fd61..0df294cb0 100644 --- a/cpp/LinearFactorGraph.cpp +++ b/cpp/LinearFactorGraph.cpp @@ -63,7 +63,7 @@ LinearFactorGraph::eliminate_partially(const Ordering& ordering) BOOST_FOREACH(string key, ordering) { ConditionalGaussian::shared_ptr cg = eliminateOne(key); - chordalBayesNet->insert(cg); + chordalBayesNet->push_back(cg); } return chordalBayesNet; diff --git a/cpp/SymbolicFactorGraph.cpp b/cpp/SymbolicFactorGraph.cpp index e305d8273..d36588b3d 100644 --- a/cpp/SymbolicFactorGraph.cpp +++ b/cpp/SymbolicFactorGraph.cpp @@ -26,7 +26,7 @@ namespace gtsam { BOOST_FOREACH(string key, ordering) { SymbolicConditional::shared_ptr conditional = eliminateOne(key); - bayesNet->insert(conditional); + bayesNet->push_back(conditional); } return bayesNet; diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index fc056ef05..df1fdb56f 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -203,8 +203,8 @@ GaussianBayesNet createSmallGaussianBayesNet() Px_y(new ConditionalGaussian("x",d1,R11,"y",S12)), Py(new ConditionalGaussian("y",d2,R22)); GaussianBayesNet cbn; - cbn.insert(Px_y); - cbn.insert(Py); + cbn.push_back(Px_y); + cbn.push_back(Py); return cbn; } diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 41a0bbb43..dbcb6b3be 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -27,10 +27,12 @@ SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L( /* ************************************************************************* */ TEST( BayesTree, Front ) { - Front f1(B); - f1.add(L); - Front f2(L); - f2.add(B); + BayesNet f1; + f1.push_back(B); + f1.push_back(L); + BayesNet f2; + f2.push_back(L); + f2.push_back(B); CHECK(f1.equals(f1)); CHECK(!f1.equals(f2)); } @@ -51,21 +53,24 @@ TEST( BayesTree, constructor ) LONGS_EQUAL(4,bayesTree.size()); // Check root - Front expected_root(B); - expected_root.add(L); - expected_root.add(E); - Front actual_root = bayesTree.root(); + BayesNet expected_root; + expected_root.push_back(B); + expected_root.push_back(L); + expected_root.push_back(E); + BayesNet 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; - ASIA.insert(X); - ASIA.insert(T); - ASIA.insert(S); - ASIA.insert(E); - ASIA.insert(L); - ASIA.insert(B); - BayesTree bayesTree2(ASIA); + ASIA.push_back(X); + ASIA.push_back(T); + ASIA.push_back(S); + ASIA.push_back(E); + ASIA.push_back(L); + ASIA.push_back(B); + bool verbose = true; + BayesTree bayesTree2(ASIA,verbose); + if (verbose) bayesTree2.print("bayesTree2"); // Check whether the same CHECK(assert_equal(bayesTree,bayesTree2)); diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index 043487e53..931c20ba1 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -276,9 +276,9 @@ TEST( LinearFactorGraph, eliminateAll ) ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian("x2",d3, R3, "l1", A21, "x1", A22)); GaussianBayesNet expected; - expected.insert(cg3); - expected.insert(cg2); - expected.insert(cg1); + expected.push_back(cg3); + expected.push_back(cg2); + expected.push_back(cg1); // Check one ordering LinearFactorGraph fg1 = createLinearFactorGraph(); diff --git a/cpp/testSymbolicBayesNet.cpp b/cpp/testSymbolicBayesNet.cpp index 8db84e20a..10b423237 100644 --- a/cpp/testSymbolicBayesNet.cpp +++ b/cpp/testSymbolicBayesNet.cpp @@ -27,9 +27,9 @@ TEST( SymbolicBayesNet, constructor ) l1(new SymbolicConditional("l1","x1")), x1(new SymbolicConditional("x1")); SymbolicBayesNet expected; - expected.insert(x2); - expected.insert(l1); - expected.insert(x1); + expected.push_back(x2); + expected.push_back(l1); + expected.push_back(x1); // Create from a factor graph LinearFactorGraph factorGraph = createLinearFactorGraph(); diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index 0a451e0d0..060273f88 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -129,9 +129,9 @@ TEST( LinearFactorGraph, eliminate ) SymbolicConditional::shared_ptr x1(new SymbolicConditional("x1")); SymbolicBayesNet expected; - expected.insert(x2); - expected.insert(l1); - expected.insert(x1); + expected.push_back(x2); + expected.push_back(l1); + expected.push_back(x1); // create a test graph LinearFactorGraph factorGraph = createLinearFactorGraph();