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

Removed debug code, added marginal function

release/4.3a0
Frank Dellaert 2009-11-04 03:22:29 +00:00
parent ec6611ae56
commit 4865edb883
3 changed files with 71 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
cpp/BayesTree-inl.h
View File

@ -6,6 +6,7 @@
#include <boost/foreach.hpp>
#include "BayesTree.h"
#include "FactorGraph.h"
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
if (!separator_.empty()) {
cout << " :";
BOOST_FOREACH(string key, separator_)
cout << " " << key;
cout << " " << key;
}
cout << endl;
}
@ -48,10 +49,10 @@ namespace gtsam {
/* ************************************************************************* */
// TODO: traversal is O(n*log(n)) but could be O(n) with better bayesNet
template<class Conditional>
BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet, bool verbose) {
BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet) {
typename BayesNet<Conditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*rit,verbose);
insert(*rit);
}
/* ************************************************************************* */
@ -73,22 +74,29 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::insert(const boost::shared_ptr<Conditional>& conditional, bool verbose) {
void BayesTree<Conditional>::addClique
(const boost::shared_ptr<Conditional>& conditional, node_ptr parent_clique)
{
node_ptr new_clique(new Node(conditional));
nodeMap_.insert(make_pair(conditional->key(), nodes_.size()));
nodes_.push_back(new_clique);
if (parent_clique==NULL) return;
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
}
string key = conditional->key();
if (verbose) cout << "Inserting " << key << "| ";
// get parents
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::insert
(const boost::shared_ptr<Conditional>& conditional)
{
// get key and parents
string key = conditional->key();
list<string> parents = conditional->parents();
if (verbose) BOOST_FOREACH(string p, parents) cout << p << " ";
if (verbose) cout << endl;
// if no parents, start a new root clique
if (parents.empty()) {
if (verbose) cout << "Creating root clique" << endl;
node_ptr root(new Node(conditional));
nodes_.push_back(root);
nodeMap_.insert(make_pair(key, 0));
addClique(conditional);
return;
}
@ -96,26 +104,35 @@ namespace gtsam {
string parent = parents.front();
NodeMap::const_iterator it = nodeMap_.find(parent);
if (it == nodeMap_.end()) throw(invalid_argument(
"BayesTree::insert('"+key+"'): parent '" + parent + "' was not yet inserted"));
int index = it->second;
node_ptr parent_clique = nodes_[index];
if (verbose) cout << "Parent clique " << index << " of size " << parent_clique->size() << endl;
"BayesTree::insert('"+key+"'): parent '" + parent + "' not yet inserted"));
int parent_index = it->second;
node_ptr parent_clique = nodes_[parent_index];
// if the parents and parent clique have the same size, add to parent clique
if (parent_clique->size() == parents.size()) {
if (verbose) cout << "Adding to clique " << index << endl;
nodeMap_.insert(make_pair(key, index));
nodeMap_.insert(make_pair(key, parent_index));
parent_clique->push_front(conditional);
return;
}
// otherwise, start a new clique and add it to the tree
if (verbose) cout << "Starting new clique" << endl;
node_ptr new_clique(new Node(conditional));
new_clique->parent_ = parent_clique;
parent_clique->children_.push_back(new_clique);
nodeMap_.insert(make_pair(key, nodes_.size()));
nodes_.push_back(new_clique);
addClique(conditional,parent_clique);
}
/* ************************************************************************* */
template<class Conditional>
boost::shared_ptr<Conditional> BayesTree<Conditional>::marginal(const string& key) const {
// find the clique to which key belongs
NodeMap::const_iterator it = nodeMap_.find(key);
if (it == nodeMap_.end()) throw(invalid_argument(
"BayesTree::marginal('"+key+"'): key not found"));
// find all cliques on the path to the root
// FactorGraph
boost::shared_ptr<Conditional> result(new Conditional);
return result;
}
/* ************************************************************************* */

11
cpp/BayesTree.h
View File

@ -30,7 +30,6 @@ namespace gtsam {
public:
typedef boost::shared_ptr<Conditional> conditional_ptr;
typedef std::pair<std::string,conditional_ptr> NamedConditional;
private:
@ -66,13 +65,16 @@ namespace gtsam {
typedef std::map<std::string, int> NodeMap;
NodeMap nodeMap_;
/** add a clique */
void addClique(const conditional_ptr& conditional, node_ptr parent_clique=node_ptr());
public:
/** Create an empty Bayes Tree */
BayesTree();
/** Create a Bayes Tree from a Bayes Net */
BayesTree(const BayesNet<Conditional>& bayesNet, bool verbose=false);
BayesTree(const BayesNet<Conditional>& bayesNet);
/** Destructor */
virtual ~BayesTree() {}
@ -84,7 +86,7 @@ namespace gtsam {
bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
/** insert a new conditional */
void insert(const boost::shared_ptr<Conditional>& conditional, bool verbose=false);
void insert(const boost::shared_ptr<Conditional>& conditional);
/** number of cliques */
inline size_t size() const { return nodes_.size();}
@ -92,6 +94,9 @@ namespace gtsam {
/** return root clique */
const BayesNet<Conditional>& root() const {return *(nodes_[0]);}
/** return marginal on any variable */
boost::shared_ptr<Conditional> marginal(const std::string& key) const;
}; // BayesTree
} /// namespace gtsam

28
cpp/testBayesTree.cpp
View File

@ -27,10 +27,10 @@ SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L(
/* ************************************************************************* */
TEST( BayesTree, Front )
{
BayesNet<SymbolicConditional> f1;
SymbolicBayesNet f1;
f1.push_back(B);
f1.push_back(L);
BayesNet<SymbolicConditional> f2;
SymbolicBayesNet f2;
f2.push_back(L);
f2.push_back(B);
CHECK(f1.equals(f1));
@ -68,9 +68,8 @@ TEST( BayesTree, constructor )
ASIA.push_back(E);
ASIA.push_back(L);
ASIA.push_back(B);
bool verbose = false;
BayesTree<SymbolicConditional> bayesTree2(ASIA,verbose);
if (verbose) bayesTree2.print("bayesTree2");
BayesTree<SymbolicConditional> bayesTree2(ASIA);
//bayesTree2.print("bayesTree2");
// Check whether the same
CHECK(assert_equal(bayesTree,bayesTree2));
@ -97,7 +96,7 @@ TEST( BayesTree, smoother )
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering);
// Create the Bayes tree
BayesTree<ConditionalGaussian> bayesTree(*chordalBayesNet,false);
BayesTree<ConditionalGaussian> bayesTree(*chordalBayesNet);
LONGS_EQUAL(6,bayesTree.size());
}
@ -108,7 +107,7 @@ TEST( BayesTree, smoother )
x1 : x2
x7 : x6
/* ************************************************************************* */
TEST( BayesTree, balanced_smoother )
TEST( BayesTree, balanced_smoother_marginals )
{
// Create smoother with 7 nodes
LinearFactorGraph smoother = createSmoother(7);
@ -119,8 +118,21 @@ TEST( BayesTree, balanced_smoother )
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering);
// Create the Bayes tree
BayesTree<ConditionalGaussian> bayesTree(*chordalBayesNet,false);
BayesTree<ConditionalGaussian> bayesTree(*chordalBayesNet);
LONGS_EQUAL(4,bayesTree.size());
// Check root clique
//BayesNet<ConditionalGaussian> expected_root;
//BayesNet<ConditionalGaussian> actual_root = bayesTree.root();
//CHECK(assert_equal(expected_root,actual_root));
// Check marginal on x1
ConditionalGaussian expected;
ConditionalGaussian::shared_ptr actual = bayesTree.marginal("x1");
CHECK(assert_equal(expected,*actual));
// JunctionTree is an undirected tree of cliques
// JunctionTree<ConditionalGaussian> marginals = bayesTree.marginals();
}
/* ************************************************************************* */