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

Standardized on new shared_ptr naming convention

release/4.3a0
Frank Dellaert 2009-11-07 17:24:05 +00:00
parent 6339b5d19d
commit f5d186522e
3 changed files with 74 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
cpp/BayesTree-inl.h
View File

@ -14,16 +14,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
BayesTree<Conditional>::Node::Node(const boost::shared_ptr<Conditional>& conditional) {
BayesTree<Conditional>::Clique::Clique(const boost::shared_ptr<Conditional>& conditional) {
separator_ = conditional->parents();
this->push_back(conditional);
}
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::Node::print(const string& s) const {
void BayesTree<Conditional>::Clique::print(const string& s) const {
cout << s;
BOOST_REVERSE_FOREACH(const conditional_ptr& conditional, this->conditionals_)
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, this->conditionals_)
cout << " " << conditional->key();
if (!separator_.empty()) {
cout << " :";
@ -35,19 +35,37 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::Node::printTree(const string& indent) const {
void BayesTree<Conditional>::Clique::printTree(const string& indent) const {
print(indent);
BOOST_FOREACH(shared_ptr child, children_)
child->printTree(indent+" ");
}
/* ************************************************************************* */
template<class Conditional>
typename BayesTree<Conditional>::sharedBayesNet
BayesTree<Conditional>::Clique::shortcut() {
// The shortcut density is a conditional P(S|R) of the separator of this
// clique on the root. We can compute it recursively from the parent shortcut
// P(S_p|R) as \int P(F_p|S_p) P(S_p|R), where F_p are the frontal nodes in p
// The base case is when we are the root or the parent is the root,
// in which case we return an empty Bayes net
sharedBayesNet p_S_R(new BayesNet<Conditional>);
if (parent_==NULL || parent_->parent_==NULL) return p_S_R;
// If not, calculate the parent shortcut P(S_p|R)
sharedBayesNet p_Sp_R = parent_->shortcut();
return p_S_R;
}
/* ************************************************************************* */
template<class Conditional>
BayesTree<Conditional>::BayesTree() {
}
/* ************************************************************************* */
// 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) {
typename BayesNet<Conditional>::const_reverse_iterator rit;
@ -68,21 +86,7 @@ namespace gtsam {
bool BayesTree<Conditional>::equals(const BayesTree<Conditional>& other,
double tol) const {
return size()==other.size();
//&& equal(nodes_.begin(),nodes_.end(),other.nodes_.begin(),equals_star<Node>(tol));
}
/* ************************************************************************* */
template<class Conditional>
boost::shared_ptr<typename BayesTree<Conditional>::Node> BayesTree<Conditional>::addClique
(const boost::shared_ptr<Conditional>& conditional, node_ptr parent_clique)
{
node_ptr new_clique(new Node(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;
//&& equal(nodes_.begin(),nodes_.end(),other.nodes_.begin(),equals_star<Clique>(tol));
}
/* ************************************************************************* */
@ -102,7 +106,7 @@ namespace gtsam {
// otherwise, find the parent clique
string parent = parents.front();
node_ptr parent_clique = (*this)[parent];
sharedClique parent_clique = (*this)[parent];
// if the parents and parent clique have the same size, add to parent clique
if (parent_clique->size() == parents.size()) {
@ -128,7 +132,7 @@ namespace gtsam {
boost::shared_ptr<Conditional> BayesTree<Conditional>::marginal(const string& key) const {
// get clique containing key, and remove all factors below key
node_ptr clique = (*this)[key];
sharedClique clique = (*this)[key];
Ordering ordering = clique->ordering();
FactorGraph<Factor> graph(*clique);
while(ordering.front()!=key) {

49
cpp/BayesTree.h
View File

@ -29,21 +29,22 @@ namespace gtsam {
public:
typedef boost::shared_ptr<Conditional> conditional_ptr;
typedef boost::shared_ptr<Conditional> sharedConditional;
typedef boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet;
/** A Node in the tree is an incomplete Bayes net: the variables
/** A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on is the separator. We also have pointers up and down the tree.
*/
struct Node: public BayesNet<Conditional> {
struct Clique: public BayesNet<Conditional> {
typedef boost::shared_ptr<Node> shared_ptr;
typedef boost::shared_ptr<Clique> shared_ptr;
shared_ptr parent_;
std::list<std::string> separator_; /** separator keys */
std::list<shared_ptr> children_;
std::list<std::string> separator_; /** separator keys */
//* Constructor */
Node(const conditional_ptr& conditional);
Clique(const sharedConditional& conditional);
/** The size *includes* the separator */
size_t size() const {
@ -53,24 +54,35 @@ namespace gtsam {
/** print this node */
void print(const std::string& s = "Bayes tree node") const;
/** print this node and entire subtree below it*/
/** print this node and entire subtree below it */
void printTree(const std::string& indent) const;
/** return the conditional P(S|Root) on the separator given the root */
sharedBayesNet shortcut();
};
typedef boost::shared_ptr<Node> node_ptr;
typedef boost::shared_ptr<Clique> sharedClique;
private:
/** Map from keys to Node */
typedef std::map<std::string, node_ptr> Nodes;
/** Map from keys to Clique */
typedef std::map<std::string, sharedClique> Nodes;
Nodes nodes_;
/** Roor clique */
node_ptr root_;
sharedClique root_;
/** add a clique */
node_ptr addClique(const conditional_ptr& conditional,
node_ptr parent_clique = node_ptr());
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;
}
public:
@ -91,7 +103,7 @@ namespace gtsam {
bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
/** insert a new conditional */
void insert(const conditional_ptr& conditional);
void insert(const sharedConditional& conditional);
/** number of cliques */
inline size_t size() const {
@ -99,23 +111,22 @@ namespace gtsam {
}
/** return root clique */
node_ptr root() const {
sharedClique root() const {
return root_;
}
/** find the clique to which key belongs */
node_ptr operator[](const std::string& key) const {
sharedClique operator[](const std::string& key) const {
typename Nodes::const_iterator it = nodes_.find(key);
if (it == nodes_.end()) throw(std::invalid_argument(
"BayesTree::operator['" + key + "'): key not found"));
node_ptr clique = it->second;
sharedClique clique = it->second;
return clique;
}
/** return marginal on any variable */
template<class Factor>
conditional_ptr marginal(const std::string& key) const;
sharedConditional marginal(const std::string& key) const;
}; // BayesTree
} /// namespace gtsam

29
cpp/testBayesTree.cpp
View File

@ -17,6 +17,8 @@ using namespace boost::assign;
using namespace gtsam;
typedef BayesTree<ConditionalGaussian> Gaussian;
// Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr B(new SymbolicConditional("B")), L(
new SymbolicConditional("L", "B")), E(
@ -77,12 +79,12 @@ TEST( BayesTree, constructor )
/* ************************************************************************* *
Bayes tree for smoother with "natural" ordering:
x6 x7
x5 : x6
x4 : x5
x3 : x4
x2 : x3
x1 : x2
C1 x6 x7
C2 x5 : x6
C3 x4 : x5
C4 x3 : x4
C5 x2 : x3
C6 x1 : x2
/* ************************************************************************* */
TEST( BayesTree, smoother )
{
@ -96,8 +98,16 @@ TEST( BayesTree, smoother )
GaussianBayesNet::shared_ptr chordalBayesNet = smoother.eliminate(ordering);
// Create the Bayes tree
BayesTree<ConditionalGaussian> bayesTree(*chordalBayesNet);
Gaussian bayesTree(*chordalBayesNet);
LONGS_EQUAL(7,bayesTree.size());
// Get the conditional P(S6|Root)
Vector sigma1 = repeat(2, 0.786153);
ConditionalGaussian::shared_ptr cg(new ConditionalGaussian("x2", zero(2), eye(2), sigma1));
BayesNet<ConditionalGaussian> expected; expected.push_back(cg);
Gaussian::sharedClique C6 = bayesTree["x1"];
Gaussian::sharedBayesNet actual = C6->shortcut();
//CHECK(assert_equal(expected,*actual,1e-4));
}
/* ************************************************************************* *
@ -130,7 +140,7 @@ TEST( BayesTree, balanced_smoother_marginals )
CHECK(assert_equal(expectedSolution,*actualSolution,1e-4));
// Create the Bayes tree
BayesTree<ConditionalGaussian> bayesTree(*chordalBayesNet);
Gaussian bayesTree(*chordalBayesNet);
LONGS_EQUAL(7,bayesTree.size());
// Check root clique
@ -158,9 +168,6 @@ TEST( BayesTree, balanced_smoother_marginals )
ConditionalGaussian expected3("x3", delta, R, sigma3);
ConditionalGaussian::shared_ptr actual3 = bayesTree.marginal<LinearFactor>("x3");
CHECK(assert_equal(expected3,*actual3,1e-4));
// JunctionTree is an undirected tree of cliques
// JunctionTree<ConditionalGaussian> marginals = bayesTree.marginals();
}
/* ************************************************************************* */