diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 547b7ed1b..f845797f4 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -264,11 +264,11 @@ namespace gtsam { /* ************************************************************************* */ template - void BayesTree::insert(const sharedConditional& conditional) + void BayesTree::insert(const sharedConditional& conditional, const list* ordering) { // get key and parents const Symbol& key = conditional->key(); - list parents = conditional->parents(); // rtodo: const reference? + list parents = conditional->parents(); // todo: const reference? // if no parents, start a new root clique if (parents.empty()) { @@ -277,7 +277,18 @@ namespace gtsam { } // otherwise, find the parent clique - Symbol parent = parents.front(); + Symbol parent; + if (!ordering) { + parent = parents.front(); // assumes parents are in current variable order, which is not the case (after COLAMD was activated) + } else { + for (list::const_iterator it = ordering->begin(); it!=ordering->end(); it++) { + list::iterator pit = find(parents.begin(), parents.end(), *it); + if (pit!=parents.end()) { + parent = *pit; + break; + } + } + } sharedClique parent_clique = (*this)[parent]; // if the parents and parent clique have the same size, add to parent clique @@ -370,20 +381,25 @@ namespace gtsam { /* ************************************************************************* */ template template - pair, typename BayesTree::Cliques> - BayesTree::removePath(sharedClique clique) { - - FactorGraph factors; - Cliques orphans; + void BayesTree::removePath(sharedClique clique, + FactorGraph &factors, 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); + // remove me this->removeClique(clique); // remove path above me - boost::tie(factors,orphans) = this->removePath(clique->parent_); + this->removePath(clique->parent_, factors, orphans); // add children to list of orphans (splice also removed them from clique->children_) orphans.splice (orphans.begin(), clique->children_); @@ -394,31 +410,30 @@ namespace gtsam { // 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 - return make_pair(factors,orphans); + } } /* ************************************************************************* */ template template void BayesTree::removeTop(const list& keys, - FactorGraph &factors, typename BayesTree::Cliques& orphans) { + FactorGraph &factors, typename BayesTree::Cliques& orphans) { // process each key of the new factor BOOST_FOREACH(const Symbol& key, keys) try { - // get the clique and remove it from orphans (if it exists) + // get the clique sharedClique clique = (*this)[key]; - orphans.remove(clique); // remove path from clique to root - FactorGraph factors1; Cliques orphans1; - boost::tie(factors1,orphans1) = this->removePath(clique); - - // add to global factors and orphans - factors.push_back(factors1); - orphans.splice (orphans.begin(), orphans1); + this->removePath(clique, factors, orphans); } catch (std::invalid_argument e) { } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 30ae356bf..52305f9c2 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -129,7 +129,7 @@ namespace gtsam { bool equals(const BayesTree& other, double tol = 1e-9) const; /** insert a new conditional */ - void insert(const sharedConditional& conditional); + void insert(const sharedConditional& conditional, const std::list* ordering = NULL); /** number of cliques */ inline size_t size() const { @@ -171,7 +171,7 @@ namespace gtsam { * plus a list of orphaned subtree roots. Used in removeTop below. */ template - std::pair, Cliques> removePath(sharedClique clique); + void removePath(sharedClique clique, FactorGraph &factors, Cliques& orphans); /** * Given a list of keys, turn "contaminated" part of the tree back into a factor graph. diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index e03d37e85..4ddaaf9b8 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -34,7 +34,7 @@ namespace gtsam { boost::shared_ptr conditional; boost::tie(conditional, factor) = joint_factor->eliminate(key); - // remember the intermediate result to be able to later restart computation in the middle + // ADDED: remember the intermediate result to be able to later restart computation in the middle cached[key] = factor; // add new factor on separator back into the graph @@ -119,12 +119,14 @@ namespace gtsam { void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, const Config& config, Cliques& orphans) { + +#if 1 // determine which variables to relinearize FactorGraph affectedFactors; list newFactorsKeys = newFactors.keys(); -#if 1 +#if 0 // relinearize all keys that are in newFactors, and already exist (not new variables!) list keysToRelinearize; @@ -155,6 +157,15 @@ namespace gtsam { list keysToBeRemoved = nonlinearFactors_.keys(); #endif +#if 0 + printf("original tree\n"); + this->print(); + printf("ToBeRemoved\n"); + BOOST_FOREACH(string key, keysToBeRemoved) { + printf("%s ", key.c_str()); + } +#endif + // remove affected factors this->removeTop(keysToBeRemoved, affectedFactors, orphans); @@ -170,9 +181,23 @@ namespace gtsam { list affectedKeys = affectedFactors.keys(); FactorGraph factors = relinearizeAffectedFactors(affectedKeys); // todo: searches through all factors, potentially expensive +#if 0 + printf("affected factors:\n"); + factors.print(); +#endif + // ... 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); @@ -198,8 +223,16 @@ 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 ) - this->insert(*rit); + for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) { + this->insert(*rit, &ordering); + } + +#if 0 + printf("new factors\n"); + newFactors.print(); + printf("orphans:\n"); + orphans.print(); +#endif // add orphans to the bottom of the new tree BOOST_FOREACH(sharedClique orphan, orphans) { @@ -211,6 +244,8 @@ namespace gtsam { orphan->parent_ = parent; // set new parent! } +// this->print(); + // update solution - todo: potentially expensive delta_ = optimize2(*this); } diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 6f030b914..061fb2d6f 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -145,7 +145,7 @@ TEST( BayesTree, removePath ) FactorGraph factors; SymbolicBayesTree::Cliques orphans; - boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["C"]); + bayesTree.removePath(bayesTree["C"], factors, orphans); CHECK(assert_equal((FactorGraph)expected, factors)); CHECK(assert_equal(expectedOrphans, orphans)); @@ -155,9 +155,11 @@ TEST( BayesTree, removePath ) SymbolicBayesTree::Cliques expectedOrphans2; expectedOrphans2 += bayesTree["F"]; - boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["E"]); - CHECK(assert_equal((FactorGraph)expected2, factors)); - CHECK(assert_equal(expectedOrphans2, orphans)); + FactorGraph factors2; + SymbolicBayesTree::Cliques orphans2; + bayesTree.removePath(bayesTree["E"], factors2, orphans2); + CHECK(assert_equal((FactorGraph)expected2, factors2)); + CHECK(assert_equal(expectedOrphans2, orphans2)); } /* ************************************************************************* */ @@ -168,7 +170,7 @@ TEST( BayesTree, removePath2 ) // Call remove-path with clique B FactorGraph factors; SymbolicBayesTree::Cliques orphans; - boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["B"]); + bayesTree.removePath(bayesTree["B"], factors, orphans); // Check expected outcome SymbolicFactorGraph expected; @@ -189,7 +191,7 @@ TEST( BayesTree, removePath3 ) // Call remove-path with clique S FactorGraph factors; SymbolicBayesTree::Cliques orphans; - boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["S"]); + bayesTree.removePath(bayesTree["S"], factors, orphans); // Check expected outcome SymbolicFactorGraph expected; @@ -238,7 +240,6 @@ TEST( BayesTree, removeTop ) CHECK(assert_equal(expectedOrphans2, orphans2)); } - /* ************************************************************************* */ TEST( BayesTree, removeTop2 ) { @@ -267,6 +268,32 @@ TEST( BayesTree, removeTop2 ) } /* ************************************************************************* */ +TEST( BayesTree, removeTop3 ) +{ + // simple test case that failed after COLAMD was fixed/activated + SymbolicConditional::shared_ptr + X(new SymbolicConditional("l5")), + A(new SymbolicConditional("x4", "l5")), + B(new SymbolicConditional("x3", "x4")), + C(new SymbolicConditional("x2", "x3")); + + SymbolicBayesTree bayesTree; + bayesTree.insert(X); + bayesTree.insert(A); + bayesTree.insert(B); + bayesTree.insert(C); + + // remove all + list keys; + keys += "l5", "x2", "x3", "x4"; + FactorGraph factors; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(keys, factors, orphans); + + CHECK(orphans.size() == 0); +} +/* ************************************************************************* */ + int main() { TestResult tr; return TestRegistry::runAllTests(tr);