Created BayesTreeUnordered
Richard Roberts
parent
12ba0351d0
commit
3c7558d4be
|
|
@ -0,0 +1,815 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file BayesTree-inl.h
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
* @author Michael Kaess
|
||||
* @author Viorela Ila
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/FastSet.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
#include <gtsam/inference/GenericSequentialSolver.h>
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <algorithm>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using boost::assign::operator+=;
|
||||
#include <boost/format.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData
|
||||
BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const {
|
||||
CliqueData data;
|
||||
getCliqueData(data, root_);
|
||||
return data;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
|
||||
data.conditionalSizes.push_back((*clique)->nrFrontals());
|
||||
data.separatorSizes.push_back((*clique)->nrParents());
|
||||
BOOST_FOREACH(sharedClique c, clique->children_) {
|
||||
getCliqueData(data, c);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
size_t BayesTree<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
|
||||
return (root_) ? root_->numCachedSeparatorMarginals() : 0;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const {
|
||||
if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
|
||||
std::ofstream of(s.c_str());
|
||||
of<< "digraph G{\n";
|
||||
saveGraph(of, root_, indexFormatter);
|
||||
of<<"}";
|
||||
of.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
|
||||
static int num = 0;
|
||||
bool first = true;
|
||||
std::stringstream out;
|
||||
out << num;
|
||||
std::string parent = out.str();
|
||||
parent += "[label=\"";
|
||||
|
||||
BOOST_FOREACH(Index index, clique->conditional_->frontals()) {
|
||||
if(!first) parent += ","; first = false;
|
||||
parent += indexFormatter(index);
|
||||
}
|
||||
|
||||
if( clique != root_){
|
||||
parent += " : ";
|
||||
s << parentnum << "->" << num << "\n";
|
||||
}
|
||||
|
||||
first = true;
|
||||
BOOST_FOREACH(Index sep, clique->conditional_->parents()) {
|
||||
if(!first) parent += ","; first = false;
|
||||
parent += indexFormatter(sep);
|
||||
}
|
||||
parent += "\"];\n";
|
||||
s << parent;
|
||||
parentnum = num;
|
||||
|
||||
BOOST_FOREACH(sharedClique c, clique->children_) {
|
||||
num++;
|
||||
saveGraph(s, c, indexFormatter, parentnum);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const {
|
||||
std::cout << s
|
||||
<<"avg Conditional Size: " << avgConditionalSize << std::endl
|
||||
<< "max Conditional Size: " << maxConditionalSize << std::endl
|
||||
<< "avg Separator Size: " << avgSeparatorSize << std::endl
|
||||
<< "max Separator Size: " << maxSeparatorSize << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
|
||||
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
|
||||
CliqueStats stats;
|
||||
|
||||
double sum = 0.0;
|
||||
size_t max = 0;
|
||||
BOOST_FOREACH(size_t s, conditionalSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgConditionalSize = sum / (double)conditionalSizes.size();
|
||||
stats.maxConditionalSize = max;
|
||||
|
||||
sum = 0.0;
|
||||
max = 1;
|
||||
BOOST_FOREACH(size_t s, separatorSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgSeparatorSize = sum / (double)separatorSizes.size();
|
||||
stats.maxSeparatorSize = max;
|
||||
|
||||
return stats;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const {
|
||||
std::cout << s << ":\n";
|
||||
BOOST_FOREACH(sharedClique clique, *this)
|
||||
clique->printTree("", indexFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
|
||||
return other == *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique
|
||||
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
|
||||
sharedClique new_clique(new Clique(conditional));
|
||||
addClique(new_clique, parent_clique);
|
||||
return new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
|
||||
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
|
||||
BOOST_FOREACH(Index j, (*clique)->frontals())
|
||||
nodes_[j] = clique;
|
||||
if (parent_clique != NULL) {
|
||||
clique->parent_ = parent_clique;
|
||||
parent_clique->children_.push_back(clique);
|
||||
} else {
|
||||
assert(!root_);
|
||||
root_ = clique;
|
||||
}
|
||||
clique->assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
|
||||
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) {
|
||||
sharedClique new_clique(new Clique(conditional));
|
||||
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
|
||||
BOOST_FOREACH(Index j, conditional->frontals())
|
||||
nodes_[j] = new_clique;
|
||||
new_clique->children_ = child_cliques;
|
||||
BOOST_FOREACH(sharedClique& child, child_cliques)
|
||||
child->parent_ = new_clique;
|
||||
new_clique->assertInvariants();
|
||||
return new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
// recursively permute the cliques and internal conditionals
|
||||
if (root_)
|
||||
root_->permuteWithInverse(inversePermutation);
|
||||
|
||||
// need to know what the largest key is to get the right number of cliques
|
||||
Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end());
|
||||
|
||||
// Update the nodes structure
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::Nodes newNodes(maxIndex+1);
|
||||
// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation
|
||||
for(size_t i = 0; i < nodes_.size(); ++i)
|
||||
newNodes[inversePermutation[i]] = nodes_[i];
|
||||
|
||||
nodes_ = newNodes;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
|
||||
static const bool debug = false;
|
||||
#ifndef NDEBUG
|
||||
// Debug check to make sure the conditional variable is ordered lower than
|
||||
// its parents and that all of its parents are present either in this
|
||||
// clique or its separator.
|
||||
BOOST_FOREACH(Index parent, conditional->parents()) {
|
||||
assert(parent > conditional->lastFrontalKey());
|
||||
const Clique& cliquer(*clique);
|
||||
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
|
||||
}
|
||||
#endif
|
||||
if(debug) conditional->print("Adding conditional ");
|
||||
if(debug) clique->print("To clique ");
|
||||
Index j = conditional->lastFrontalKey();
|
||||
bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size()));
|
||||
bayesTree.nodes_[j] = clique;
|
||||
FastVector<Index> newIndices((*clique)->size() + 1);
|
||||
newIndices[0] = j;
|
||||
std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1);
|
||||
clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
|
||||
if(debug) clique->print("Expanded clique is ");
|
||||
clique->assertInvariants();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
|
||||
|
||||
if (clique->isRoot())
|
||||
root_.reset();
|
||||
else { // detach clique from parent
|
||||
sharedClique parent = clique->parent_.lock();
|
||||
typename FastList<typename CLIQUE::shared_ptr>::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(Index j, clique->conditional()->frontals()) {
|
||||
nodes_[j].reset();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
|
||||
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent) {
|
||||
|
||||
// Helper function to build a non-symbolic tree (e.g. Gaussian) using a
|
||||
// symbolic tree, used in the BT(BN) constructor.
|
||||
|
||||
// Build the current clique
|
||||
FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
|
||||
BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
|
||||
cliqueConditionals.push_back(conditionals[j]); }
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end())));
|
||||
|
||||
// Add the new clique with the current parent
|
||||
this->addClique(thisClique, parent);
|
||||
|
||||
// Build the children, whose parent is the new clique
|
||||
BOOST_FOREACH(const BayesTree<IndexConditional>::sharedClique& child, symbolic->children()) {
|
||||
this->recursiveTreeBuild(child, conditionals, thisClique); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) {
|
||||
// First generate symbolic BT to determine clique structure
|
||||
BayesTree<IndexConditional> sbt(bayesNet);
|
||||
|
||||
// Build index of variables to conditionals
|
||||
std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1);
|
||||
BOOST_FOREACH(const boost::shared_ptr<CONDITIONAL>& c, bayesNet) {
|
||||
if(c->nrFrontals() != 1)
|
||||
throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals");
|
||||
if(c->firstFrontalKey() >= conditionals.size())
|
||||
throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!");
|
||||
if(conditionals[c->firstFrontalKey()])
|
||||
throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!");
|
||||
|
||||
conditionals[c->firstFrontalKey()] = c;
|
||||
}
|
||||
|
||||
// Build the new tree
|
||||
this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<>
|
||||
inline BayesTree<IndexConditional>::BayesTree(const BayesNet<IndexConditional>& bayesNet) {
|
||||
BayesNet<IndexConditional>::const_reverse_iterator rit;
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||
insert(*this, *rit);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees) {
|
||||
if (bayesNet.size() == 0)
|
||||
throw std::invalid_argument("BayesTree::insert: empty bayes net!");
|
||||
|
||||
// get the roots of child subtrees and merge their nodes_
|
||||
std::list<sharedClique> childRoots;
|
||||
typedef BayesTree<CONDITIONAL,CLIQUE> Tree;
|
||||
BOOST_FOREACH(const Tree& subtree, subtrees) {
|
||||
nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end());
|
||||
childRoots.push_back(subtree.root());
|
||||
}
|
||||
|
||||
// create a new clique and add all the conditionals to the clique
|
||||
sharedClique new_clique;
|
||||
typename BayesNet<CONDITIONAL>::sharedConditional conditional;
|
||||
BOOST_REVERSE_FOREACH(conditional, bayesNet) {
|
||||
if (!new_clique.get())
|
||||
new_clique = addClique(conditional,childRoots);
|
||||
else
|
||||
addToCliqueFront(*this, conditional, new_clique);
|
||||
}
|
||||
|
||||
root_ = new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) {
|
||||
*this = other;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) {
|
||||
this->clear();
|
||||
other.cloneTo(*this);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const {
|
||||
if (root_.use_count() == 0) {
|
||||
std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl;
|
||||
return;
|
||||
}
|
||||
std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl;
|
||||
if (nodes_.empty()) return;
|
||||
root_->printTree("", indexFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// binary predicate to test equality of a pair for use in equals
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool check_sharedCliques(
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2
|
||||
) {
|
||||
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
bool BayesTree<CONDITIONAL,CLIQUE>::equals(const BayesTree<CONDITIONAL,CLIQUE>& other,
|
||||
double tol) const {
|
||||
return size()==other.size() &&
|
||||
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
template<class CONTAINER>
|
||||
inline Index BayesTree<CONDITIONAL,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 CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional)
|
||||
{
|
||||
static const bool debug = false;
|
||||
|
||||
// get indices and parents
|
||||
const typename CONDITIONAL::Parents& parents = conditional->parents();
|
||||
|
||||
if(debug) conditional->print("Adding conditional ");
|
||||
|
||||
// if no parents, start a new root clique
|
||||
if (parents.empty()) {
|
||||
if(debug) std::cout << "No parents so making root" << std::endl;
|
||||
bayesTree.root_ = bayesTree.addClique(conditional);
|
||||
return;
|
||||
}
|
||||
|
||||
// otherwise, find the parent clique by using the index data structure
|
||||
// to find the lowest-ordered parent
|
||||
Index parentRepresentative = bayesTree.findParentClique(parents);
|
||||
if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
|
||||
sharedClique parent_clique = bayesTree[parentRepresentative];
|
||||
if(debug) parent_clique->print("Parent clique is ");
|
||||
|
||||
// if the parents and parent clique have the same size, add to parent clique
|
||||
if ((*parent_clique)->size() == size_t(parents.size())) {
|
||||
if(debug) std::cout << "Adding to parent clique" << std::endl;
|
||||
#ifndef NDEBUG
|
||||
// Debug check that the parent indices of the new conditional match the indices
|
||||
// currently in the clique.
|
||||
// list<Index>::const_iterator parent = parents.begin();
|
||||
// typename Clique::const_iterator cond = parent_clique->begin();
|
||||
// while(parent != parents.end()) {
|
||||
// assert(cond != parent_clique->end());
|
||||
// assert(*parent == (*cond)->key());
|
||||
// ++ parent;
|
||||
// ++ cond;
|
||||
// }
|
||||
#endif
|
||||
addToCliqueFront(bayesTree, conditional, parent_clique);
|
||||
} else {
|
||||
if(debug) std::cout << "Starting new clique" << std::endl;
|
||||
// otherwise, start a new clique and add it to the tree
|
||||
bayesTree.addClique(conditional,parent_clique);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TODO: remove this function after removing TSAM.cpp
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert(
|
||||
const sharedConditional& clique, std::list<sharedClique>& children, bool isRootClique) {
|
||||
|
||||
if (clique->nrFrontals() == 0)
|
||||
throw std::invalid_argument("BayesTree::insert: empty clique!");
|
||||
|
||||
// create a new clique and add all the conditionals to the clique
|
||||
sharedClique new_clique = addClique(clique, children);
|
||||
if (isRootClique) root_ = new_clique;
|
||||
|
||||
return new_clique;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
|
||||
// Add each frontal variable of this root node
|
||||
BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
|
||||
// Fill index for each child
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, subtree->children_) {
|
||||
fillNodesIndex(child); }
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) {
|
||||
if(subtree) {
|
||||
// Find the parent clique of the new subtree. By the running intersection
|
||||
// property, those separator variables in the subtree that are ordered
|
||||
// lower than the highest frontal variable of the subtree root will all
|
||||
// appear in the separator of the subtree root.
|
||||
if(subtree->conditional()->parents().empty()) {
|
||||
assert(!root_);
|
||||
root_ = subtree;
|
||||
} else {
|
||||
Index parentRepresentative = findParentClique(subtree->conditional()->parents());
|
||||
sharedClique parent = (*this)[parentRepresentative];
|
||||
parent->children_ += subtree;
|
||||
subtree->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
// Now fill in the nodes index
|
||||
if(nodes_.size() == 0 ||
|
||||
*std::max_element(subtree->conditional()->beginFrontals(), subtree->conditional()->endFrontals()) > (nodes_.size() - 1)) {
|
||||
nodes_.resize(subtree->conditional()->lastFrontalKey() + 1);
|
||||
}
|
||||
fillNodesIndex(subtree);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// First finds clique marginal then marginalizes that
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor(
|
||||
Index j, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTree_marginalFactor);
|
||||
|
||||
// get clique containing Index j
|
||||
sharedClique clique = (*this)[j];
|
||||
|
||||
// calculate or retrieve its marginal P(C) = P(F,S)
|
||||
#ifdef OLD_SHORTCUT_MARGINALS
|
||||
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
|
||||
#else
|
||||
FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function);
|
||||
#endif
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
gttoc(Reduce);
|
||||
|
||||
// now, marginalize out everything that is not variable j
|
||||
GenericSequentialSolver<FactorType> solver(cliqueMarginal);
|
||||
boost::shared_ptr<FactorType> result = solver.marginalFactor(inverseReduction[j], function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
result->permuteWithInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet(
|
||||
Index j, Eliminate function) const
|
||||
{
|
||||
gttic(BayesTree_marginalBayesNet);
|
||||
|
||||
// calculate marginal as a factor graph
|
||||
FactorGraph<FactorType> fg;
|
||||
fg.push_back(this->marginalFactor(j,function));
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(fg.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
gttoc(Reduce);
|
||||
|
||||
// eliminate factor graph marginal to a Bayes net
|
||||
boost::shared_ptr<BayesNet<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
bn->permuteWithInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
return bn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Find two cliques, their joint, then marginalizes
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr
|
||||
BayesTree<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const {
|
||||
gttic(BayesTree_joint);
|
||||
|
||||
// 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
|
||||
B = this->root();
|
||||
typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
|
||||
while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
|
||||
B = *p1;
|
||||
++p1;
|
||||
++p2;
|
||||
}
|
||||
}
|
||||
gttoc(Lowest_common_ancestor);
|
||||
|
||||
// Compute marginal on lowest common ancestor clique
|
||||
gttic(LCA_marginal);
|
||||
FactorGraph<FactorType> p_B = B->marginal2(this->root(), function);
|
||||
gttoc(LCA_marginal);
|
||||
|
||||
// Compute shortcuts of the requested cliques given the lowest common ancestor
|
||||
gttic(Clique_shortcuts);
|
||||
BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
|
||||
BayesNet<CONDITIONAL> 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);
|
||||
sharedConditional p_C1_B; {
|
||||
std::vector<Index> C1_minus_B; {
|
||||
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
|
||||
BOOST_FOREACH(const Index 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.
|
||||
FactorGraph<FactorType> temp_remaining;
|
||||
boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
|
||||
}
|
||||
sharedConditional p_C2_B; {
|
||||
std::vector<Index> C2_minus_B; {
|
||||
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
|
||||
BOOST_FOREACH(const Index 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.
|
||||
FactorGraph<FactorType> temp_remaining;
|
||||
boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
|
||||
}
|
||||
gttoc(Full_root_factoring);
|
||||
|
||||
gttic(Variable_joint);
|
||||
// Build joint on all involved variables
|
||||
FactorGraph<FactorType> p_BC1C2;
|
||||
p_BC1C2.push_back(p_B);
|
||||
p_BC1C2.push_back(p_C1_B->toFactor());
|
||||
p_BC1C2.push_back(p_C2_B->toFactor());
|
||||
if(C1 != B)
|
||||
p_BC1C2.push_back(C1->conditional()->toFactor());
|
||||
if(C2 != B)
|
||||
p_BC1C2.push_back(C2->conditional()->toFactor());
|
||||
|
||||
// Reduce the variable indices to start at zero
|
||||
gttic(Reduce);
|
||||
const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys());
|
||||
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
|
||||
if(factor) factor->reduceWithInverse(inverseReduction); }
|
||||
std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]);
|
||||
gttoc(Reduce);
|
||||
|
||||
// now, marginalize out everything that is not variable j
|
||||
GenericSequentialSolver<FactorType> solver(p_BC1C2);
|
||||
boost::shared_ptr<FactorGraph<FactorType> > result = solver.jointFactorGraph(js, function);
|
||||
|
||||
// Undo the reduction
|
||||
gttic(Undo_Reduce);
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, *result) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
|
||||
if(factor) factor->permuteWithInverse(reduction); }
|
||||
gttoc(Undo_Reduce);
|
||||
return result;
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet(
|
||||
Index j1, Index j2, Eliminate function) const {
|
||||
|
||||
// eliminate factor graph marginal to a Bayes net
|
||||
return GenericSequentialSolver<FactorType> (
|
||||
*this->joint(j1, j2, function)).eliminate(function);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::clear() {
|
||||
// Remove all nodes and clear the root pointer
|
||||
nodes_.clear();
|
||||
root_.reset();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
|
||||
|
||||
// base case is NULL, if so we do nothing and return empties above
|
||||
if (clique!=NULL) {
|
||||
|
||||
// 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.splice(orphans.begin(), clique->children_);
|
||||
|
||||
bn.push_back(clique->conditional());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
template<class CONTAINER>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
|
||||
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
BOOST_FOREACH(const Index& j, keys) {
|
||||
|
||||
// get the clique
|
||||
if(j < nodes_.size()) {
|
||||
const sharedClique& clique(nodes_[j]);
|
||||
if(clique) {
|
||||
// remove path from clique to root
|
||||
this->removePath(clique, bn, orphans);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Delete cachedShortcuts for each orphan subtree
|
||||
//TODO: Consider Improving
|
||||
BOOST_FOREACH(sharedClique& orphan, orphans)
|
||||
orphan->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
typename BayesTree<CONDITIONAL,CLIQUE>::Cliques BayesTree<CONDITIONAL,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().remove(subtree);
|
||||
else
|
||||
root_.reset();
|
||||
|
||||
// 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
|
||||
BOOST_FOREACH(const sharedClique& child, (*clique)->children()) {
|
||||
cliques.push_back(child); }
|
||||
|
||||
// Delete cached shortcuts
|
||||
(*clique)->deleteCachedShortcutsNonRecursive();
|
||||
|
||||
// Remove this node from the nodes index
|
||||
BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) {
|
||||
nodes_[j].reset(); }
|
||||
|
||||
// Erase the parent and children pointers
|
||||
(*clique)->parent_.reset();
|
||||
(*clique)->children_.clear();
|
||||
}
|
||||
|
||||
return cliques;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {
|
||||
if(root())
|
||||
cloneTo(newTree, root(), sharedClique());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(
|
||||
This& newTree, const sharedClique& subtree, const sharedClique& parent) const {
|
||||
sharedClique newClique(subtree->clone());
|
||||
newTree.addClique(newClique, parent);
|
||||
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
|
||||
cloneTo(newTree, childClique, newClique);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -0,0 +1,415 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file BayesTree.h
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
#include <deque>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/BayesNet.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase.h>
|
||||
#include <gtsam/inference/IndexConditional.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
|
||||
template<class CONDITIONAL> struct BayesTreeClique;
|
||||
|
||||
/**
|
||||
* Bayes tree
|
||||
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
|
||||
* which could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional.
|
||||
* @tparam CLIQUE The type of the clique data structure, defaults to BayesTreeClique, normally do not change this
|
||||
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
|
||||
*
|
||||
* \addtogroup Multifrontal
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
|
||||
class BayesTree {
|
||||
|
||||
public:
|
||||
|
||||
typedef BayesTree<CONDITIONAL, CLIQUE> This;
|
||||
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
|
||||
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
|
||||
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
|
||||
typedef CONDITIONAL ConditionalType;
|
||||
typedef typename CONDITIONAL::FactorType FactorType;
|
||||
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
|
||||
|
||||
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
|
||||
|
||||
// typedef for shared pointers to cliques
|
||||
typedef boost::shared_ptr<Clique> sharedClique;
|
||||
|
||||
// A convenience class for a list of shared cliques
|
||||
struct Cliques : public FastList<sharedClique> {
|
||||
void print(const std::string& s = "Cliques",
|
||||
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
|
||||
bool equals(const Cliques& other, double tol = 1e-9) const;
|
||||
};
|
||||
|
||||
/** clique statistics */
|
||||
struct CliqueStats {
|
||||
double avgConditionalSize;
|
||||
std::size_t maxConditionalSize;
|
||||
double avgSeparatorSize;
|
||||
std::size_t maxSeparatorSize;
|
||||
void print(const std::string& s = "") const ;
|
||||
};
|
||||
|
||||
/** store all the sizes */
|
||||
struct CliqueData {
|
||||
std::vector<std::size_t> conditionalSizes;
|
||||
std::vector<std::size_t> separatorSizes;
|
||||
CliqueStats getStats() const;
|
||||
};
|
||||
|
||||
/** Map from indices to Clique */
|
||||
typedef std::vector<sharedClique> Nodes;
|
||||
|
||||
protected:
|
||||
|
||||
/** Map from indices to Clique */
|
||||
Nodes nodes_;
|
||||
|
||||
/** Root clique */
|
||||
sharedClique root_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
BayesTree() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
|
||||
explicit BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
|
||||
/** Copy constructor */
|
||||
BayesTree(const This& other);
|
||||
|
||||
/** Assignment operator */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
|
||||
* new root clique and the subtrees are connected to the root clique.
|
||||
*/
|
||||
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~BayesTree() {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** check equality */
|
||||
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "",
|
||||
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Find parent clique of a conditional. It will look at all parents and
|
||||
* return the one with the lowest index in the ordering.
|
||||
*/
|
||||
template<class CONTAINER>
|
||||
Index findParentClique(const CONTAINER& parents) const;
|
||||
|
||||
/** number of cliques */
|
||||
inline size_t size() const {
|
||||
if(root_)
|
||||
return root_->treeSize();
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** Check if there are any cliques in the tree */
|
||||
inline bool empty() const {
|
||||
return nodes_.empty();
|
||||
}
|
||||
|
||||
/** return nodes */
|
||||
const Nodes& nodes() const { return nodes_; }
|
||||
|
||||
/** return root clique */
|
||||
const sharedClique& root() const { return root_; }
|
||||
|
||||
/** find the clique that contains the variable with Index j */
|
||||
inline sharedClique operator[](Index j) const {
|
||||
return nodes_.at(j);
|
||||
}
|
||||
|
||||
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
|
||||
inline sharedClique clique(Index j) const {
|
||||
return nodes_.at(j);
|
||||
}
|
||||
|
||||
/** Gather data on all cliques */
|
||||
CliqueData getCliqueData() const;
|
||||
|
||||
/** Collect number of cliques with cached separator marginals */
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
|
||||
/** return marginal on any variable */
|
||||
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* return marginal on any variable, as a Bayes Net
|
||||
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
|
||||
* This is more expensive than the above function
|
||||
*/
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* return joint on two variables
|
||||
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
|
||||
*/
|
||||
typename FactorGraph<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* return joint on two variables as a BayesNet
|
||||
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
|
||||
*/
|
||||
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const;
|
||||
|
||||
/**
|
||||
* Read only with side effects
|
||||
*/
|
||||
|
||||
/** saves the Tree to a text file in GraphViz format */
|
||||
void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Access the root clique (non-const version) */
|
||||
sharedClique& root() { return root_; }
|
||||
|
||||
/** Access the nodes (non-cost version) */
|
||||
Nodes& nodes() { return nodes_; }
|
||||
|
||||
/** Remove all nodes */
|
||||
void clear();
|
||||
|
||||
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
|
||||
void deleteCachedShortcuts() {
|
||||
root_->deleteCachedShortcuts();
|
||||
}
|
||||
|
||||
/** Apply a permutation to the full tree - also updates the nodes structure */
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/**
|
||||
* Remove path from clique to root and return that path as factors
|
||||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
*/
|
||||
void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* 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& indices, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Remove the requested subtree. */
|
||||
Cliques removeSubtree(const sharedClique& subtree);
|
||||
|
||||
/**
|
||||
* Hang a new subtree off of the existing tree. This finds the appropriate
|
||||
* parent clique for the subtree (which may be the root), and updates the
|
||||
* nodes index with the new cliques in the subtree. None of the frontal
|
||||
* variables in the subtree may appear in the separators of the existing
|
||||
* BayesTree.
|
||||
*/
|
||||
void insert(const sharedClique& subtree);
|
||||
|
||||
/** Insert a new conditional
|
||||
* This function only applies for Symbolic case with IndexCondtional,
|
||||
* We make it static so that it won't be compiled in GaussianConditional case.
|
||||
* */
|
||||
static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
|
||||
|
||||
/**
|
||||
* Insert a new clique corresponding to the given Bayes net.
|
||||
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
|
||||
* i.e. all the variables (frontal and separator) are connected
|
||||
*/
|
||||
sharedClique insert(const sharedConditional& clique,
|
||||
std::list<sharedClique>& children, bool isRootClique = false);
|
||||
|
||||
/**
|
||||
* Create a clone of this object as a shared pointer
|
||||
* Necessary for inheritance in matlab interface
|
||||
*/
|
||||
virtual shared_ptr clone() const {
|
||||
return shared_ptr(new This(*this));
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/** private helper method for saving the Tree to a text file in GraphViz format */
|
||||
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
|
||||
int parentnum = 0) const;
|
||||
|
||||
/** Gather data on a single clique */
|
||||
void getCliqueData(CliqueData& stats, sharedClique clique) const;
|
||||
|
||||
/** remove a clique: warning, can result in a forest */
|
||||
void removeClique(sharedClique clique);
|
||||
|
||||
/** add a clique (top down) */
|
||||
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
|
||||
|
||||
/** add a clique (top down) */
|
||||
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
|
||||
|
||||
/** add a clique (bottom up) */
|
||||
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
|
||||
|
||||
/**
|
||||
* Add a conditional to the front of a clique, i.e. a conditional whose
|
||||
* parents are already in the clique or its separators. This function does
|
||||
* not check for this condition, it just updates the data structures.
|
||||
*/
|
||||
static void addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree,
|
||||
const sharedConditional& conditional, const sharedClique& clique);
|
||||
|
||||
/** Fill the nodes index for a subtree */
|
||||
void fillNodesIndex(const sharedClique& subtree);
|
||||
|
||||
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
|
||||
* symbolic tree, used in the BT(BN) constructor.
|
||||
*/
|
||||
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
|
||||
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent);
|
||||
|
||||
private:
|
||||
|
||||
/** deep copy to another tree */
|
||||
void cloneTo(This& newTree) const;
|
||||
|
||||
/** deep copy to another tree */
|
||||
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(nodes_);
|
||||
ar & BOOST_SERIALIZATION_NVP(root_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class CLIQUE>
|
||||
void _BayesTree_dim_adder(
|
||||
std::vector<size_t>& dims,
|
||||
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
|
||||
|
||||
if(clique) {
|
||||
// Add dims from this clique
|
||||
for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
|
||||
dims[*it] = (*clique)->dim(it);
|
||||
|
||||
// Traverse children
|
||||
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& child, clique->children()) {
|
||||
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL,class CLIQUE>
|
||||
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
|
||||
std::vector<size_t> dimensions(bt.nodes().size(), 0);
|
||||
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
|
||||
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* 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 are the separator. We also have pointers up and down the tree.
|
||||
*
|
||||
* Since our Conditional class already handles multiple frontal variables,
|
||||
* this Clique contains exactly 1 conditional.
|
||||
*
|
||||
* This is the default clique type in a BayesTree, but some algorithms, like
|
||||
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
|
||||
* extra data along with the clique.
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
|
||||
public:
|
||||
typedef CONDITIONAL ConditionalType;
|
||||
typedef BayesTreeClique<CONDITIONAL> This;
|
||||
typedef BayesTreeCliqueBase<This, CONDITIONAL> Base;
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
BayesTreeClique() {}
|
||||
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
|
||||
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
};
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
#include <gtsam/inference/BayesTree-inl.h>
|
||||
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>
|
||||
Loading…
Reference in New Issue