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> template<class Conditional>
BayesTree<Conditional>::Clique::Clique(const sharedConditional& conditional) { BayesTree<Conditional>::Clique::Clique(const sharedConditional& conditional) {
separator_ = conditional->parents(); separator_ = conditional->parents();
this->push_back(conditional); 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> template<class Conditional>

4
inference/BayesTree.h
View File

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

31
inference/JunctionTree-inl.h
View File

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

4
inference/JunctionTree.h
View File

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

7
linear/GaussianJunctionTree.cpp
View File

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

6
linear/tests/testGaussianJunctionTree.cpp
View File

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