From 0e6607d160f56bc5c97457298af01c903342eab0 Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Mon, 18 Jan 2010 22:37:44 +0000 Subject: [PATCH] relinearization working, but currently applied to all variables --- .cproject | 12 ++++++++++-- cpp/BayesTree-inl.h | 19 ++----------------- cpp/BayesTree.h | 12 ++---------- cpp/ISAM-inl.h | 2 +- cpp/ISAM2-inl.h | 8 ++++++-- cpp/testBayesTree.cpp | 6 +++--- 6 files changed, 24 insertions(+), 35 deletions(-) diff --git a/.cproject b/.cproject index fb4a2edae..6fb1f3e80 100644 --- a/.cproject +++ b/.cproject @@ -753,10 +753,18 @@ true true - + make -j2 -testKey.run +testISAM.run +true +true +true + + +make +-j2 +testGaussianISAM.run true true true diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 3ae4c9b64..547b7ed1b 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -402,11 +402,11 @@ namespace gtsam { /* ************************************************************************* */ template template - void BayesTree::removeTop(const boost::shared_ptr& newFactor, + void BayesTree::removeTop(const list& keys, FactorGraph &factors, typename BayesTree::Cliques& orphans) { // process each key of the new factor - BOOST_FOREACH(const Symbol& key, newFactor->keys()) + BOOST_FOREACH(const Symbol& key, keys) try { // get the clique and remove it from orphans (if it exists) sharedClique clique = (*this)[key]; @@ -424,21 +424,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - template - template - pair, typename BayesTree::Cliques> - BayesTree::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); - - return make_pair(factors,orphans); - } - /* ************************************************************************* */ } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 999a649f8..30ae356bf 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -174,21 +174,13 @@ namespace gtsam { std::pair, Cliques> removePath(sharedClique clique); /** - * Given a factor, turn "contaminated" part of the tree back into a factor graph. + * 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 boost::shared_ptr& newFactor, + void removeTop(const std::list& keys, FactorGraph &factors, Cliques& orphans); - /** - * Given a set of factors, turn "contaminated" part of the tree back into a - * factor graph and return it along with a list of orphaned subtree roots. - * Used for incrementally updating a BayesTree given new measurements (factors). - */ - template - std::pair, Cliques> removeTop(const FactorGraph& newFactors); - }; // BayesTree } /// namespace gtsam diff --git a/cpp/ISAM-inl.h b/cpp/ISAM-inl.h index 7bd75702e..f4ccd853f 100644 --- a/cpp/ISAM-inl.h +++ b/cpp/ISAM-inl.h @@ -31,7 +31,7 @@ namespace gtsam { // Remove the contaminated part of the Bayes tree FactorGraph factors; - boost::tie(factors, orphans) = removeTop(newFactors); + removeTop(newFactors.keys(), factors, orphans); // add the factors themselves factors.push_back(newFactors); diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 708424c44..ffc236790 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -119,6 +119,9 @@ namespace gtsam { void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, const Config& config, Cliques& orphans) { + // todo: updates all variables... needs to be synced with removeTop!!! + linPoint_ = expmap(linPoint_, delta_); + // add new variables linPoint_.insert(config); @@ -126,7 +129,9 @@ namespace gtsam { // Remove the contaminated part of the Bayes tree FactorGraph affectedFactors; - boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized); +// list keysToBeRemoved = newFactorsLinearized.keys(); // todo + list keysToBeRemoved = nonlinearFactors_.keys(); + this->removeTop(keysToBeRemoved, affectedFactors, orphans); // relinearize the affected factors ... list affectedKeys = affectedFactors.keys(); @@ -172,7 +177,6 @@ namespace gtsam { // update solution - todo: potentially expensive delta_ = optimize2(*this); - } template diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 628e8d6fb..6f030b914 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -214,7 +214,7 @@ TEST( BayesTree, removeTop ) // Remove the contaminated part of the Bayes tree FactorGraph factors; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(newFactor, factors, orphans); + bayesTree.removeTop(newFactor->keys(), factors, orphans); // Check expected outcome SymbolicFactorGraph expected; @@ -231,7 +231,7 @@ TEST( BayesTree, removeTop ) boost::shared_ptr newFactor2(new SymbolicFactor("B")); FactorGraph factors2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removeTop(newFactor2, factors2, orphans2); + bayesTree.removeTop(newFactor2->keys(), factors2, orphans2); SymbolicFactorGraph expected2; CHECK(assert_equal((FactorGraph)expected2, factors2)); SymbolicBayesTree::Cliques expectedOrphans2; @@ -252,7 +252,7 @@ TEST( BayesTree, removeTop2 ) // Remove the contaminated part of the Bayes tree FactorGraph factors; SymbolicBayesTree::Cliques orphans; - boost::tie(factors,orphans) = bayesTree.removeTop(newFactors); + bayesTree.removeTop(newFactors.keys(), factors, orphans); // Check expected outcome SymbolicFactorGraph expected;