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

moved addClique and removeClique implementations to inl.h, 

removePath takes clique argument now
release/4.3a0
Frank Dellaert 2009-11-21 06:07:46 +00:00
parent 286d826119
commit 072846a70f
3 changed files with 57 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
cpp/BayesTree-inl.h
View File

@ -171,6 +171,37 @@ namespace gtsam {
return marginalize<Factor,Conditional>(*bn,keys12);
}
/* ************************************************************************* */
template<class Conditional>
typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::addClique
(const sharedConditional& conditional, sharedClique parent_clique) {
sharedClique new_clique(new Clique(conditional));
nodes_.insert(make_pair(conditional->key(), new_clique));
if (parent_clique != NULL) {
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
}
return new_clique;
}
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::removeClique(sharedClique clique) {
if (!clique->isRoot())
clique->parent_->children_.remove(clique);
else {
// we remove the root clique: have to make another clique the root
if (clique->children_.empty())
root_.reset();
else
root_ = *(clique->children_.begin());
}
BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_.reset();
BOOST_FOREACH(std::string key, clique->ordering())
nodes_.erase(key);
}
/* ************************************************************************* */
template<class Conditional>
BayesTree<Conditional>::BayesTree() {
@ -319,21 +350,20 @@ namespace gtsam {
template<class Conditional>
template<class Factor>
FactorGraph<Factor>
BayesTree<Conditional>::removePath(const string& key) {
sharedClique clique = (*this)[key];
BayesTree<Conditional>::removePath(sharedClique clique) {
#if 0
cout << "removing:" << endl;
clique->print();
cout << "from" << endl;
typedef std::pair<string, sharedClique> sometype;
BOOST_FOREACH(sometype clique, nodes_) {
clique.second->print();
//if (clique==NULL) return;
bool verbose = false;
if (verbose) {
clique->print("removing");
cout << "before" << endl;
BOOST_FOREACH(typename Nodes::value_type clique, nodes_)
clique.second->print();
}
#endif
// convert clique to factor
FactorGraph<Factor> factors(*clique);
FactorGraph<Factor> factors(*clique);// = removePath(clique->parent_);
while (!(clique->isRoot())) {
sharedClique old_clique = clique;
@ -343,6 +373,12 @@ namespace gtsam {
}
removeClique(clique);
if (verbose) {
cout << "after" << endl;
BOOST_FOREACH(typename Nodes::value_type clique, nodes_)
clique.second->print();
}
return factors;
}

37
cpp/BayesTree.h
View File

@ -51,7 +51,7 @@ namespace gtsam {
Ordering keys() const;
/** print this node */
void print(const std::string& s = "Bayes tree node") const;
void print(const std::string& s = "") const;
/** The size *includes* the separator */
size_t size() const {
@ -94,34 +94,10 @@ namespace gtsam {
/** add a clique */
sharedClique addClique(const sharedConditional& conditional,
sharedClique parent_clique = sharedClique()) {
sharedClique new_clique(new Clique(conditional));
nodes_.insert(make_pair(conditional->key(), new_clique));
if (parent_clique != NULL) {
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
}
return new_clique;
}
sharedClique parent_clique = sharedClique());
/** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique) {
if (clique->parent_ != NULL) {
clique->parent_->children_.remove(clique);
} else {
// we remove the root clique: have to make another clique the root
if (clique->children_.empty()) {
root_.reset();
} else {
root_ = *(clique->children_.begin());
}
}
BOOST_FOREACH(sharedClique child, clique->children_) {
child->parent_.reset();
}
std::string key = *(clique->keys().begin());
nodes_.erase(key);
}
void removeClique(sharedClique clique);
public:
@ -179,11 +155,10 @@ namespace gtsam {
template<class Factor>
BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
/** IMPERATIVE! Return the factor containing all nodes contaminated by key; also,
* removes those entries from the Bayes Tree, resulting in a forest of orphans
*/
/** Remove path from clique to root and return that path as factors */
template<class Factor>
FactorGraph<Factor> removePath(const std::string& key);
FactorGraph<Factor> removePath(sharedClique clique);
}; // BayesTree
} /// namespace gtsam

14
cpp/testBayesTree.cpp
View File

@ -332,21 +332,15 @@ TEST( BayesTree, removePath )
expected.push_factor("A","B");
expected.push_factor("A");
SymbolicFactorGraph actual = bayesTree.removePath<SymbolicFactor>("C");
SymbolicFactorGraph actual = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
CHECK(assert_equal(expected, actual));
// remove A, nothing should happen (already removed)
SymbolicFactorGraph expected2; // empty factor
actual = bayesTree.removePath<SymbolicFactor>("A");
// CHECK(assert_equal(expected2, actual));
// remove E: factor graph with EB; E|B removed from second orphan tree
SymbolicFactorGraph expected3;
expected3.push_factor("C","A");
expected3.push_factor("B","E");
actual = bayesTree.removePath<SymbolicFactor>("E");
// CHECK(assert_equal(expected3, actual));
actual = bayesTree.removePath<SymbolicFactor>(bayesTree["E"]);
CHECK(assert_equal(expected3, actual));
}
/* ************************************************************************* */