From 71074b21887d2651998405ee1a366ea4d39305af Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Sat, 31 Jul 2010 07:19:03 +0000 Subject: [PATCH] avoid constructing and deconstructing btree overhead and speed up the multi-frontal solver --- inference/BayesTree-inl.h | 13 +++++++--- inference/BayesTree.h | 4 ++- inference/JunctionTree-inl.h | 31 ++++++++++------------- inference/JunctionTree.h | 4 +-- linear/GaussianJunctionTree.cpp | 7 +++-- linear/tests/testGaussianJunctionTree.cpp | 6 +++-- 6 files changed, 36 insertions(+), 29 deletions(-) diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index 63e511d1a..79f57a9f2 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -32,9 +32,16 @@ namespace gtsam { /* ************************************************************************* */ template BayesTree::Clique::Clique(const sharedConditional& conditional) { - separator_ = conditional->parents(); - this->push_back(conditional); - } + separator_ = conditional->parents(); + this->push_back(conditional); + } + + /* ************************************************************************* */ + template + BayesTree::Clique::Clique(const BayesNet& bayesNet) + : BayesNet(bayesNet) { + separator_ = (*bayesNet.rbegin())->parents(); + } /* ************************************************************************* */ template diff --git a/inference/BayesTree.h b/inference/BayesTree.h index 996ff68da..a2b5fc903 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -49,9 +49,11 @@ namespace gtsam { friend class BayesTree; //* Constructor */ + Clique(); + Clique(const sharedConditional& conditional); - Clique(); + Clique(const BayesNet& bayesNet); /** return keys in frontal:separator order */ Ordering keys() const; diff --git a/inference/JunctionTree-inl.h b/inference/JunctionTree-inl.h index 773f8a795..c1a294746 100644 --- a/inference/JunctionTree-inl.h +++ b/inference/JunctionTree-inl.h @@ -61,29 +61,21 @@ namespace gtsam { /* ************************************************************************* */ template template - pair > + pair::sharedClique> JunctionTree::eliminateOneClique(sharedClique current) { -// current->frontal_.print("current clique:"); - typedef typename BayesTree::sharedClique sharedBtreeClique; FG fg; // factor graph will be assembled from local factors and marginalized children - list > children; + list children; fg.push_back(*current); // add the local factor graph -// BOOST_FOREACH(const typename FG::sharedFactor& factor_, fg) -// Ordering(factor_->keys()).print("local factor:"); - BOOST_FOREACH(sharedClique& child, current->children_) { // receive the factors from the child and its clique point - FG fgChild; BayesTree childTree; - boost::tie(fgChild, childTree) = eliminateOneClique(child); - -// BOOST_FOREACH(const typename FG::sharedFactor& factor_, fgChild) -// Ordering(factor_->keys()).print("factor from child:"); + FG fgChild; sharedBtreeClique childRoot; + boost::tie(fgChild, childRoot) = eliminateOneClique(child); fg.push_back(fgChild); - children.push_back(childTree); + children.push_back(childRoot); } // eliminate the combined factors @@ -91,15 +83,20 @@ namespace gtsam { BayesNet bn = fg.eliminateFrontals(current->frontal_); // create a new clique corresponding the combined factors - BayesTree bayesTree(bn, children); +// BayesTree bayesTree(bn, children); + sharedBtreeClique new_clique(new typename BayesTree::Clique(bn)); + new_clique->children_ = children; - return make_pair(fg, bayesTree); + BOOST_FOREACH(sharedBtreeClique& childRoot, children) + childRoot->parent_ = new_clique; + + return make_pair(fg, new_clique); } /* ************************************************************************* */ template template - BayesTree JunctionTree::eliminate() { - pair > ret = this->eliminateOneClique(this->root()); + typename BayesTree::sharedClique JunctionTree::eliminate() { + pair::sharedClique> ret = this->eliminateOneClique(this->root()); if (ret.first.nrFactors() != 0) throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!"); return ret.second; diff --git a/inference/JunctionTree.h b/inference/JunctionTree.h index ce2433fbd..a98a30b12 100644 --- a/inference/JunctionTree.h +++ b/inference/JunctionTree.h @@ -42,7 +42,7 @@ namespace gtsam { // utility function called by eliminate template - std::pair > eliminateOneClique(sharedClique fg_); + std::pair::sharedClique> eliminateOneClique(sharedClique fg_); public: // constructor @@ -54,7 +54,7 @@ namespace gtsam { // eliminate the factors in the subgraphs template - BayesTree eliminate(); + typename BayesTree::sharedClique eliminate(); }; // JunctionTree diff --git a/linear/GaussianJunctionTree.cpp b/linear/GaussianJunctionTree.cpp index 086c96581..5f6e772ee 100644 --- a/linear/GaussianJunctionTree.cpp +++ b/linear/GaussianJunctionTree.cpp @@ -43,13 +43,12 @@ namespace gtsam { /* ************************************************************************* */ VectorConfig GaussianJunctionTree::optimize() { // eliminate from leaves to the root - typedef JunctionTree Base; - BayesTree bayesTree; - bayesTree = this->eliminate(); + BayesTree::sharedClique rootClique; + rootClique = this->eliminate(); // back-substitution VectorConfig result; - btreeBackSubstitue(bayesTree.root(), result); + btreeBackSubstitue(rootClique, result); return result; } diff --git a/linear/tests/testGaussianJunctionTree.cpp b/linear/tests/testGaussianJunctionTree.cpp index d570e2f0f..3ef2f83b1 100644 --- a/linear/tests/testGaussianJunctionTree.cpp +++ b/linear/tests/testGaussianJunctionTree.cpp @@ -59,7 +59,7 @@ TEST( GaussianJunctionTree, eliminate ) GaussianFactorGraph fg = createChain(); Ordering ordering; ordering += "x2","x1","x3","x4"; GaussianJunctionTree junctionTree(fg, ordering); - BayesTree bayesTree = junctionTree.eliminate(); + BayesTree::sharedClique rootClique = junctionTree.eliminate(); typedef BayesTree::sharedConditional sharedConditional; Matrix two = Matrix_(1,1,2.); @@ -69,7 +69,9 @@ TEST( GaussianJunctionTree, eliminate ) bayesTree_expected.insert(sharedConditional(new GaussianConditional("x3", Vector_(1,2.), two, "x4", two, Vector_(1,1.))), ordering); bayesTree_expected.insert(sharedConditional(new GaussianConditional("x1", Vector_(1,0.), one*(-1), "x3", one, Vector_(1,1.))), ordering); bayesTree_expected.insert(sharedConditional(new GaussianConditional("x2", Vector_(1,2.), two, "x1", one, "x3", one, Vector_(1,1.))), ordering); - CHECK(assert_equal(bayesTree_expected, bayesTree)); + CHECK(assert_equal(*bayesTree_expected.root(), *rootClique)); + CHECK(assert_equal(*(bayesTree_expected.root()->children_.front()), *(rootClique->children_.front()))); + }