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

Updated and made removeTop unit tests pass

release/4.3a0
Richard Roberts 2013-07-27 18:33:43 +00:00
parent 594c0412cb
commit 619c772a80
3 changed files with 85 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/inference/BayesTreeUnordered-inst.h
View File

@ -201,28 +201,6 @@ namespace gtsam {
treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique<FactorType,CLIQUE>, boost::ref(graph), _1));
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
{
if (clique->isRoot())
roots_.erase(std::find(roots_.begin(), roots_.end(), clique));
else { // detach clique from parent
sharedClique parent = clique->parent_.lock();
typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
assert(child != parent->children.end());
parent->children.erase(child);
}
// orphan my children
BOOST_FOREACH(sharedClique child, clique->children)
child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
nodes_.erase(j);
}
}
/* ************************************************************************* */
//template<class CLIQUE>
//void BayesTreeUnordered<CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
@ -535,6 +513,30 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
{
if (clique->isRoot()) {
std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
if(root != roots_.end())
roots_.erase(root);
} else { // detach clique from parent
sharedClique parent = clique->parent_.lock();
typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
assert(child != parent->children.end());
parent->children.erase(child);
}
// orphan my children
BOOST_FOREACH(sharedClique child, clique->children)
child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
nodes_.erase(j);
}
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
@ -562,19 +564,17 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
template<class CONTAINER>
void BayesTreeUnordered<CLIQUE>::removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans)
void BayesTreeUnordered<CLIQUE>::removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans)
{
// process each key of the new factor
BOOST_FOREACH(const Key& j, keys) {
BOOST_FOREACH(const Key& j, keys)
{
// get the clique
if(j < nodes_.size()) {
const sharedClique& clique(nodes_[j]);
if(clique) {
// TODO: Nodes will be searched again in removeClique
Nodes::const_iterator node = nodes_.find(j);
if(node != nodes_.end()) {
// remove path from clique to root
this->removePath(clique, bn, orphans);
}
this->removePath(node->second, bn, orphans);
}
}

3
gtsam/inference/BayesTreeUnordered.h
View File

@ -208,8 +208,7 @@ namespace gtsam {
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class CONTAINER>
void removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans);
void removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans);
/**
* Remove the requested subtree. */

96
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -295,25 +295,19 @@ TEST( BayesTree, removeTop )
{
SymbolicBayesTreeUnordered bayesTree = asiaBayesTree;
bayesTree.print("asiaBayesTree: ");
// create a new factor to be inserted
//boost::shared_ptr<IndexFactor> newFactor(new IndexFactor(_S_,_B_));
// Remove the contaminated part of the Bayes tree
SymbolicBayesNetUnordered bn;
SymbolicBayesTreeUnordered::Cliques orphans;
list<Key> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraphUnordered factors(bn);
bayesTree.removeTop(list_of(_B_)(_S_), bn, orphans);
// Check expected outcome
SymbolicFactorGraphUnordered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesNetUnordered expected;
expected += SymbolicConditionalUnordered::FromKeys(list_of(_B_)(_L_)(_E_)(_S_), 4);
CHECK(assert_equal(expected, bn));
SymbolicBayesTreeUnordered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected));
@ -322,8 +316,7 @@ TEST( BayesTree, removeTop )
//boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
SymbolicBayesNetUnordered bn2;
SymbolicBayesTreeUnordered::Cliques orphans2;
keys.clear(); keys += _B_;
bayesTree.removeTop(keys, bn2, orphans2);
bayesTree.removeTop(list_of(_B_), bn2, orphans2);
SymbolicFactorGraphUnordered factors2(bn2);
SymbolicFactorGraphUnordered expected2;
CHECK(assert_equal(expected2, factors2));
@ -344,50 +337,65 @@ TEST( BayesTree, removeTop2 )
// Remove the contaminated part of the Bayes tree
SymbolicBayesNetUnordered bn;
SymbolicBayesTreeUnordered::Cliques orphans;
list<Key> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraphUnordered factors(bn);
bayesTree.removeTop(list_of(_T_), bn, orphans);
// Check expected outcome
SymbolicFactorGraphUnordered expected;
expected.push_factor(_E_,_L_,_B_);
// expected.push_factor(_L_,_B_);
// expected.push_factor(_B_);
expected.push_factor(_S_,_L_,_B_);
CHECK(assert_equal(expected, factors));
SymbolicBayesNetUnordered expected = list_of
(SymbolicConditionalUnordered::FromKeys(list_of(_B_)(_L_)(_E_)(_S_), 4))
(SymbolicConditionalUnordered::FromKeys(list_of(_T_)(_E_)(_L_), 1));
CHECK(assert_equal(expected, bn));
SymbolicBayesTreeUnordered::Cliques expectedOrphans;
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
expectedOrphans += bayesTree[_X_];
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected));
}
/* ************************************************************************* */
TEST( BayesTree, removeTop3 )
{
const Key _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated
SymbolicConditionalUnordered::shared_ptr
X(new SymbolicConditionalUnordered(_l5_)),
A(new SymbolicConditionalUnordered(_x4_, _l5_)),
B(new SymbolicConditionalUnordered(_x2_, _x4_)),
C(new SymbolicConditionalUnordered(_x3_, _x2_));
// Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
SymbolicBayesTreeUnordered bayesTree;
SymbolicBayesTreeUnordered::insert(bayesTree, X);
SymbolicBayesTreeUnordered::insert(bayesTree, A);
SymbolicBayesTreeUnordered::insert(bayesTree, B);
SymbolicBayesTreeUnordered::insert(bayesTree, C);
SymbolicFactorGraphUnordered graph = list_of
(SymbolicFactorUnordered(L(5)))
(SymbolicFactorUnordered(X(4), L(5)))
(SymbolicFactorUnordered(X(2), X(4)))
(SymbolicFactorUnordered(X(3), X(2)));
SymbolicBayesTreeUnordered bayesTree = *graph.eliminateMultifrontal(
OrderingUnordered(list_of (X(3)) (X(2)) (X(4)) (L(5)) ));
// remove all
list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<SymbolicConditionalUnordered> bn;
SymbolicBayesNetUnordered bn;
SymbolicBayesTreeUnordered::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), bn, orphans);
CHECK(orphans.size() == 0);
SymbolicBayesNetUnordered expectedBn = list_of
(SymbolicConditionalUnordered::FromKeys(list_of(L(5))(X(4)), 2))
(SymbolicConditionalUnordered(X(2), X(4)))
(SymbolicConditionalUnordered(X(3), X(2)));
EXPECT(assert_equal(expectedBn, bn));
EXPECT(orphans.empty());
}
/* ************************************************************************* */
TEST( BayesTree, removeTop4 )
{
SymbolicFactorGraphUnordered graph = list_of
(SymbolicFactorUnordered(L(5)))
(SymbolicFactorUnordered(X(4), L(5)))
(SymbolicFactorUnordered(X(2), X(4)))
(SymbolicFactorUnordered(X(3), X(2)));
SymbolicBayesTreeUnordered bayesTree = *graph.eliminateMultifrontal(
OrderingUnordered(list_of (X(3)) (X(2)) (X(4)) (L(5)) ));
// remove all
SymbolicBayesNetUnordered bn;
SymbolicBayesTreeUnordered::Cliques orphans;
bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), bn, orphans);
SymbolicBayesNetUnordered expectedBn = list_of
(SymbolicConditionalUnordered::FromKeys(list_of(L(5))(X(4)), 2))
(SymbolicConditionalUnordered(X(2), X(4)))
(SymbolicConditionalUnordered(X(3), X(2)));
EXPECT(assert_equal(expectedBn, bn));
EXPECT(orphans.empty());
}
/////* ************************************************************************* */