Loading...
Searching...
No Matches
BayesTree-inst.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/timing.h>
#include <boost/optional.hpp>
#include <fstream>
namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
BayesTreeCliqueData stats;
for (const sharedClique& root : roots_) getCliqueData(root, &stats);
return stats;
}
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::getCliqueData(sharedClique clique,
BayesTreeCliqueData* stats) const {
const auto conditional = clique->conditional();
stats->conditionalSizes.push_back(conditional->nrFrontals());
stats->separatorSizes.push_back(conditional->nrParents());
for (sharedClique c : clique->children) {
getCliqueData(c, stats);
}
}
/* ************************************************************************* */
template<class CLIQUE>
size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
size_t count = 0;
for(const sharedClique& root: roots_)
count += root->numCachedSeparatorMarginals();
return count;
}
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::dot(std::ostream& os,
const KeyFormatter& keyFormatter) const {
if (roots_.empty())
throw std::invalid_argument(
"the root of Bayes tree has not been initialized!");
os << "digraph G{\n";
for (const sharedClique& root : roots_) dot(os, root, keyFormatter);
os << "}";
std::flush(os);
}
/* ************************************************************************* */
template <class CLIQUE>
std::string BayesTree<CLIQUE>::dot(const KeyFormatter& keyFormatter) const {
std::stringstream ss;
dot(ss, keyFormatter);
return ss.str();
}
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::saveGraph(const std::string& filename,
const KeyFormatter& keyFormatter) const {
std::ofstream of(filename.c_str());
dot(of, keyFormatter);
of.close();
}
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
const KeyFormatter& keyFormatter,
int parentnum) const {
static int num = 0;
bool first = true;
std::stringstream out;
out << num;
std::string parent = out.str();
parent += "[label=\"";
for (Key key : clique->conditional_->frontals()) {
if (!first) parent += ", ";
first = false;
parent += keyFormatter(key);
}
if (clique->parent()) {
parent += " : ";
s << parentnum << "->" << num << "\n";
}
first = true;
for (Key parentKey : clique->conditional_->parents()) {
if (!first) parent += ", ";
first = false;
parent += keyFormatter(parentKey);
}
parent += "\"];\n";
s << parent;
parentnum = num;
for (sharedClique c : clique->children) {
num++;
dot(s, c, keyFormatter, parentnum);
}
}
/* ************************************************************************* */
template<class CLIQUE>
size_t BayesTree<CLIQUE>::size() const {
size_t size = 0;
for(const sharedClique& clique: roots_)
size += clique->treeSize();
return size;
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
for(Key j: clique->conditional()->frontals())
nodes_[j] = clique;
if (parent_clique != nullptr) {
clique->parent_ = parent_clique;
parent_clique->children.push_back(clique);
} else {
roots_.push_back(clique);
}
}
/* ************************************************************************* */
namespace {
template <class FACTOR, class CLIQUE>
struct _pushCliqueFunctor {
_pushCliqueFunctor(FactorGraph<FACTOR>* graph_) : graph(graph_) {}
FactorGraph<FACTOR>* graph;
int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
graph->push_back(clique->conditional_);
return 0;
}
};
} // namespace
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::addFactorsToGraph(
FactorGraph<FactorType>* graph) const {
// Traverse the BayesTree and add all conditionals to this graph
int data = 0; // Unused
_pushCliqueFunctor<FactorType, CLIQUE> functor(graph);
treeTraversal::DepthFirstForest(*this, data, functor);
}
/* ************************************************************************* */
template<class CLIQUE>
BayesTree<CLIQUE>::BayesTree(const This& other) {
*this = other;
}
/* ************************************************************************* */
namespace {
template<typename NODE>
boost::shared_ptr<NODE>
BayesTreeCloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
{
// Clone the current node and add it to its cloned parent
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
clone->children.clear();
clone->parent_ = parentPointer;
parentPointer->children.push_back(clone);
return clone;
}
}
/* ************************************************************************* */
template<class CLIQUE>
BayesTree<CLIQUE>& BayesTree<CLIQUE>::operator=(const This& other) {
this->clear();
boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
for(const sharedClique& root: rootContainer->children) {
root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique
insertRoot(root);
}
return *this;
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
treeTraversal::PrintForest(*this, s, keyFormatter);
}
/* ************************************************************************* */
// binary predicate to test equality of a pair for use in equals
template<class CLIQUE>
bool check_sharedCliques(
const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v1,
const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v2
) {
return v1.first == v2.first &&
((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
}
/* ************************************************************************* */
template<class CLIQUE>
bool BayesTree<CLIQUE>::equals(const BayesTree<CLIQUE>& other, double tol) const {
return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
}
/* ************************************************************************* */
template<class CLIQUE>
template<class CONTAINER>
Key BayesTree<CLIQUE>::findParentClique(const CONTAINER& parents) const {
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent;
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node
for(const Key& j: subtree->conditional()->frontals()) {
bool inserted = nodes_.insert(std::make_pair(j, subtree)).second;
assert(inserted); (void)inserted;
}
// Fill index for each child
typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
for(const sharedClique& child: subtree->children) {
fillNodesIndex(child); }
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::insertRoot(const sharedClique& subtree) {
roots_.push_back(subtree); // Add to roots
fillNodesIndex(subtree); // Populate nodes index
}
/* ************************************************************************* */
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTree<CLIQUE>::sharedConditional
BayesTree<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
{
gttic(BayesTree_marginalFactor);
// get clique containing Key j
sharedClique clique = this->clique(j);
// calculate or retrieve its marginal P(C) = P(F,S)
FactorGraphType cliqueMarginal = clique->marginal2(function);
// Now, marginalize out everything that is not variable j
BayesNetType marginalBN =
*cliqueMarginal.marginalMultifrontalBayesNet(Ordering{j}, function);
// The Bayes net should contain only one conditional for variable j, so return it
return marginalBN.front();
}
/* ************************************************************************* */
// Find two cliques, their joint, then marginalizes
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTree<CLIQUE>::sharedFactorGraph
BayesTree<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
{
gttic(BayesTree_joint);
return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
}
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTree<CLIQUE>::sharedBayesNet
BayesTree<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
{
gttic(BayesTree_jointBayesNet);
// get clique C1 and C2
sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
gttic(Lowest_common_ancestor);
// Find lowest common ancestor clique
sharedClique B; {
// Build two paths to the root
FastList<sharedClique> path1, path2; {
sharedClique p = C1;
while(p) {
path1.push_front(p);
p = p->parent();
}
} {
sharedClique p = C2;
while(p) {
path2.push_front(p);
p = p->parent();
}
}
// Find the path intersection
typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
if(*p1 == *p2)
B = *p1;
while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
B = *p1;
++p1;
++p2;
}
}
gttoc(Lowest_common_ancestor);
// Build joint on all involved variables
FactorGraphType p_BC1C2;
if(B)
{
// Compute marginal on lowest common ancestor clique
gttic(LCA_marginal);
FactorGraphType p_B = B->marginal2(function);
gttoc(LCA_marginal);
// Compute shortcuts of the requested cliques given the lowest common ancestor
gttic(Clique_shortcuts);
BayesNetType p_C1_Bred = C1->shortcut(B, function);
BayesNetType p_C2_Bred = C2->shortcut(B, function);
gttoc(Clique_shortcuts);
// Factor the shortcuts to be conditioned on the full root
// Get the set of variables to eliminate, which is C1\B.
gttic(Full_root_factoring);
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
KeyVector C1_minus_B; {
KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
for(const Key j: *B->conditional()) {
C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
}
// Factor into C1\B | B.
sharedFactorGraph temp_remaining;
boost::tie(p_C1_B, temp_remaining) =
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
}
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
KeyVector C2_minus_B; {
KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
for(const Key j: *B->conditional()) {
C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
}
// Factor into C2\B | B.
sharedFactorGraph temp_remaining;
boost::tie(p_C2_B, temp_remaining) =
FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
}
gttoc(Full_root_factoring);
gttic(Variable_joint);
p_BC1C2 += p_B;
p_BC1C2 += *p_C1_B;
p_BC1C2 += *p_C2_B;
if(C1 != B)
p_BC1C2 += C1->conditional();
if(C2 != B)
p_BC1C2 += C2->conditional();
gttoc(Variable_joint);
}
else
{
// The nodes have no common ancestor, they're in different trees, so they're joint is just the
// product of their marginals.
gttic(Disjoint_marginals);
p_BC1C2 += C1->marginal2(function);
p_BC1C2 += C2->marginal2(function);
gttoc(Disjoint_marginals);
}
// now, marginalize out everything that is not variable j1 or j2
return p_BC1C2.marginalMultifrontalBayesNet(Ordering{j1, j2}, function);
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::clear() {
// Remove all nodes and clear the root pointer
nodes_.clear();
roots_.clear();
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::deleteCachedShortcuts() {
for(const sharedClique& root: roots_) {
root->deleteCachedShortcuts();
}
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::removeClique(sharedClique clique)
{
if (clique->isRoot()) {
typename Roots::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 Roots::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
assert(child != parent->children.end());
parent->children.erase(child);
}
// orphan my children
for(sharedClique child: clique->children)
child->parent_ = typename Clique::weak_ptr();
for(Key j: clique->conditional()->frontals()) {
nodes_.unsafe_erase(j);
}
}
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
Cliques* orphans) {
// base case is nullptr, if so we do nothing and return empties above
if (clique) {
// remove the clique from orphans in case it has been added earlier
orphans->remove(clique);
// remove me
this->removeClique(clique);
// remove path above me
this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn,
orphans);
// add children to list of orphans (splice also removed them from
// clique->children_)
orphans->insert(orphans->begin(), clique->children.begin(),
clique->children.end());
clique->children.clear();
bn->push_back(clique->conditional_);
}
}
/* *************************************************************************
*/
template <class CLIQUE>
void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType* bn,
Cliques* orphans) {
gttic(removetop);
// process each key of the new factor
for (const Key& j : keys) {
// get the clique
// TODO(frank): Nodes will be searched again in removeClique
typename Nodes::const_iterator node = nodes_.find(j);
if (node != nodes_.end()) {
// remove path from clique to root
this->removePath(node->second, bn, orphans);
}
}
// Delete cachedShortcuts for each orphan subtree
// TODO(frank): Consider Improving
for (sharedClique& orphan : *orphans) orphan->deleteCachedShortcuts();
}
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTree<CLIQUE>::Cliques BayesTree<CLIQUE>::removeSubtree(
const sharedClique& subtree)
{
// Result clique list
Cliques cliques;
cliques.push_back(subtree);
// Remove the first clique from its parents
if(!subtree->isRoot())
subtree->parent()->children.erase(std::find(
subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree));
else
roots_.erase(std::find(roots_.begin(), roots_.end(), subtree));
// Add all subtree cliques and erase the children and parent of each
for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
{
// Add children
for(const sharedClique& child: (*clique)->children) {
cliques.push_back(child); }
// Delete cached shortcuts
(*clique)->deleteCachedShortcutsNonRecursive();
// Remove this node from the nodes index
for(Key j: (*clique)->conditional()->frontals()) {
nodes_.unsafe_erase(j); }
// Erase the parent and children pointers
(*clique)->parent_.reset();
(*clique)->children.clear();
}
return cliques;
}
}
timing.h
Timing utilities.
treeTraversal-inst.h
Ordering.h
Variable ordering for the elimination algorithm.
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:205
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
gtsam::BayesTreeCliqueData
store all the sizes
Definition BayesTree.h:48
gtsam::BayesTree::nodes_
Nodes nodes_
Map from indices to Clique.
Definition BayesTree.h:100
gtsam::BayesTree::removeClique
void removeClique(sharedClique clique)
remove a clique: warning, can result in a forest
Definition BayesTree-inst.h:424
gtsam::BayesTree::joint
sharedFactorGraph joint(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables Limitation: can only calculate joint if cliques are disjoint or one of ...
Definition BayesTree-inst.h:293
gtsam::BayesTree::fillNodesIndex
void fillNodesIndex(const sharedClique &subtree)
Fill the nodes index for a subtree.
Definition BayesTree-inst.h:246
gtsam::BayesTree::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output to graphviz format, stream version.
Definition BayesTree-inst.h:64
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
gtsam::BayesTree::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition BayesTree-inst.h:230
gtsam::BayesTree::operator=
This & operator=(const This &other)
Assignment operator.
Definition BayesTree-inst.h:199
gtsam::BayesTree::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
gtsam::BayesTree::BayesTree
BayesTree()
Create an empty Bayes Tree.
Definition BayesTree.h:109
gtsam::BayesTree::clear
void clear()
Remove all nodes.
Definition BayesTree-inst.h:408
gtsam::BayesTree::roots_
Roots roots_
Root cliques.
Definition BayesTree.h:103
gtsam::BayesTree::addClique
void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
add a clique (top down)
Definition BayesTree-inst.h:142
gtsam::BayesTree::jointBayesNet
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables as a BayesNet Limitation: can only calculate joint if cliques are disjo...
Definition BayesTree-inst.h:302
gtsam::BayesTree::findParentClique
Key findParentClique(const CONTAINER &parents) const
Find parent clique of a conditional.
Definition BayesTree-inst.h:238
gtsam::BayesTree::size
size_t size() const
number of cliques
Definition BayesTree-inst.h:133
gtsam::BayesTree::deleteCachedShortcuts
void deleteCachedShortcuts()
Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
Definition BayesTree-inst.h:416
gtsam::BayesTree::removePath
void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans)
Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots...
Definition BayesTree-inst.h:448
gtsam::BayesTree::Cliques
FastList< sharedClique > Cliques
A convenience class for a list of shared cliques.
Definition BayesTree.h:89
gtsam::BayesTree::marginalFactor
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Return marginal on any variable.
Definition BayesTree-inst.h:270
gtsam::BayesTree::clique
const sharedClique & clique(Key j) const
alternate syntax for matlab: find the clique that contains the variable with Key j
Definition BayesTree.h:152
gtsam::BayesTree::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTree-inst.h:55
gtsam::BayesTree::getCliqueData
BayesTreeCliqueData getCliqueData() const
Gather data on all cliques.
Definition BayesTree-inst.h:35
gtsam::BayesTree::removeSubtree
Cliques removeSubtree(const sharedClique &subtree)
Remove the requested subtree.
Definition BayesTree-inst.h:496
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Insert a new subtree with known parent clique.
Definition BayesTree-inst.h:260
gtsam::BayesTree::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
output to file with graphviz format.
Definition BayesTree-inst.h:85
gtsam::BayesTree::removeTop
void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans)
Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
Definition BayesTree-inst.h:475
gtsam::Ordering
Definition Ordering.h:34