diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 01bace854..fa72de202 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -237,6 +237,10 @@ namespace gtsam { void BayesTree::print(const string& s) const { cout << s << ": size == " << size() << endl; if (nodes_.empty()) return; + if (root_.use_count() == 0) { + printf("WARNING: Forest...\n"); + return; + } root_->printTree(""); } @@ -367,7 +371,7 @@ namespace gtsam { template template pair, typename BayesTree::Cliques> - BayesTree::removePath(sharedClique clique) { + ISAM::removePath(sharedClique clique) { FactorGraph factors; Cliques orphans; @@ -376,10 +380,10 @@ namespace gtsam { if (clique!=NULL) { // remove me - removeClique(clique); + this->removeClique(clique); // remove path above me - boost::tie(factors,orphans) = removePath(clique->parent_); + boost::tie(factors,orphans) = this->removePath(clique->parent_); // add children to list of orphans (splice also removed them from clique->children_) orphans.splice (orphans.begin(), clique->children_); @@ -387,8 +391,9 @@ namespace gtsam { // Convert clique to a factor graph, using constructor in FactorGraph FactorGraph clique_factors(*clique); - // add to the list of "imvalidated" factors + // add to the list of "invalidated" factors factors.push_back(clique_factors); + } return make_pair(factors,orphans); @@ -398,7 +403,7 @@ namespace gtsam { // TODO: add to factors and orphans template template - void BayesTree::removeTop(const boost::shared_ptr& newFactor, + void ISAM::removeTop(const boost::shared_ptr& newFactor, FactorGraph &factors, typename BayesTree::Cliques& orphans) { // process each key of the new factor @@ -410,11 +415,12 @@ namespace gtsam { // remove path from clique to root FactorGraph factors1; Cliques orphans1; - boost::tie(factors1,orphans1) = removePath(clique); + boost::tie(factors1,orphans1) = this->removePath(clique); // add to global factors and orphans factors.push_back(factors1); orphans.splice (orphans.begin(), orphans1); + } catch (std::invalid_argument e) { } } @@ -423,11 +429,11 @@ namespace gtsam { template template pair, typename BayesTree::Cliques> - BayesTree::removeTop(const FactorGraph& newFactors) { - + ISAM::removeTop(const FactorGraph& newFactors) { // Remove the contaminated part of the Bayes tree FactorGraph factors; Cliques orphans; + BOOST_FOREACH(boost::shared_ptr factor, newFactors) this->removeTop(factor, factors, orphans); @@ -437,17 +443,24 @@ namespace gtsam { /* ************************************************************************* */ template template - void BayesTree::update_internal(const FactorGraph& newFactors, Cliques& orphans) { + void ISAM::update_internal(const FactorGraph& newFactors, Cliques& orphans) { // Remove the contaminated part of the Bayes tree FactorGraph factors; - boost::tie(factors, orphans) = removeTop(newFactors); + boost::tie(factors, orphans) = this->removeTop(newFactors); // add the factors themselves factors.push_back(newFactors); // create an ordering for the new and contaminated factors - Ordering ordering = factors.getOrdering(); + Ordering ordering; + if (true) { + ordering = factors.getOrdering(); + } else { + list keys = factors.keys(); + keys.sort(); // todo: correct sorting order? + ordering = keys; + } // eliminate into a Bayes net BayesNet bayesNet = eliminate(factors,ordering); @@ -455,7 +468,7 @@ namespace gtsam { // insert conditionals back in, straight into the topless bayesTree typename BayesNet::const_reverse_iterator rit; for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) - insert(*rit); + this->insert(*rit); int count = 0; // add orphans to the bottom of the new tree @@ -472,9 +485,9 @@ namespace gtsam { template template - void BayesTree::update(const FactorGraph& newFactors) { + void ISAM::update(const FactorGraph& newFactors) { Cliques orphans; - update_internal(newFactors, orphans); + this->update_internal(newFactors, orphans); } /* ************************************************************************* */ diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 03765a23f..e5baa73aa 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -104,6 +104,8 @@ namespace gtsam { sharedClique addClique(const sharedConditional& conditional, sharedClique parent_clique = sharedClique()); + protected: + /** remove a clique: warning, can result in a forest */ void removeClique(sharedClique clique); @@ -163,6 +165,27 @@ namespace gtsam { template BayesNet jointBayesNet(const std::string& key1, const std::string& key2) const; + }; // BayesTree + + template + class ISAM: public BayesTree { + + public: + + /** Create an empty Bayes Tree */ + ISAM() : BayesTree() {} + + /** Create a Bayes Tree from a Bayes Net */ + ISAM(const BayesNet& bayesNet) : BayesTree(bayesNet) {} + + /** Destructor */ + virtual ~ISAM() { + } + + typedef typename BayesTree::sharedClique sharedClique; + + typedef typename BayesTree::Cliques Cliques; + /** * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. @@ -195,6 +218,6 @@ namespace gtsam { template void update(const FactorGraph& newFactors); - }; // BayesTree + }; // ISAM } /// namespace gtsam diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index adc7c696b..81665c7fe 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -20,6 +20,10 @@ using namespace boost::assign; using namespace gtsam; typedef BayesTree SymbolicBayesTree; +typedef ISAM SymbolicISAM; + +//template class BayesTree; // todo: needed? +//template class ISAM; /* ************************************************************************* */ // SLAM example from RSS sqrtSAM paper @@ -30,9 +34,9 @@ SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")), l2(new SymbolicConditional("l2","x1","x3")); // Bayes Tree for sqrtSAM example -SymbolicBayesTree createSlamSymbolicBayesTree(){ +SymbolicISAM createSlamSymbolicBayesTree(){ // Create using insert - SymbolicBayesTree bayesTree_slam; + SymbolicISAM bayesTree_slam; bayesTree_slam.insert(x3); bayesTree_slam.insert(x2); bayesTree_slam.insert(x1); @@ -54,8 +58,8 @@ SymbolicConditional::shared_ptr X(new SymbolicConditional("X", "E")); // Bayes Tree for Asia example -SymbolicBayesTree createAsiaSymbolicBayesTree() { - SymbolicBayesTree bayesTree; +SymbolicISAM createAsiaSymbolicBayesTree() { + SymbolicISAM bayesTree; bayesTree.insert(B); bayesTree.insert(L); bayesTree.insert(E); @@ -124,7 +128,7 @@ TEST( BayesTree, removePath ) D(new SymbolicConditional("D", "C")), E(new SymbolicConditional("E", "B")), F(new SymbolicConditional("F", "E")); - SymbolicBayesTree bayesTree; + SymbolicISAM bayesTree; bayesTree.insert(A); bayesTree.insert(B); bayesTree.insert(C); @@ -138,11 +142,11 @@ TEST( BayesTree, removePath ) expected.push_factor("A","B"); expected.push_factor("A"); expected.push_factor("A","C"); - SymbolicBayesTree::Cliques expectedOrphans; + SymbolicISAM::Cliques expectedOrphans; expectedOrphans += bayesTree["D"], bayesTree["E"]; FactorGraph factors; - SymbolicBayesTree::Cliques orphans; + SymbolicISAM::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["C"]); CHECK(assert_equal((FactorGraph)expected, factors)); CHECK(assert_equal(expectedOrphans, orphans)); @@ -150,7 +154,7 @@ TEST( BayesTree, removePath ) // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected2; expected2.push_factor("B","E"); - SymbolicBayesTree::Cliques expectedOrphans2; + SymbolicISAM::Cliques expectedOrphans2; expectedOrphans2 += bayesTree["F"]; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["E"]); @@ -161,11 +165,11 @@ TEST( BayesTree, removePath ) /* ************************************************************************* */ TEST( BayesTree, removePath2 ) { - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); + SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique B FactorGraph factors; - SymbolicBayesTree::Cliques orphans; + SymbolicISAM::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["B"]); // Check expected outcome @@ -174,7 +178,7 @@ TEST( BayesTree, removePath2 ) expected.push_factor("B","L"); expected.push_factor("B"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; + SymbolicISAM::Cliques expectedOrphans; expectedOrphans += bayesTree["S"], bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); } @@ -182,11 +186,11 @@ TEST( BayesTree, removePath2 ) /* ************************************************************************* */ TEST( BayesTree, removePath3 ) { - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); + SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique S FactorGraph factors; - SymbolicBayesTree::Cliques orphans; + SymbolicISAM::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["S"]); // Check expected outcome @@ -196,22 +200,22 @@ TEST( BayesTree, removePath3 ) expected.push_factor("B"); expected.push_factor("L","B","S"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; + SymbolicISAM::Cliques expectedOrphans; expectedOrphans += bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); } /* ************************************************************************* */ -TEST( BayesTree, removeTop ) +TEST( ISAM, removeTop ) { - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); + SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); // create a new factor to be inserted boost::shared_ptr newFactor(new SymbolicFactor("B","S")); // Remove the contaminated part of the Bayes tree FactorGraph factors; - SymbolicBayesTree::Cliques orphans; + SymbolicISAM::Cliques orphans; bayesTree.removeTop(newFactor, factors, orphans); // Check expected outcome @@ -221,26 +225,26 @@ TEST( BayesTree, removeTop ) expected.push_factor("B"); expected.push_factor("L","B","S"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; + SymbolicISAM::Cliques expectedOrphans; expectedOrphans += bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); // Try removeTop again with a factor that should not change a thing boost::shared_ptr newFactor2(new SymbolicFactor("B")); FactorGraph factors2; - SymbolicBayesTree::Cliques orphans2; + SymbolicISAM::Cliques orphans2; bayesTree.removeTop(newFactor2, factors2, orphans2); SymbolicFactorGraph expected2; CHECK(assert_equal((FactorGraph)expected2, factors2)); - SymbolicBayesTree::Cliques expectedOrphans2; + SymbolicISAM::Cliques expectedOrphans2; CHECK(assert_equal(expectedOrphans2, orphans2)); } /* ************************************************************************* */ -TEST( BayesTree, removeTop2 ) +TEST( ISAM, removeTop2 ) { - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); + SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); // create two factors to be inserted SymbolicFactorGraph newFactors; @@ -249,7 +253,7 @@ TEST( BayesTree, removeTop2 ) // Remove the contaminated part of the Bayes tree FactorGraph factors; - SymbolicBayesTree::Cliques orphans; + SymbolicISAM::Cliques orphans; boost::tie(factors,orphans) = bayesTree.removeTop(newFactors); // Check expected outcome @@ -259,15 +263,15 @@ TEST( BayesTree, removeTop2 ) expected.push_factor("B"); expected.push_factor("L","B","S"); CHECK(assert_equal((FactorGraph)expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; + SymbolicISAM::Cliques expectedOrphans; expectedOrphans += bayesTree["T"], bayesTree["X"]; CHECK(assert_equal(expectedOrphans, orphans)); } /* ************************************************************************* */ -TEST( BayesTree, iSAM ) +TEST( ISAM, iSAM ) { - SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); + SymbolicISAM bayesTree = createAsiaSymbolicBayesTree(); // Now we modify the Bayes tree by inserting a new factor over B and S @@ -279,7 +283,7 @@ TEST( BayesTree, iSAM ) B_(new SymbolicConditional("B", "E", "L", "S")); // Create expected Bayes tree - SymbolicBayesTree expected; + SymbolicISAM expected; expected.insert(S_); expected.insert(L_); expected.insert(E_); @@ -298,19 +302,19 @@ TEST( BayesTree, iSAM ) // Check whether the same CHECK(assert_equal(expected,bayesTree)); } + /* ************************************************************************* */ -TEST( BayesTree, iSAM_slam ) +TEST( ISAM, iSAM_slam ) { // Create using insert - SymbolicBayesTree bayesTree_slam = createSlamSymbolicBayesTree(); + SymbolicISAM bayesTree_slam = createSlamSymbolicBayesTree(); //New conditionals for the expected Bayes tree - SymbolicConditional::shared_ptr l1_(new SymbolicConditional("l1","x1","x2","x3")); // Create expected Bayes tree - SymbolicBayesTree expected_slam; + SymbolicISAM expected_slam; expected_slam.insert(x3); expected_slam.insert(x2); expected_slam.insert(x1); @@ -327,7 +331,7 @@ TEST( BayesTree, iSAM_slam ) bayesTree_slam.update(factorGraph_slam); // Check whether the same - CHECK(assert_equal(expected_slam,bayesTree_slam)); + CHECK(assert_equal(expected_slam,bayesTree_slam)); } /* ************************************************************************* */