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

More work on BayesTree, replaced some print functions with generic tree print, moved Key, and some formatting fixes.

release/4.3a0
Richard Roberts 2013-06-06 15:36:43 +00:00
parent d11833317f
commit f2fbd14f96
27 changed files with 1048 additions and 750 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/base/treeTraversal-inst.h
View File

@ -23,6 +23,7 @@ namespace gtsam {
/** Internal functions used for traversing trees */
namespace treeTraversal {
/* ************************************************************************* */
namespace {
// Internal node used in DFS preorder stack
template<typename NODE, typename DATA>
@ -96,7 +97,7 @@ namespace gtsam {
} else {
// If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear)
(void) std::for_each(node.treeNode->children.rbegin(), node.treeNode->children.rend(),
(void) std::for_each(node.treeNode.children.rbegin(), node.treeNode.children.rend(),
Expander(visitorPre, node.data, stack));
node.expanded = true;
}
@ -121,6 +122,8 @@ namespace gtsam {
forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
}
/* ************************************************************************* */
/** Traversal function for CloneForest */
namespace {
template<typename NODE>
@ -147,6 +150,27 @@ namespace gtsam {
return std::vector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end());
}
/* ************************************************************************* */
/** Traversal function for PrintForest */
namespace {
template<typename NODE>
std::string PrintForestVisitorPre(const NODE& node, const std::string& parentString, const KeyFormatter& formatter)
{
// Print the current node
node.print(parentString + "-", formatter);
// Increment the indentation
return parentString + "| ";
}
}
/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
* To print each node, this function calls the \c print function of the tree nodes. */
template<class FOREST>
void PrintForest(const FOREST& forest, const std::string& str, const KeyFormatter& keyFormatter) {
typedef typename FOREST::Node Node;
DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre<Node>, _1, _2, formatter));
}
}
}

11
gtsam/base/types.cpp
View File

@ -20,11 +20,22 @@
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h>
#include <gtsam/nonlinear/Symbol.h>
namespace gtsam {
/* ************************************************************************* */
std::string _defaultIndexFormatter(Index j) {
return boost::lexical_cast<std::string>(j);
}
/* ************************************************************************* */
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if(asSymbol.chr() > 0)
return (std::string)asSymbol;
else
return boost::lexical_cast<std::string>(key);
}
}

16
gtsam/base/types.h
View File

@ -40,6 +40,22 @@ namespace gtsam {
/** The default IndexFormatter outputs the index */
static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter;
/// Integer nonlinear key type
typedef size_t Key;
/// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter;
// Helper function for DefaultKeyFormatter
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
/**
* Helper class that uses templates to select between two types based on
* whether TEST_TYPE is const or not.

53
gtsam/inference/BayesNetUnordered-inst.h
View File

@ -0,0 +1,53 @@
/* ----------------------------------------------------------------------------
* 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 BayesNet.h
* @brief Bayes network
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const
{
Base::print(s, formatter);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::saveGraph(
const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter)
{
std::ofstream of(s.c_str());
of << "digraph G{\n";
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Index me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
BOOST_FOREACH(Index p, parents)
of << p << "->" << me << std::endl;
}
of << "}";
of.close();
}
}

35
gtsam/inference/BayesNetUnordered.h
View File

@ -15,10 +15,13 @@
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorGraphUnordered.h>
namespace gtsam {
/**
@ -28,32 +31,36 @@ namespace gtsam {
* \nosubgrouping
*/
template<class CONDITIONAL>
class BayesNetUnordered {
class BayesNetUnordered : public FactorGraphUnordered<CONDITIONAL> {
public:
typedef FactorGraphUnordered<CONDITIONAL> Base;
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional
/// Internal tree node that stores the conditional and the elimination parent
struct Node {
sharedConditional conditional;
boost::shared_ptr<Node> parent;
};
typedef boost::shared_ptr<Node> sharedNode; ///< A shared pointer to a node (used internally)
protected:
sharedNode roots_; ///< Tree roots
public:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNet() {};
BayesNetUnordered() {};
/// @}
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "BayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface
/// @{
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
}

14
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

@ -131,7 +131,7 @@ namespace gtsam {
const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; }
friend class BayesTreeUnordered<ConditionalType, DerivedType>;
friend class BayesTreeUnordered<DerivedType>;
protected:
@ -182,17 +182,5 @@ namespace gtsam {
/// @}
};
// \struct Clique
template<class DERIVED, class CONDITIONAL>
const DERIVED* asDerived(
const BayesTreeCliqueBase<DERIVED, CONDITIONAL>* base) {
return static_cast<const DERIVED*>(base);
}
template<class DERIVED, class CONDITIONAL>
DERIVED* asDerived(BayesTreeCliqueBase<DERIVED, CONDITIONAL>* base) {
return static_cast<DERIVED*>(base);
}
}

60
gtsam/inference/BayesTreeCliqueDefault.h Normal file
View File

@ -0,0 +1,60 @@
/* ----------------------------------------------------------------------------
* 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 BayesTreeCliqueDefault.h
* @brief A standard BayesTree clique. iSAM2 uses a specialized clique.
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
namespace gtsam {
/* ************************************************************************* */
/**
* 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 FACTORGRAPH, class BAYESNET>
struct BayesTreeCliqueDefaultUnordered :
public BayesTreeCliqueBaseUnordered<BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET>, FACTORGRAPH, BAYESNET> {
public:
typedef typename BAYESNET::ConditionalType ConditionalType;
typedef BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET> This;
typedef BayesTreeCliqueBaseUnordered<This, FACTORGRAPH, BAYESNET> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeCliqueDefaultUnordered() {}
BayesTreeCliqueDefaultUnordered(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeCliqueDefaultUnordered(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);
}
};
}

757
gtsam/inference/BayesTreeUnordered-inst.h
View File

@ -20,36 +20,24 @@
#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 <gtsam/inference/BayesTreeUnordered.h>
#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 {
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::CliqueData
BayesTreeUnordered<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 {
template<class CLIQUE>
void BayesTreeUnordered<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_) {
@ -58,25 +46,25 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
size_t BayesTree<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const {
template<class CLIQUE>
size_t BayesTreeUnordered<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 {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) 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);
saveGraph(of, root_, keyFormatter);
of<<"}";
of.close();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const {
static int num = 0;
bool first = true;
std::stringstream out;
@ -84,7 +72,7 @@ namespace gtsam {
std::string parent = out.str();
parent += "[label=\"";
BOOST_FOREACH(Index index, clique->conditional_->frontals()) {
BOOST_FOREACH(Key index, clique->conditional_->frontals()) {
if(!first) parent += ","; first = false;
parent += indexFormatter(index);
}
@ -95,7 +83,7 @@ namespace gtsam {
}
first = true;
BOOST_FOREACH(Index sep, clique->conditional_->parents()) {
BOOST_FOREACH(Key sep, clique->conditional_->parents()) {
if(!first) parent += ","; first = false;
parent += indexFormatter(sep);
}
@ -111,8 +99,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::CliqueStats::print(const std::string& s) const {
std::cout << s
<< "avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl
@ -121,9 +109,10 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const {
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::CliqueStats
BayesTreeUnordered<CLIQUE>::CliqueData::getStats() const
{
CliqueStats stats;
double sum = 0.0;
@ -148,31 +137,31 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::Cliques::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << ":\n";
BOOST_FOREACH(sharedClique clique, *this)
clique->printTree("", indexFormatter);
BOOST_FOREACH(sharedClique clique, *this) {
clique->printTree("", keyFormatter); }
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
template<class CLIQUE>
bool BayesTreeUnordered<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) {
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedClique
BayesTreeUnordered<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) {
template<class CLIQUE>
void BayesTreeUnordered<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;
@ -187,9 +176,10 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique(
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) {
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedClique BayesTreeUnordered<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())
@ -202,60 +192,14 @@ namespace gtsam {
}
/* ************************************************************************* */
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) {
template<class CLIQUE>
void BayesTreeUnordered<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);
typename FastList<sharedClique>::iterator child = std::find(parent->children().begin(), parent->children().end(), clique);
assert(child != parent->children().end());
parent->children().erase(child);
}
@ -264,223 +208,153 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Index j, clique->conditional()->frontals()) {
nodes_[j].reset();
BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
nodes_.erase(j);
}
}
/* ************************************************************************* */
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) {
//template<class CLIQUE>
//void BayesTreeUnordered<CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
// const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
// const typename BayesTreeUnordered<CLIQUE>::sharedClique& parent) {
// Helper function to build a non-symbolic tree (e.g. Gaussian) using a
// symbolic tree, used in the BT(BN) constructor.
// // 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())));
// // Build the current clique
// FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
// BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
// cliqueConditionals.push_back(conditionals[j]); }
// typename BayesTreeUnordered<CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end())));
// Add the new clique with the current parent
this->addClique(thisClique, parent);
// // 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); }
}
// // Build the children, whose parent is the new clique
// BOOST_FOREACH(const BayesTreeUnordered<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);
//template<class CLIQUE>
//BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const BayesNet<CONDITIONAL>& bayesNet) {
// // First generate symbolic BT to determine clique structure
// BayesTreeUnordered<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!");
// // 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("BayesTreeUnordered 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 BayesTreeUnordered constructor!");
// if(conditionals[c->firstFrontalKey()])
// throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTreeUnordered constructor!");
conditionals[c->firstFrontalKey()] = c;
}
// conditionals[c->firstFrontalKey()] = c;
// }
// Build the new tree
this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique());
}
// // 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<>
//inline BayesTreeUnordered<IndexConditional>::BayesTreeUnordered(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!");
//template<class CLIQUE>
//BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTreeUnordered<CLIQUE> > subtrees) {
// if (bayesNet.size() == 0)
// throw std::invalid_argument("BayesTreeUnordered::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());
}
// // get the roots of child subtrees and merge their nodes_
// std::list<sharedClique> childRoots;
// typedef BayesTreeUnordered<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);
}
// // 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;
}
// root_ = new_clique;
//}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) {
template<class CLIQUE>
BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const This& other) {
*this = other;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) {
template<class CLIQUE>
BayesTreeUnordered<CLIQUE>& BayesTreeUnordered<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);
template<class CLIQUE>
void BayesTreeUnordered<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 CONDITIONAL, class CLIQUE>
template<class CLIQUE>
bool check_sharedCliques(
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2
const typename BayesTreeUnordered<CLIQUE>::sharedClique& v1,
const typename BayesTreeUnordered<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 {
template<class CLIQUE>
bool BayesTreeUnordered<CLIQUE>::equals(const BayesTreeUnordered<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 CLIQUE>
template<class CONTAINER>
inline Index BayesTree<CONDITIONAL,CLIQUE>::findParentClique(const CONTAINER& parents) const {
Key BayesTreeUnordered<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) {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node
BOOST_FOREACH(const Index& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
typedef typename BayesTreeUnordered<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) {
template<class CLIQUE>
void BayesTreeUnordered<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
@ -490,17 +364,13 @@ namespace gtsam {
assert(!root_);
root_ = subtree;
} else {
Index parentRepresentative = findParentClique(subtree->conditional()->parents());
Key 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);
}
}
@ -508,209 +378,211 @@ namespace gtsam {
/* ************************************************************************* */
// 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
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedFactor BayesTreeUnordered<CLIQUE>::marginalFactor(
Key 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;
return boost::make_shared<FactorType>();
// gttic(BayesTree_marginalFactor);
//
// // get clique containing Index j
// sharedClique clique = this->clique(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
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedBayesNet BayesTreeUnordered<CLIQUE>::marginalBayesNet(
Key j, Eliminate function) const
{
gttic(BayesTree_marginalBayesNet);
return boost::make_shared<BayesNetType>();
//gttic(BayesTree_marginalBayesNet);
// calculate marginal as a factor graph
FactorGraph<FactorType> fg;
fg.push_back(this->marginalFactor(j,function));
//// 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);
//// 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);
//// 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;
//// 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);
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedFactorGraph
BayesTreeUnordered<CLIQUE>::joint(Key j1, Key j2, Eliminate function) const {
return boost::make_shared<FactorGraphType>();
//gttic(BayesTree_joint);
// get clique C1 and C2
sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
//// 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);
//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 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);
//// 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);
//// 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());
//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);
//// 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;
//// 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 CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedBayesNet BayesTreeUnordered<CLIQUE>::jointBayesNet(
Key j1, Key j2, Eliminate function) const
{
return boost::make_shared<BayesNetType>();
//// 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() {
template<class CLIQUE>
void BayesTreeUnordered<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) {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
{
// base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) {
@ -732,13 +604,12 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
template<class CLIQUE>
template<class CONTAINER>
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) {
void BayesTreeUnordered<CLIQUE>::removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans)
{
// process each key of the new factor
BOOST_FOREACH(const Index& j, keys) {
BOOST_FOREACH(const Key& j, keys) {
// get the clique
if(j < nodes_.size()) {
@ -757,8 +628,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::Cliques BayesTree<CONDITIONAL,CLIQUE>::removeSubtree(
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::Cliques BayesTreeUnordered<CLIQUE>::removeSubtree(
const sharedClique& subtree)
{
// Result clique list
@ -782,8 +653,8 @@ namespace gtsam {
(*clique)->deleteCachedShortcutsNonRecursive();
// Remove this node from the nodes index
BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) {
nodes_[j].reset(); }
BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) {
nodes_.erase(j); }
// Erase the parent and children pointers
(*clique)->parent_.reset();
@ -793,23 +664,5 @@ namespace gtsam {
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

229
gtsam/inference/BayesTreeUnordered.h
View File

@ -20,26 +20,14 @@
#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 <string>
#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>
#include <gtsam/base/FastMap.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,
@ -50,28 +38,29 @@ namespace gtsam {
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTree {
template<class CLIQUE>
class BayesTreeUnordered {
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 BayesTreeUnordered<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
typedef typename CLIQUE::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate;
// typedef for shared pointers to cliques
typedef boost::shared_ptr<Clique> sharedClique;
// A convenience class for a list of shared cliques
/** 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;
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
bool equals(const Cliques& other, double tol = 1e-9) const;
};
@ -91,16 +80,16 @@ namespace gtsam {
CliqueStats getStats() const;
};
/** Map from indices to Clique */
typedef std::vector<sharedClique> Nodes;
/** Map from keys to Clique */
typedef FastMap<Key, sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root clique */
sharedClique root_;
/** Root cliques */
std::vector<sharedClique> roots_;
public:
@ -108,13 +97,10 @@ namespace gtsam {
/// @{
/** 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);
BayesTreeUnordered() {}
/** Copy constructor */
BayesTree(const This& other);
BayesTreeUnordered(const This& other);
/** Assignment operator */
This& operator=(const This& other);
@ -123,37 +109,24 @@ namespace gtsam {
/// @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() {}
virtual ~BayesTreeUnordered() {}
/// @}
/// @name Testable
/// @{
/** check equality */
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
bool equals(const This& other, double tol = 1e-9) const;
/** print */
void print(const std::string& s = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
const KeyFormatter& keyFormatter = DefaultKeyFormatter) 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_)
@ -170,17 +143,16 @@ namespace gtsam {
/** 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);
}
/** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
inline sharedClique clique(Index j) const {
return nodes_.at(j);
const sharedClique& clique(Key j) const {
Nodes::const_iterator c = nodes_.find(j);
if(c == nodes_.end())
throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
else
return *c;
}
/** Gather data on all cliques */
@ -190,43 +162,44 @@ namespace gtsam {
size_t numCachedSeparatorMarginals() const;
/** return marginal on any variable */
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
sharedFactor marginalFactor(Key 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;
sharedBayesNet marginalBayesNet(Key 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;
sharedFactorGraph 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;
sharedBayesNet 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;
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) 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_; }
/**
* 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;
/** Remove all nodes */
void clear();
@ -236,21 +209,18 @@ namespace gtsam {
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);
void removePath(sharedClique clique, BayesNetType& 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);
void removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans);
/**
* Remove the requested subtree. */
@ -265,20 +235,6 @@ namespace gtsam {
*/
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
@ -290,7 +246,7 @@ namespace gtsam {
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,
void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
@ -308,32 +264,17 @@ namespace gtsam {
/** 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);
//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>
@ -346,70 +287,4 @@ namespace gtsam {
}; // 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>

20
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -24,13 +24,15 @@
namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE>
boost::shared_ptr<typename ELIMINATIONTREE::BayesNetType>
EliminateableFactorGraph<DERIVED,ELIMINATIONTREE,JUNCTIONTREE>::eliminateSequential(
template<class FACTOR, class FACTORGRAPH, class CONDITIONAL,
class BAYESNET, class ELIMINATIONTREE, class BAYESTREE, class JUNCTIONTREE>
boost::shared_ptr<BAYESNET>
EliminateableFactorGraph<FACTOR, FACTORGRAPH, CONDITIONAL, BAYESNET, ELIMINATIONTREE, BAYESTREE, JUNCTIONTREE>::
eliminateSequential(
const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const
{
// Do elimination
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > result;
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> > result;
if(ordering) {
// Do elimination with given ordering
result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function);
@ -49,13 +51,15 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE>
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType>
EliminateableFactorGraph<DERIVED,ELIMINATIONTREE,JUNCTIONTREE>::eliminateMultifrontal(
template<class FACTOR, class FACTORGRAPH, class CONDITIONAL,
class BAYESNET, class ELIMINATIONTREE, class BAYESTREE, class JUNCTIONTREE>
boost::shared_ptr<BAYESTREE>
EliminateableFactorGraph<FACTOR, FACTORGRAPH, CONDITIONAL, BAYESNET, ELIMINATIONTREE, BAYESTREE, JUNCTIONTREE>::
eliminateMultifrontal(
const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const
{
// Do elimination
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > result;
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> > result;
if(ordering) {
// Do elimination with given ordering
result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function);

41
gtsam/inference/EliminateableFactorGraph.h
View File

@ -30,23 +30,16 @@ namespace gtsam {
* algorithms. Any factor graph holding eliminateable factors can derive from this class to
* expose functions for computing marginals, conditional marginals, doing multifrontal and
* sequential elimination, etc. */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE>
template<class FACTOR, class FACTORGRAPH, class CONDITIONAL,
class BAYESNET, class ELIMINATIONTREE, class BAYESTREE, class JUNCTIONTREE>
class EliminateableFactorGraph {
public:
typedef EliminateableFactorGraph<DERIVED, ELIMINATIONTREE, JUNCTIONTREE> This;
typedef DERIVED FactorGraphType;
typedef ELIMINATIONTREE EliminationTreeType;
typedef JUNCTIONTREE JunctionTreeType;
typedef typename EliminationTreeType::FactorType FactorType;
typedef typename EliminationTreeType::BayesNetType BayesNetType;
typedef typename JunctionTreeType::BayesTreeType BayesTreeType;
typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef boost::function<std::pair<sharedConditional,sharedFactor>(
std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for a dense eliminate subroutine
typedef EliminateableFactorGraph<FACTOR, FACTORGRAPH, CONDITIONAL, BAYESNET, ELIMINATIONTREE, BAYESTREE, JUNCTIONTREE> This;
typedef boost::optional<const OrderingUnordered&> OptionalOrdering;
typedef boost::function<std::pair<boost::shared_ptr<CONDITIONAL>, boost::shared_ptr<FACTOR> >(
std::vector<boost::shared_ptr<FACTOR> >, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
* provided, the ordering provided by COLAMD will be used.
@ -68,7 +61,7 @@ namespace gtsam {
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, boost::none, varIndex);
* \endcode
* */
boost::shared_ptr<BayesNetType>
boost::shared_ptr<BAYESNET>
eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const;
@ -92,7 +85,7 @@ namespace gtsam {
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex);
* \endcode
* */
boost::shared_ptr<BayesTreeType>
boost::shared_ptr<BAYESTREE>
eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const;
@ -100,32 +93,32 @@ namespace gtsam {
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
* where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$
* B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential(const Eliminate& function, const Ordering& ordering,
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential(const Eliminate& function, const std::vector& variables,
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialSequential(const Eliminate& function, const std::vector<Key>& variables,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(const Eliminate& function, const Ordering& ordering,
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes
* tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B)
* \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and
* \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(const Eliminate& function, const std::vector& ordering,
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialMultifrontal(const Eliminate& function, const std::vector<Key>& variables,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
};

31
gtsam/inference/EliminationTreeUnordered-inst.h
View File

@ -59,6 +59,20 @@ namespace gtsam {
return eliminationResult.second;
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::Node::print(
const std::string& str, const KeyFormatter& keyFormatter) const
{
std::cout << str << "(" << formatter(node->key) << ")\n";
BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) {
if(factor)
factor->print(str + "| ");
else
std::cout << str << "| null factor\n";
}
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
@ -186,26 +200,11 @@ namespace gtsam {
return std::make_pair(result, allRemainingFactors);
}
/* ************************************************************************* */
namespace {
template<class ETREE>
std::string printVisitor(const typename ETREE::sharedNode& node, const std::string& myString, const KeyFormatter& formatter) {
std::cout << myString << "-(" << formatter(node->key) << ")\n";
BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) {
if(factor)
factor->print(myString + "| ");
else
std::cout << myString << "| null factor\n";
}
return myString + "| ";
}
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
{
treeTraversal::DepthFirstForest(*this, name, boost::bind(&printVisitor<This>, _1, _2, formatter));
treeTraversal::PrintForest(*this, name, formatter);
}
/* ************************************************************************* */

7
gtsam/inference/EliminationTreeUnordered.h
View File

@ -21,7 +21,6 @@
#include <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
@ -59,9 +58,7 @@ namespace gtsam {
typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef boost::function<std::pair<sharedConditional,sharedFactor>(
std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
typedef typename GRAPH::Eliminate Eliminate;
struct Node {
typedef std::vector<sharedFactor> Factors;
@ -73,6 +70,8 @@ namespace gtsam {
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
void print(const std::string& str, const KeyFormatter& keyFormatter) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node

20
gtsam/inference/FactorGraphUnordered.h
View File

@ -22,6 +22,8 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
@ -38,6 +40,7 @@ namespace gtsam {
public:
typedef FactorGraphUnordered<FACTOR> This;
typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
@ -103,12 +106,18 @@ namespace gtsam {
// TODO: are these needed?
/** Add a factor */
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
}
/** Add a factor, will be copy-constructed into a shared_ptr (use push_back to avoid the copy). */
template<class DERIVEDFACTOR>
void add(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
}
/** push back many factors */
void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end());
@ -120,6 +129,15 @@ namespace gtsam {
factors_.insert(end(), firstFactor, lastFactor);
}
/** += syntax for push_back, e.g. graph += f1, f2, f3 */
boost::assign::list_inserter<boost::assign_detail::call_push_back<This>, sharedFactor>
operator+=(const sharedFactor& factor)
{
return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(factor);
}
/**
* @brief Add a vector of derived factors
* @param factors to add

2
gtsam/inference/JunctionTreeUnordered-inst.h
View File

@ -158,7 +158,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
JunctionTreeUnordered<BAYESTREE,GRAPH>& JunctionTreeUnordered<BAYESREE,GRAPH>::operator=(const This& other)
JunctionTreeUnordered<BAYESTREE,GRAPH>& JunctionTreeUnordered<BAYESTREE,GRAPH>::operator=(const This& other)
{
// Start by duplicating the tree.
roots_ = treeTraversal::CloneForest(other);

13
gtsam/inference/JunctionTreeUnordered.h
View File

@ -57,12 +57,11 @@ namespace gtsam {
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef JunctionTreeUnordered<BAYESTREE, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination
typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef boost::function<std::pair<sharedConditional,sharedFactor>(std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
typedef boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
struct Node {
typedef std::vector<Key> Keys;
@ -92,7 +91,7 @@ namespace gtsam {
/// @name Standard Constructors
/// @{
/** Build the junction tree from an elimination tree and a symbolic Bayes net. */
/** Build the junction tree from an elimination tree. */
template<class ETREE>
JunctionTreeUnordered(const ETREE& eliminationTree);
@ -114,8 +113,8 @@ namespace gtsam {
* in GaussianFactorGraph.h
* @return The Bayes net and factor graph resulting from elimination
*/
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminate(Eliminate function) const;
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminate(const Eliminate& function) const;
/// @}

8
gtsam/inference/Key.cpp
View File

@ -27,14 +27,6 @@
namespace gtsam {
/* ************************************************************************* */
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if(asSymbol.chr() > 0)
return (std::string)asSymbol;
else
return boost::lexical_cast<std::string>(key);
}
/* ************************************************************************* */
std::string _multirobotKeyFormatter(gtsam::Key key) {

13
gtsam/inference/Key.h
View File

@ -27,19 +27,6 @@
namespace gtsam {
/// Integer nonlinear key type
typedef size_t Key;
/// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter;
// Helper function for DefaultKeyFormatter
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
// Helper function for Multi-robot Key Formatter
GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);

6
gtsam/inference/OrderingUnordered.h
View File

@ -22,6 +22,8 @@
#include <gtsam/inference/Key.h>
namespace gtsam {
class OrderingUnordered :
typedef std::vector<Key> OrderingUnordered;
class OrderingUnordered : std::vector<Key> {
public:
OrderingUnordered() {}
};
}

28
gtsam/symbolic/SymbolicBayesNetUnordered.cpp Normal file
View File

@ -0,0 +1,28 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicBayesNet.cpp
* @date Oct 29, 2009
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
namespace gtsam {
/* ************************************************************************* */
void SymbolicBayesNetUnordered::noop() const {
}
}

13
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <gtsam/base/types.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
namespace gtsam {
@ -27,12 +27,14 @@ namespace gtsam {
/** Symbolic Bayes Net
* \nosubgrouping
*/
class SymbolicBayesNetUnordered: public SymbolicFactorGraphUnordered {
class SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
public:
typedef SymbolicFactorGraphUnordered Base;
typedef BayesNetUnordered<SymbolicConditionalUnordered> Base;
typedef SymbolicBayesNetUnordered This;
typedef SymbolicConditionalUnordered ConditionalType;
typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard Constructors
/// @{
@ -43,6 +45,11 @@ namespace gtsam {
/// @}
/// @name Standard Interface
/// @{
/// @}
private:
void noop() const; // Function defined in cpp file so that compiler instantiates the base class
};
} // namespace gtsam

98
gtsam/symbolic/SymbolicBayesTreeUnordered.cpp Normal file
View File

@ -0,0 +1,98 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicBayesTree.h
* @date Oct 29, 2009
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <boost/foreach.hpp>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
namespace gtsam {
/* ************************************************************************* */
void SymbolicBayesTreeUnordered::insert(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);
}
}
/* ************************************************************************* */
void SymbolicBayesTreeUnordered::addToCliqueFront(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(Key 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();
this->nodes_.resize(std::max(j+1, this->nodes_.size()));
this->nodes_[j] = clique;
FastVector<Index> newIndices(clique->conditional()->size() + 1);
newIndices[0] = j;
std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1);
clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
}

46
gtsam/symbolic/SymbolicBayesTreeUnordered.h Normal file
View File

@ -0,0 +1,46 @@
/* ----------------------------------------------------------------------------
* 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 SymbolicBayesTree.h
* @date Oct 29, 2009
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
namespace gtsam {
class SymbolicBayesTreeUnordered :
public BayesTreeUnordered<BayesTreeCliqueBaseUnordered<>
{
public:
/** Insert a new conditional */
void insert(const sharedConditional& conditional);
protected:
/**
* 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.
*/
void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
};
}

3
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

@ -18,7 +18,10 @@
#include <boost/make_shared.hpp>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
#include <gtsam/symbolic/SymbolicJunctionTreeUnordered.h>
namespace gtsam {

23
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

@ -20,19 +20,36 @@
#include <gtsam/base/types.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
namespace gtsam { class SymbolicConditionalUnordered; }
namespace gtsam { class SymbolicBayesNet; }
namespace gtsam { class SymbolicEliminationTreeUnordered; }
namespace gtsam { class SymbolicBayesTree; }
namespace gtsam { class SymbolicJunctionTreeUnordered; }
namespace gtsam {
/** Symbolic Factor Graph
* \nosubgrouping
*/
class GTSAM_EXPORT SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
class GTSAM_EXPORT SymbolicFactorGraphUnordered:
public FactorGraphUnordered<SymbolicFactorUnordered>,
public EliminateableFactorGraph<
SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered,
SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered>
{
public:
typedef SymbolicFactorGraphUnordered This;
typedef FactorGraphUnordered<SymbolicFactorUnordered> Base;
typedef EliminateableFactorGraph<
SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered,
SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered>
BaseEliminateable;
typedef BaseEliminateable::Eliminate Eliminate;
/// @name Standard Constructors
/// @{
@ -40,6 +57,10 @@ namespace gtsam {
/** Construct empty factor graph */
SymbolicFactorGraphUnordered() {}
/** Construct from any factor graph with factors derived from SymbolicFactor. */
template<class DERIVEDFACTOR>
SymbolicFactorGraphUnordered(const FactorGraphUnordered<DERIVEDFACTOR>& graph) : Base(graph.begin(), graph.end()) {}
/** Constructor from iterator over factors */
template<typename ITERATOR>
SymbolicFactorGraphUnordered(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}

99
gtsam/symbolic/tests/testSymbolicBayesNet.cpp Normal file
View File

@ -0,0 +1,99 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicBayesNet.cpp
* @brief Unit tests for a symbolic Bayes chain
* @author Frank Dellaert
*/
#include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
using namespace std;
using namespace gtsam;
static const Key _L_ = 0;
static const Key _A_ = 1;
static const Key _B_ = 2;
static const Key _C_ = 3;
static const Key _D_ = 4;
static const Key _E_ = 5;
static SymbolicConditionalUnordered::shared_ptr
B(new SymbolicConditionalUnordered(_B_)),
L(new SymbolicConditionalUnordered(_L_, _B_));
/* ************************************************************************* */
TEST( SymbolicBayesNet, equals )
{
SymbolicBayesNetUnordered f1;
f1.push_back(B);
f1.push_back(L);
SymbolicBayesNetUnordered f2;
f2.push_back(L);
f2.push_back(B);
CHECK(f1.equals(f1));
CHECK(!f1.equals(f2));
}
/* ************************************************************************* */
TEST( SymbolicBayesNet, combine )
{
SymbolicConditionalUnordered::shared_ptr
A(new SymbolicConditionalUnordered(_A_,_B_,_C_)),
B(new SymbolicConditionalUnordered(_B_,_C_)),
C(new SymbolicConditionalUnordered(_C_));
// p(A|BC)
SymbolicBayesNetUnordered p_ABC;
p_ABC.push_back(A);
// P(BC)=P(B|C)P(C)
SymbolicBayesNetUnordered p_BC;
p_BC.push_back(B);
p_BC.push_back(C);
// P(ABC) = P(A|BC) P(BC)
p_ABC.push_back(p_BC);
SymbolicBayesNetUnordered expected;
expected.push_back(A);
expected.push_back(B);
expected.push_back(C);
CHECK(assert_equal(expected,p_ABC));
}
/* ************************************************************************* */
TEST(SymbolicBayesNet, saveGraph) {
SymbolicBayesNetUnordered bn;
bn.add(SymbolicConditionalUnordered(_A_, _B_));
std::vector<Index> keys;
keys.push_back(_B_);
keys.push_back(_C_);
keys.push_back(_D_);
bn.add(SymbolicConditionalUnordered::FromKeys(keys,2));
bn.add(SymbolicConditionalUnordered(_D_));
bn.saveGraph("SymbolicBayesNet.dot");
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

116
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp Normal file
View File

@ -0,0 +1,116 @@
/* ----------------------------------------------------------------------------
* 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 testSymbolicFactorGraph.cpp
* @brief Unit tests for symbolic factor graphs
* @author Christian Potthast
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
//TEST(SymbolicFactorGraph, eliminateFrontals) {
//
// SymbolicFactorGraph sfgOrig;
// sfgOrig.push_factor(0,1);
// sfgOrig.push_factor(0,2);
// sfgOrig.push_factor(1,3);
// sfgOrig.push_factor(1,4);
// sfgOrig.push_factor(2,3);
// sfgOrig.push_factor(4,5);
//
// SymbolicConditionalUnordered::shared_ptr actualCond;
// SymbolicFactorGraph::shared_ptr actualSfg;
// boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2);
//
// vector<Index> condIndices;
// condIndices += 0,1,2,3,4;
// IndexConditional expectedCond(condIndices, 2);
//
// SymbolicFactorGraph expectedSfg;
// expectedSfg.push_factor(2,3);
// expectedSfg.push_factor(4,5);
// expectedSfg.push_factor(2,3,4);
//
// EXPECT(assert_equal(expectedSfg, actualSfg));
// EXPECT(assert_equal(expectedCond, *actualCond));
//}
///* ************************************************************************* */
//TEST( SymbolicFactorGraph, EliminateOne )
//{
// // create a test graph
// SymbolicFactorGraph fg;
// fg.push_factor(vx2, vx1);
//
// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1);
// SymbolicFactorGraph expected;
// expected.push_back(boost::shared_ptr<IndexFactor>());
// expected.push_factor(vx1);
//
// CHECK(assert_equal(expected, fg));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, constructFromBayesNet )
{
// create expected factor graph
SymbolicFactorGraphUnordered expected;
expected.push_factor(0, 1, 2);
expected.push_factor(1, 2);
expected.push_factor(1);
// create Bayes Net
SymbolicBayesNetUnordered bayesNet;
bayesNet.add(SymbolicConditionalUnordered(0, 1, 2));
bayesNet.add(SymbolicConditionalUnordered(1, 2));
bayesNet.add(SymbolicConditionalUnordered(1));
// create actual factor graph from a Bayes Net
SymbolicFactorGraphUnordered actual(bayesNet);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, push_back )
{
// Create two factor graphs and expected combined graph
SymbolicFactorGraphUnordered fg1, fg2, expected;
fg1.push_factor(1);
fg1.push_factor(0, 1);
fg2.push_factor(1, 2);
fg2.push_factor(0, 2);
expected.push_factor(1);
expected.push_factor(0, 1);
expected.push_factor(1, 2);
expected.push_factor(0, 2);
// combine
SymbolicFactorGraphUnordered actual;
actual.push_back(fg1);
actual.push_back(fg2);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */