diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 75081b8b6..5374c48bb 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -34,7 +34,7 @@ namespace gtsam { template void BayesTree::Clique::print(const string& s) const { cout << s; - BOOST_REVERSE_FOREACH(const sharedConditional& conditional, this->conditionals_) + BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) cout << " " << conditional->key(); if (!separator_.empty()) { cout << " :"; @@ -199,6 +199,11 @@ namespace gtsam { const pair::sharedClique >& v1, const pair::sharedClique >& v2 ) { +// cout << v1.first << " =? " << v2.first << endl; +// cout << (v1.first == v2.first) << endl; +// cout << v1.second->equals(*(v2.second)) << endl; +// v1.second->print("v1"); +// v2.second->print("v2"); return v1.first == v2.first && v1.second->equals(*(v2.second)); } diff --git a/cpp/testIncremental.cpp b/cpp/testIncremental.cpp index 5bc6cedbc..31db87cab 100644 --- a/cpp/testIncremental.cpp +++ b/cpp/testIncremental.cpp @@ -25,7 +25,7 @@ typedef BayesTree GaussianBayesTree; // Conditionals for ASIA example from the tutorial with A and D evidence SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L( new SymbolicConditional("L", "B")), E( - new SymbolicConditional("E", "L", "B")), S(new SymbolicConditional("S", + new SymbolicConditional("E", "B", "L")), S(new SymbolicConditional("S", "L", "B")), T(new SymbolicConditional("T", "E", "L")), X( new SymbolicConditional("X", "E")); @@ -34,9 +34,8 @@ SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L( SymbolicBayesTree update(const SymbolicBayesTree& initial, const boost::shared_ptr& newFactor) { - // create a factor graph with the new factor in it + // create an empty factor graph SymbolicFactorGraph factorGraph; - factorGraph.push_back(newFactor); // get the ELB clique SymbolicBayesTree::sharedClique ELB = initial["B"]; @@ -52,6 +51,9 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial, // add it to the factor graph factorGraph = combine(factorGraph, SLB_factors); + // now add the new factor + factorGraph.push_back(newFactor); + // create an ordering ESLB Ordering ordering; ordering += "E","S","L","B"; @@ -62,13 +64,14 @@ SymbolicBayesTree update(const SymbolicBayesTree& initial, // turn back into a Bayes Tree BayesTree newTree(bayesNet); - // add orphans to the bottom of the new tree - // get the ophan cliques + // get the orphan cliques SymbolicBayesTree::sharedClique TEL = initial["T"]; SymbolicBayesTree::sharedClique XE = initial["X"]; + // get clique from new tree to attach to SymbolicBayesTree::sharedClique new_ELB = newTree["E"]; + // add orphans to the bottom of the new tree new_ELB->children_ += TEL,XE; return newTree; @@ -85,7 +88,6 @@ TEST( BayesTree, iSAM ) bayesTree.insert(S); bayesTree.insert(T); bayesTree.insert(X); - //bayesTree.print("bayesTree"); // Create expected Bayes tree SymbolicBayesTree expected; @@ -95,7 +97,6 @@ TEST( BayesTree, iSAM ) expected.insert(E); expected.insert(T); expected.insert(X); - //expected.print("expected"); // create a new factor to be inserted list keys; @@ -106,7 +107,7 @@ TEST( BayesTree, iSAM ) SymbolicBayesTree actual = update(bayesTree, newFactor); // Check whether the same - // CHECK(assert_equal(expected,actual)); + //CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */