12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

avoid constructing and deconstructing btree overhead and speed up the multi-frontal solver

release/4.3a0
Kai Ni 2010-07-31 07:19:03 +00:00
parent 27c009d792
commit 71074b2188
6 changed files with 36 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
inference/BayesTree-inl.h
View File

@ -32,9 +32,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
BayesTree<Conditional>::Clique::Clique(const sharedConditional& conditional) {
separator_ = conditional->parents();
this->push_back(conditional);
}
separator_ = conditional->parents();
this->push_back(conditional);
}
/* ************************************************************************* */
template<class Conditional>
BayesTree<Conditional>::Clique::Clique(const BayesNet<Conditional>& bayesNet)
: BayesNet<Conditional>(bayesNet) {
separator_ = (*bayesNet.rbegin())->parents();
}
/* ************************************************************************* */
template<class Conditional>

4
inference/BayesTree.h
View File

@ -49,9 +49,11 @@ namespace gtsam {
friend class BayesTree<Conditional>;
//* Constructor */
Clique();
Clique(const sharedConditional& conditional);
Clique();
Clique(const BayesNet<Conditional>& bayesNet);
/** return keys in frontal:separator order */
Ordering keys() const;

31
inference/JunctionTree-inl.h
View File

@ -61,29 +61,21 @@ namespace gtsam {
/* ************************************************************************* */
template <class FG> template <class Conditional>
pair<FG, BayesTree<Conditional> >
pair<FG, typename BayesTree<Conditional>::sharedClique>
JunctionTree<FG>::eliminateOneClique(sharedClique current) {
// current->frontal_.print("current clique:");
typedef typename BayesTree<Conditional>::sharedClique sharedBtreeClique;
FG fg; // factor graph will be assembled from local factors and marginalized children
list<BayesTree<Conditional> > children;
list<sharedBtreeClique> 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<Conditional> childTree;
boost::tie(fgChild, childTree) = eliminateOneClique<Conditional>(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<Conditional>(child);
fg.push_back(fgChild);
children.push_back(childTree);
children.push_back(childRoot);
}
// eliminate the combined factors
@ -91,15 +83,20 @@ namespace gtsam {
BayesNet<Conditional> bn = fg.eliminateFrontals(current->frontal_);
// create a new clique corresponding the combined factors
BayesTree<Conditional> bayesTree(bn, children);
// BayesTree<Conditional> bayesTree(bn, children);
sharedBtreeClique new_clique(new typename BayesTree<Conditional>::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 <class FG> template <class Conditional>
BayesTree<Conditional> JunctionTree<FG>::eliminate() {
pair<FG, BayesTree<Conditional> > ret = this->eliminateOneClique<Conditional>(this->root());
typename BayesTree<Conditional>::sharedClique JunctionTree<FG>::eliminate() {
pair<FG, typename BayesTree<Conditional>::sharedClique> ret = this->eliminateOneClique<Conditional>(this->root());
if (ret.first.nrFactors() != 0)
throw runtime_error("JuntionTree::eliminate: elimination failed because of factors left over!");
return ret.second;

4
inference/JunctionTree.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
// utility function called by eliminate
template<class Conditional>
std::pair<FG, BayesTree<Conditional> > eliminateOneClique(sharedClique fg_);
std::pair<FG, typename BayesTree<Conditional>::sharedClique> eliminateOneClique(sharedClique fg_);
public:
// constructor
@ -54,7 +54,7 @@ namespace gtsam {
// eliminate the factors in the subgraphs
template<class Conditional>
BayesTree<Conditional> eliminate();
typename BayesTree<Conditional>::sharedClique eliminate();
}; // JunctionTree

7
linear/GaussianJunctionTree.cpp
View File

@ -43,13 +43,12 @@ namespace gtsam {
/* ************************************************************************* */
VectorConfig GaussianJunctionTree::optimize() {
// eliminate from leaves to the root
typedef JunctionTree<GaussianFactorGraph> Base;
BayesTree<GaussianConditional> bayesTree;
bayesTree = this->eliminate<GaussianConditional>();
BayesTree<GaussianConditional>::sharedClique rootClique;
rootClique = this->eliminate<GaussianConditional>();
// back-substitution
VectorConfig result;
btreeBackSubstitue(bayesTree.root(), result);
btreeBackSubstitue(rootClique, result);
return result;
}

6
linear/tests/testGaussianJunctionTree.cpp
View File

@ -59,7 +59,7 @@ TEST( GaussianJunctionTree, eliminate )
GaussianFactorGraph fg = createChain();
Ordering ordering; ordering += "x2","x1","x3","x4";
GaussianJunctionTree junctionTree(fg, ordering);
BayesTree<GaussianConditional> bayesTree = junctionTree.eliminate<GaussianConditional>();
BayesTree<GaussianConditional>::sharedClique rootClique = junctionTree.eliminate<GaussianConditional>();
typedef BayesTree<GaussianConditional>::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())));
}