diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 6807b00e0..5e82695eb 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -397,18 +397,12 @@ namespace gtsam { /* ************************************************************************* */ template - template void BayesTree::removePath(sharedClique clique, - FactorGraph &factors, typename BayesTree::Cliques& orphans) { + BayesNet& bn, typename BayesTree::Cliques& orphans) { // base case is NULL, if so we do nothing and return empties above if (clique!=NULL) { -#if 0 - printf("++++++ removing\n"); - clique->print(); -#endif - // remove the clique from orphans in case it has been added earlier orphans.remove(clique); @@ -416,32 +410,20 @@ namespace gtsam { this->removeClique(clique); // remove path above me - this->removePath(clique->parent_, factors, orphans); + this->removePath(clique->parent_, bn, orphans); // add children to list of orphans (splice also removed them from clique->children_) orphans.splice (orphans.begin(), clique->children_); - // Convert clique to a factor graph, using constructor in FactorGraph - FactorGraph clique_factors(*clique); - - // add to the list of "invalidated" factors - factors.push_back(clique_factors); - -#if 0 - printf("++++++ factors\n"); - factors.print(); - printf("++++++ orphans\n"); - orphans.print(); -#endif + bn.push_back(*clique); } } /* ************************************************************************* */ template - template void BayesTree::removeTop(const list& keys, - FactorGraph &factors, typename BayesTree::Cliques& orphans) { + BayesNet& bn, typename BayesTree::Cliques& orphans) { // process each key of the new factor BOOST_FOREACH(const Symbol& key, keys) @@ -450,7 +432,7 @@ namespace gtsam { sharedClique clique = (*this)[key]; // remove path from clique to root - this->removePath(clique, factors, orphans); + this->removePath(clique, bn, orphans); } catch (std::invalid_argument e) { } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 2ad5f5d70..4a1855ad6 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -181,16 +181,13 @@ namespace gtsam { * Remove path from clique to root and return that path as factors * plus a list of orphaned subtree roots. Used in removeTop below. */ - template - void removePath(sharedClique clique, FactorGraph &factors, Cliques& orphans); + void removePath(sharedClique clique, BayesNet& bn, Cliques& orphans); /** * Given a list of keys, turn "contaminated" part of the tree back into a factor graph. * Factors and orphans are added to the in/out arguments. */ - template - void removeTop(const std::list& keys, - FactorGraph &factors, Cliques& orphans); + void removeTop(const std::list& keys, BayesNet& bn, Cliques& orphans); }; // BayesTree diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h index a05aae622..4e83aa2d3 100644 --- a/cpp/ISAM-inl.h +++ b/cpp/ISAM-inl.h @@ -31,8 +31,9 @@ namespace gtsam { void ISAM::update_internal(const FactorGraph& newFactors, Cliques& orphans) { // Remove the contaminated part of the Bayes tree - FactorGraph factors; - removeTop(newFactors.keys(), factors, orphans); + BayesNet bn; + removeTop(newFactors.keys(), bn, orphans); + FactorGraph factors(bn); // add the factors themselves factors.push_back(newFactors); diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index eded8f26e..350f7ad88 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -137,9 +137,6 @@ namespace gtsam { void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, const Config& config, Cliques& orphans) { -#if 1 // 0=skip most, do batch - - FactorGraph affectedFactors; list newFactorsKeys = newFactors.keys(); #if 1 // 0=relinearize all in each step @@ -163,7 +160,8 @@ namespace gtsam { #endif // remove affected factors - this->removeTop(keysToBeRemoved, affectedFactors, orphans); + BayesNet affectedBayesNet; + this->removeTop(keysToBeRemoved, affectedBayesNet, orphans); // selectively update the linearization point VectorConfig selected_delta; @@ -174,21 +172,12 @@ namespace gtsam { linPoint_ = expmap(linPoint_, selected_delta); // relinearize the affected factors ... - list affectedKeys = affectedFactors.keys(); + list affectedKeys = affectedBayesNet.ordering(); // all keys in conditionals, there cannot be others because path to root included FactorGraph factors = relinearizeAffectedFactors(affectedKeys); // ... add the cached intermediate results from the boundary of the orphans ... FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); factors.push_back(cachedBoundary); -#else - // todo - debug only: batch operation - FactorGraph affectedFactors; - list keysToBeRemoved = nonlinearFactors_.keys(); - this->removeTop(keysToBeRemoved, affectedFactors, orphans); - this->print("---------------"); - linPoint_ = expmap(linPoint_, delta_); // todo-debug only - FactorGraph factors = nonlinearFactors_.linearize(linPoint_); -#endif // add new variables linPoint_.insert(config); diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 950a11536..ae094aa2a 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -155,9 +155,10 @@ TEST( BayesTree, removePath ) SymbolicBayesTree::Cliques expectedOrphans; expectedOrphans += bayesTree["D"], bayesTree["E"]; - FactorGraph factors; + BayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree["C"], factors, orphans); + bayesTree.removePath(bayesTree["C"], bn, orphans); + FactorGraph factors(bn); CHECK(assert_equal((FactorGraph)expected, factors)); CHECK(assert_equal(expectedOrphans, orphans)); @@ -167,9 +168,10 @@ TEST( BayesTree, removePath ) SymbolicBayesTree::Cliques expectedOrphans2; expectedOrphans2 += bayesTree["F"]; - FactorGraph factors2; + BayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removePath(bayesTree["E"], factors2, orphans2); + bayesTree.removePath(bayesTree["E"], bn2, orphans2); + FactorGraph factors2(bn2); CHECK(assert_equal((FactorGraph)expected2, factors2)); CHECK(assert_equal(expectedOrphans2, orphans2)); } @@ -180,9 +182,10 @@ TEST( BayesTree, removePath2 ) SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique B - FactorGraph factors; + BayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree["B"], factors, orphans); + bayesTree.removePath(bayesTree["B"], bn, orphans); + FactorGraph factors(bn); // Check expected outcome SymbolicFactorGraph expected; @@ -201,9 +204,10 @@ TEST( BayesTree, removePath3 ) SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); // Call remove-path with clique S - FactorGraph factors; + BayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removePath(bayesTree["S"], factors, orphans); + bayesTree.removePath(bayesTree["S"], bn, orphans); + FactorGraph factors(bn); // Check expected outcome SymbolicFactorGraph expected; @@ -226,9 +230,10 @@ TEST( BayesTree, removeTop ) boost::shared_ptr newFactor(new SymbolicFactor("B","S")); // Remove the contaminated part of the Bayes tree - FactorGraph factors; + BayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(newFactor->keys(), factors, orphans); + bayesTree.removeTop(newFactor->keys(), bn, orphans); + FactorGraph factors(bn); // Check expected outcome SymbolicFactorGraph expected; @@ -243,9 +248,10 @@ TEST( BayesTree, removeTop ) // Try removeTop again with a factor that should not change a thing boost::shared_ptr newFactor2(new SymbolicFactor("B")); - FactorGraph factors2; + BayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removeTop(newFactor2->keys(), factors2, orphans2); + bayesTree.removeTop(newFactor2->keys(), bn2, orphans2); + FactorGraph factors2(bn2); SymbolicFactorGraph expected2; CHECK(assert_equal((FactorGraph)expected2, factors2)); SymbolicBayesTree::Cliques expectedOrphans2; @@ -263,9 +269,10 @@ TEST( BayesTree, removeTop2 ) newFactors.push_factor("S"); // Remove the contaminated part of the Bayes tree - FactorGraph factors; + BayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(newFactors.keys(), factors, orphans); + bayesTree.removeTop(newFactors.keys(), bn, orphans); + FactorGraph factors(bn); // Check expected outcome SymbolicFactorGraph expected; @@ -298,9 +305,10 @@ TEST( BayesTree, removeTop3 ) // remove all list keys; keys += "l5", "x2", "x3", "x4"; - FactorGraph factors; + BayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(keys, factors, orphans); + bayesTree.removeTop(keys, bn, orphans); + FactorGraph factors(bn); CHECK(orphans.size() == 0); }