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 */ /** Internal functions used for traversing trees */
namespace treeTraversal { namespace treeTraversal {
/* ************************************************************************* */
namespace { namespace {
// Internal node used in DFS preorder stack // Internal node used in DFS preorder stack
template<typename NODE, typename DATA> template<typename NODE, typename DATA>
@ -96,7 +97,7 @@ namespace gtsam {
} else { } else {
// If not already visited, visit the node and add its children (use reverse iterators so // If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear) // 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)); Expander(visitorPre, node.data, stack));
node.expanded = true; node.expanded = true;
} }
@ -121,6 +122,8 @@ namespace gtsam {
forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>); forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
} }
/* ************************************************************************* */
/** Traversal function for CloneForest */ /** Traversal function for CloneForest */
namespace { namespace {
template<typename NODE> template<typename NODE>
@ -147,6 +150,27 @@ namespace gtsam {
return std::vector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end()); 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 <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/nonlinear/Symbol.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
std::string _defaultIndexFormatter(Index j) { std::string _defaultIndexFormatter(Index j) {
return boost::lexical_cast<std::string>(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 */ /** The default IndexFormatter outputs the index */
static const IndexFormatter DefaultIndexFormatter = &_defaultIndexFormatter; 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 * Helper class that uses templates to select between two types based on
* whether TEST_TYPE is const or not. * 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 Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#pragma once #pragma once
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <gtsam/inference/FactorGraphUnordered.h>
namespace gtsam { namespace gtsam {
/** /**
@ -28,32 +31,36 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template<class CONDITIONAL> template<class CONDITIONAL>
class BayesNetUnordered { class BayesNetUnordered : public FactorGraphUnordered<CONDITIONAL> {
public: public:
typedef FactorGraphUnordered<CONDITIONAL> Base;
typedef typename boost::shared_ptr<CONDITIONAL> sharedConditional; ///< A shared pointer to a conditional 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: public:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** Default constructor as an empty BayesNet */ /** 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 { const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
return cachedSeparatorMarginal_; } return cachedSeparatorMarginal_; }
friend class BayesTreeUnordered<ConditionalType, DerivedType>; friend class BayesTreeUnordered<DerivedType>;
protected: 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 #pragma once
#include <gtsam/base/FastList.h> #include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference.h>
#include <gtsam/inference/GenericSequentialSolver.h>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using boost::assign::operator+=;
#include <boost/format.hpp>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueData typename BayesTreeUnordered<CLIQUE>::CliqueData
BayesTree<CONDITIONAL,CLIQUE>::getCliqueData() const { BayesTreeUnordered<CLIQUE>::getCliqueData() const {
CliqueData data; CliqueData data;
getCliqueData(data, root_); getCliqueData(data, root_);
return data; return data;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const { void BayesTreeUnordered<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back((*clique)->nrFrontals()); data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents()); data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) { BOOST_FOREACH(sharedClique c, clique->children_) {
@ -58,25 +46,25 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
size_t BayesTree<CONDITIONAL,CLIQUE>::numCachedSeparatorMarginals() const { size_t BayesTreeUnordered<CLIQUE>::numCachedSeparatorMarginals() const {
return (root_) ? root_->numCachedSeparatorMarginals() : 0; return (root_) ? root_->numCachedSeparatorMarginals() : 0;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(const std::string &s, const IndexFormatter& indexFormatter) const { 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!"); if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str()); std::ofstream of(s.c_str());
of<< "digraph G{\n"; of<< "digraph G{\n";
saveGraph(of, root_, indexFormatter); saveGraph(of, root_, keyFormatter);
of<<"}"; of<<"}";
of.close(); of.close();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter, int parentnum) const { void BayesTreeUnordered<CLIQUE>::saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& indexFormatter, int parentnum) const {
static int num = 0; static int num = 0;
bool first = true; bool first = true;
std::stringstream out; std::stringstream out;
@ -84,7 +72,7 @@ namespace gtsam {
std::string parent = out.str(); std::string parent = out.str();
parent += "[label=\""; parent += "[label=\"";
BOOST_FOREACH(Index index, clique->conditional_->frontals()) { BOOST_FOREACH(Key index, clique->conditional_->frontals()) {
if(!first) parent += ","; first = false; if(!first) parent += ","; first = false;
parent += indexFormatter(index); parent += indexFormatter(index);
} }
@ -95,7 +83,7 @@ namespace gtsam {
} }
first = true; first = true;
BOOST_FOREACH(Index sep, clique->conditional_->parents()) { BOOST_FOREACH(Key sep, clique->conditional_->parents()) {
if(!first) parent += ","; first = false; if(!first) parent += ","; first = false;
parent += indexFormatter(sep); parent += indexFormatter(sep);
} }
@ -111,8 +99,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::CliqueStats::print(const std::string& s) const { void BayesTreeUnordered<CLIQUE>::CliqueStats::print(const std::string& s) const {
std::cout << s std::cout << s
<< "avg Conditional Size: " << avgConditionalSize << std::endl << "avg Conditional Size: " << avgConditionalSize << std::endl
<< "max Conditional Size: " << maxConditionalSize << std::endl << "max Conditional Size: " << maxConditionalSize << std::endl
@ -121,9 +109,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::CliqueStats typename BayesTreeUnordered<CLIQUE>::CliqueStats
BayesTree<CONDITIONAL,CLIQUE>::CliqueData::getStats() const { BayesTreeUnordered<CLIQUE>::CliqueData::getStats() const
{
CliqueStats stats; CliqueStats stats;
double sum = 0.0; double sum = 0.0;
@ -148,31 +137,31 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::Cliques::print(const std::string& s, const IndexFormatter& indexFormatter) const { void BayesTreeUnordered<CLIQUE>::Cliques::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << ":\n"; std::cout << s << ":\n";
BOOST_FOREACH(sharedClique clique, *this) BOOST_FOREACH(sharedClique clique, *this) {
clique->printTree("", indexFormatter); clique->printTree("", keyFormatter); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
bool BayesTree<CONDITIONAL,CLIQUE>::Cliques::equals(const Cliques& other, double tol) const { bool BayesTreeUnordered<CLIQUE>::Cliques::equals(const Cliques& other, double tol) const {
return other == *this; return other == *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique typename BayesTreeUnordered<CLIQUE>::sharedClique
BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { BayesTreeUnordered<CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
addClique(new_clique, parent_clique); addClique(new_clique, parent_clique);
return new_clique; return new_clique;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) { void BayesTreeUnordered<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, (*clique)->frontals()) BOOST_FOREACH(Index j, (*clique)->frontals())
nodes_[j] = clique; nodes_[j] = clique;
@ -187,9 +176,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::addClique( typename BayesTreeUnordered<CLIQUE>::sharedClique BayesTreeUnordered<CLIQUE>::addClique(
const sharedConditional& conditional, std::list<sharedClique>& child_cliques) { const sharedConditional& conditional, std::list<sharedClique>& child_cliques)
{
sharedClique new_clique(new Clique(conditional)); sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, conditional->frontals()) BOOST_FOREACH(Index j, conditional->frontals())
@ -202,60 +192,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::permuteWithInverse(const Permutation& inversePermutation) { void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
// recursively permute the cliques and internal conditionals {
if (root_)
root_->permuteWithInverse(inversePermutation);
// need to know what the largest key is to get the right number of cliques
Index maxIndex = *std::max_element(inversePermutation.begin(), inversePermutation.end());
// Update the nodes structure
typename BayesTree<CONDITIONAL,CLIQUE>::Nodes newNodes(maxIndex+1);
// inversePermutation.applyToCollection(newNodes, nodes_); // Uses the forward, rather than inverse permutation
for(size_t i = 0; i < nodes_.size(); ++i)
newNodes[inversePermutation[i]] = nodes_[i];
nodes_ = newNodes;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false;
#ifndef NDEBUG
// Debug check to make sure the conditional variable is ordered lower than
// its parents and that all of its parents are present either in this
// clique or its separator.
BOOST_FOREACH(Index parent, conditional->parents()) {
assert(parent > conditional->lastFrontalKey());
const Clique& cliquer(*clique);
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
}
#endif
if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique ");
Index j = conditional->lastFrontalKey();
bayesTree.nodes_.resize(std::max(j+1, bayesTree.nodes_.size()));
bayesTree.nodes_[j] = clique;
FastVector<Index> newIndices((*clique)->size() + 1);
newIndices[0] = j;
std::copy((*clique)->begin(), (*clique)->end(), newIndices.begin()+1);
clique->conditional_ = CONDITIONAL::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::removeClique(sharedClique clique) {
if (clique->isRoot()) if (clique->isRoot())
root_.reset(); root_.reset();
else { // detach clique from parent else { // detach clique from parent
sharedClique parent = clique->parent_.lock(); 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()); assert(child != parent->children().end());
parent->children().erase(child); parent->children().erase(child);
} }
@ -264,223 +208,153 @@ namespace gtsam {
BOOST_FOREACH(sharedClique child, clique->children_) BOOST_FOREACH(sharedClique child, clique->children_)
child->parent_ = typename Clique::weak_ptr(); child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Index j, clique->conditional()->frontals()) { BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
nodes_[j].reset(); nodes_.erase(j);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> //template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic, //void BayesTreeUnordered<CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals, // const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent) { // const typename BayesTreeUnordered<CLIQUE>::sharedClique& parent) {
// Helper function to build a non-symbolic tree (e.g. Gaussian) using a // // Helper function to build a non-symbolic tree (e.g. Gaussian) using a
// symbolic tree, used in the BT(BN) constructor. // // symbolic tree, used in the BT(BN) constructor.
// Build the current clique // // Build the current clique
FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals; // FastList<typename CONDITIONAL::shared_ptr> cliqueConditionals;
BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) { // BOOST_FOREACH(Index j, symbolic->conditional()->frontals()) {
cliqueConditionals.push_back(conditionals[j]); } // cliqueConditionals.push_back(conditionals[j]); }
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end()))); // typename BayesTreeUnordered<CLIQUE>::sharedClique thisClique(new CLIQUE(CONDITIONAL::Combine(cliqueConditionals.begin(), cliqueConditionals.end())));
// Add the new clique with the current parent // // Add the new clique with the current parent
this->addClique(thisClique, parent); // this->addClique(thisClique, parent);
// Build the children, whose parent is the new clique // // Build the children, whose parent is the new clique
BOOST_FOREACH(const BayesTree<IndexConditional>::sharedClique& child, symbolic->children()) { // BOOST_FOREACH(const BayesTreeUnordered<IndexConditional>::sharedClique& child, symbolic->children()) {
this->recursiveTreeBuild(child, conditionals, thisClique); } // this->recursiveTreeBuild(child, conditionals, thisClique); }
} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> //template<class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) { //BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const BayesNet<CONDITIONAL>& bayesNet) {
// First generate symbolic BT to determine clique structure // // First generate symbolic BT to determine clique structure
BayesTree<IndexConditional> sbt(bayesNet); // BayesTreeUnordered<IndexConditional> sbt(bayesNet);
// Build index of variables to conditionals // // Build index of variables to conditionals
std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1); // std::vector<boost::shared_ptr<CONDITIONAL> > conditionals(sbt.root()->conditional()->frontals().back() + 1);
BOOST_FOREACH(const boost::shared_ptr<CONDITIONAL>& c, bayesNet) { // BOOST_FOREACH(const boost::shared_ptr<CONDITIONAL>& c, bayesNet) {
if(c->nrFrontals() != 1) // if(c->nrFrontals() != 1)
throw std::invalid_argument("BayesTree constructor from BayesNet only supports single frontal variable conditionals"); // throw std::invalid_argument("BayesTreeUnordered constructor from BayesNet only supports single frontal variable conditionals");
if(c->firstFrontalKey() >= conditionals.size()) // if(c->firstFrontalKey() >= conditionals.size())
throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTree constructor!"); // throw std::invalid_argument("An inconsistent BayesNet was passed into the BayesTreeUnordered constructor!");
if(conditionals[c->firstFrontalKey()]) // if(conditionals[c->firstFrontalKey()])
throw std::invalid_argument("An inconsistent BayesNet with duplicate frontal variables was passed into the BayesTree constructor!"); // 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 // // Build the new tree
this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique()); // this->recursiveTreeBuild(sbt.root(), conditionals, sharedClique());
} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<> //template<>
inline BayesTree<IndexConditional>::BayesTree(const BayesNet<IndexConditional>& bayesNet) { //inline BayesTreeUnordered<IndexConditional>::BayesTreeUnordered(const BayesNet<IndexConditional>& bayesNet) {
BayesNet<IndexConditional>::const_reverse_iterator rit; // BayesNet<IndexConditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) // for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*this, *rit); // insert(*this, *rit);
} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> //template<class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees) { //BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTreeUnordered<CLIQUE> > subtrees) {
if (bayesNet.size() == 0) // if (bayesNet.size() == 0)
throw std::invalid_argument("BayesTree::insert: empty bayes net!"); // throw std::invalid_argument("BayesTreeUnordered::insert: empty bayes net!");
// get the roots of child subtrees and merge their nodes_ // // get the roots of child subtrees and merge their nodes_
std::list<sharedClique> childRoots; // std::list<sharedClique> childRoots;
typedef BayesTree<CONDITIONAL,CLIQUE> Tree; // typedef BayesTreeUnordered<CLIQUE> Tree;
BOOST_FOREACH(const Tree& subtree, subtrees) { // BOOST_FOREACH(const Tree& subtree, subtrees) {
nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end()); // nodes_.assign(subtree.nodes_.begin(), subtree.nodes_.end());
childRoots.push_back(subtree.root()); // childRoots.push_back(subtree.root());
} // }
// create a new clique and add all the conditionals to the clique // // create a new clique and add all the conditionals to the clique
sharedClique new_clique; // sharedClique new_clique;
typename BayesNet<CONDITIONAL>::sharedConditional conditional; // typename BayesNet<CONDITIONAL>::sharedConditional conditional;
BOOST_REVERSE_FOREACH(conditional, bayesNet) { // BOOST_REVERSE_FOREACH(conditional, bayesNet) {
if (!new_clique.get()) // if (!new_clique.get())
new_clique = addClique(conditional,childRoots); // new_clique = addClique(conditional,childRoots);
else // else
addToCliqueFront(*this, conditional, new_clique); // addToCliqueFront(*this, conditional, new_clique);
} // }
root_ = new_clique; // root_ = new_clique;
} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>::BayesTree(const This& other) { BayesTreeUnordered<CLIQUE>::BayesTreeUnordered(const This& other) {
*this = other; *this = other;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
BayesTree<CONDITIONAL,CLIQUE>& BayesTree<CONDITIONAL,CLIQUE>::operator=(const This& other) { BayesTreeUnordered<CLIQUE>& BayesTreeUnordered<CLIQUE>::operator=(const This& other) {
this->clear(); this->clear();
other.cloneTo(*this); other.cloneTo(*this);
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::print(const std::string& s, const IndexFormatter& indexFormatter) const { void BayesTreeUnordered<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
if (root_.use_count() == 0) { std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
std::cout << "WARNING: BayesTree.print encountered a forest..." << std::endl; treeTraversal::PrintForest(*this, s, keyFormatter)
return;
}
std::cout << s << ": clique size == " << size() << ", node size == " << nodes_.size() << std::endl;
if (nodes_.empty()) return;
root_->printTree("", indexFormatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// binary predicate to test equality of a pair for use in equals // binary predicate to test equality of a pair for use in equals
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
bool check_sharedCliques( bool check_sharedCliques(
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v1, const typename BayesTreeUnordered<CLIQUE>::sharedClique& v1,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& v2 const typename BayesTreeUnordered<CLIQUE>::sharedClique& v2
) { ) {
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2)); return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
bool BayesTree<CONDITIONAL,CLIQUE>::equals(const BayesTree<CONDITIONAL,CLIQUE>& other, bool BayesTreeUnordered<CLIQUE>::equals(const BayesTreeUnordered<CLIQUE>& other, double tol) const {
double tol) const {
return size()==other.size() && return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>); std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
template<class CONTAINER> 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()); typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end()); assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent; return *lowestOrderedParent;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional) void BayesTreeUnordered<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
{
static const bool debug = false;
// get indices and parents
const typename CONDITIONAL::Parents& parents = conditional->parents();
if(debug) conditional->print("Adding conditional ");
// if no parents, start a new root clique
if (parents.empty()) {
if(debug) std::cout << "No parents so making root" << std::endl;
bayesTree.root_ = bayesTree.addClique(conditional);
return;
}
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Index parentRepresentative = bayesTree.findParentClique(parents);
if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
sharedClique parent_clique = bayesTree[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");
// if the parents and parent clique have the same size, add to parent clique
if ((*parent_clique)->size() == size_t(parents.size())) {
if(debug) std::cout << "Adding to parent clique" << std::endl;
#ifndef NDEBUG
// Debug check that the parent indices of the new conditional match the indices
// currently in the clique.
// list<Index>::const_iterator parent = parents.begin();
// typename Clique::const_iterator cond = parent_clique->begin();
// while(parent != parents.end()) {
// assert(cond != parent_clique->end());
// assert(*parent == (*cond)->key());
// ++ parent;
// ++ cond;
// }
#endif
addToCliqueFront(bayesTree, conditional, parent_clique);
} else {
if(debug) std::cout << "Starting new clique" << std::endl;
// otherwise, start a new clique and add it to the tree
bayesTree.addClique(conditional,parent_clique);
}
}
/* ************************************************************************* */
//TODO: remove this function after removing TSAM.cpp
template<class CONDITIONAL, class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique BayesTree<CONDITIONAL,CLIQUE>::insert(
const sharedConditional& clique, std::list<sharedClique>& children, bool isRootClique) {
if (clique->nrFrontals() == 0)
throw std::invalid_argument("BayesTree::insert: empty clique!");
// create a new clique and add all the conditionals to the clique
sharedClique new_clique = addClique(clique, children);
if (isRootClique) root_ = new_clique;
return new_clique;
}
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node // 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 // 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_) { BOOST_FOREACH(const sharedClique& child, subtree->children_) {
fillNodesIndex(child); } fillNodesIndex(child); }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::insert(const sharedClique& subtree) { void BayesTreeUnordered<CLIQUE>::insert(const sharedClique& subtree) {
if(subtree) { if(subtree) {
// Find the parent clique of the new subtree. By the running intersection // Find the parent clique of the new subtree. By the running intersection
// property, those separator variables in the subtree that are ordered // property, those separator variables in the subtree that are ordered
@ -490,17 +364,13 @@ namespace gtsam {
assert(!root_); assert(!root_);
root_ = subtree; root_ = subtree;
} else { } else {
Index parentRepresentative = findParentClique(subtree->conditional()->parents()); Key parentRepresentative = findParentClique(subtree->conditional()->parents());
sharedClique parent = (*this)[parentRepresentative]; sharedClique parent = (*this)[parentRepresentative];
parent->children_ += subtree; parent->children_ += subtree;
subtree->parent_ = parent; // set new parent! subtree->parent_ = parent; // set new parent!
} }
// Now fill in the nodes index // 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); fillNodesIndex(subtree);
} }
} }
@ -508,209 +378,211 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// First finds clique marginal then marginalizes that // First finds clique marginal then marginalizes that
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename CONDITIONAL::FactorType::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalFactor( typename BayesTreeUnordered<CLIQUE>::sharedFactor BayesTreeUnordered<CLIQUE>::marginalFactor(
Index j, Eliminate function) const Key j, Eliminate function) const
{ {
gttic(BayesTree_marginalFactor); return boost::make_shared<FactorType>();
// gttic(BayesTree_marginalFactor);
// get clique containing Index j //
sharedClique clique = (*this)[j]; // // get clique containing Index j
// sharedClique clique = this->clique(j);
// calculate or retrieve its marginal P(C) = P(F,S) //
#ifdef OLD_SHORTCUT_MARGINALS // // calculate or retrieve its marginal P(C) = P(F,S)
FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function); //#ifdef OLD_SHORTCUT_MARGINALS
#else // FactorGraph<FactorType> cliqueMarginal = clique->marginal(root_,function);
FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function); //#else
#endif // FactorGraph<FactorType> cliqueMarginal = clique->marginal2(root_,function);
//#endif
// Reduce the variable indices to start at zero //
gttic(Reduce); // // Reduce the variable indices to start at zero
const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys()); // gttic(Reduce);
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); // const Permutation reduction = internal::createReducingPermutation(cliqueMarginal.keys());
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) { // internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
if(factor) factor->reduceWithInverse(inverseReduction); } // BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
gttoc(Reduce); // if(factor) factor->reduceWithInverse(inverseReduction); }
// gttoc(Reduce);
// now, marginalize out everything that is not variable j //
GenericSequentialSolver<FactorType> solver(cliqueMarginal); // // now, marginalize out everything that is not variable j
boost::shared_ptr<FactorType> result = solver.marginalFactor(inverseReduction[j], function); // GenericSequentialSolver<FactorType> solver(cliqueMarginal);
// boost::shared_ptr<FactorType> result = solver.marginalFactor(inverseReduction[j], function);
// Undo the reduction //
gttic(Undo_Reduce); // // Undo the reduction
result->permuteWithInverse(reduction); // gttic(Undo_Reduce);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) { // result->permuteWithInverse(reduction);
if(factor) factor->permuteWithInverse(reduction); } // BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, cliqueMarginal) {
gttoc(Undo_Reduce); // if(factor) factor->permuteWithInverse(reduction); }
return result; // gttoc(Undo_Reduce);
// return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::marginalBayesNet( typename BayesTreeUnordered<CLIQUE>::sharedBayesNet BayesTreeUnordered<CLIQUE>::marginalBayesNet(
Index j, Eliminate function) const Key j, Eliminate function) const
{ {
gttic(BayesTree_marginalBayesNet); return boost::make_shared<BayesNetType>();
//gttic(BayesTree_marginalBayesNet);
// calculate marginal as a factor graph //// calculate marginal as a factor graph
FactorGraph<FactorType> fg; //FactorGraph<FactorType> fg;
fg.push_back(this->marginalFactor(j,function)); //fg.push_back(this->marginalFactor(j,function));
// Reduce the variable indices to start at zero //// Reduce the variable indices to start at zero
gttic(Reduce); //gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(fg.keys()); //const Permutation reduction = internal::createReducingPermutation(fg.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) { //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
if(factor) factor->reduceWithInverse(inverseReduction); } // if(factor) factor->reduceWithInverse(inverseReduction); }
gttoc(Reduce); //gttoc(Reduce);
// eliminate factor graph marginal to a Bayes net //// eliminate factor graph marginal to a Bayes net
boost::shared_ptr<BayesNet<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function); //boost::shared_ptr<BayesNet<CONDITIONAL> > bn = GenericSequentialSolver<FactorType>(fg).eliminate(function);
// Undo the reduction //// Undo the reduction
gttic(Undo_Reduce); //gttic(Undo_Reduce);
bn->permuteWithInverse(reduction); //bn->permuteWithInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) { //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, fg) {
if(factor) factor->permuteWithInverse(reduction); } // if(factor) factor->permuteWithInverse(reduction); }
gttoc(Undo_Reduce); //gttoc(Undo_Reduce);
return bn; //return bn;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Find two cliques, their joint, then marginalizes // Find two cliques, their joint, then marginalizes
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename FactorGraph<typename CONDITIONAL::FactorType>::shared_ptr typename BayesTreeUnordered<CLIQUE>::sharedFactorGraph
BayesTree<CONDITIONAL,CLIQUE>::joint(Index j1, Index j2, Eliminate function) const { BayesTreeUnordered<CLIQUE>::joint(Key j1, Key j2, Eliminate function) const {
gttic(BayesTree_joint); return boost::make_shared<FactorGraphType>();
//gttic(BayesTree_joint);
// get clique C1 and C2 //// get clique C1 and C2
sharedClique C1 = (*this)[j1], C2 = (*this)[j2]; //sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
gttic(Lowest_common_ancestor); //gttic(Lowest_common_ancestor);
// Find lowest common ancestor clique //// Find lowest common ancestor clique
sharedClique B; { //sharedClique B; {
// Build two paths to the root // // Build two paths to the root
FastList<sharedClique> path1, path2; { // FastList<sharedClique> path1, path2; {
sharedClique p = C1; // sharedClique p = C1;
while(p) { // while(p) {
path1.push_front(p); // path1.push_front(p);
p = p->parent(); // p = p->parent();
} // }
} { // } {
sharedClique p = C2; // sharedClique p = C2;
while(p) { // while(p) {
path2.push_front(p); // path2.push_front(p);
p = p->parent(); // p = p->parent();
} // }
} // }
// Find the path intersection // // Find the path intersection
B = this->root(); // B = this->root();
typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin(); // typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { // while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
B = *p1; // B = *p1;
++p1; // ++p1;
++p2; // ++p2;
} // }
} //}
gttoc(Lowest_common_ancestor); //gttoc(Lowest_common_ancestor);
// Compute marginal on lowest common ancestor clique //// Compute marginal on lowest common ancestor clique
gttic(LCA_marginal); //gttic(LCA_marginal);
FactorGraph<FactorType> p_B = B->marginal2(this->root(), function); //FactorGraph<FactorType> p_B = B->marginal2(this->root(), function);
gttoc(LCA_marginal); //gttoc(LCA_marginal);
// Compute shortcuts of the requested cliques given the lowest common ancestor //// Compute shortcuts of the requested cliques given the lowest common ancestor
gttic(Clique_shortcuts); //gttic(Clique_shortcuts);
BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function); //BayesNet<CONDITIONAL> p_C1_Bred = C1->shortcut(B, function);
BayesNet<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function); //BayesNet<CONDITIONAL> p_C2_Bred = C2->shortcut(B, function);
gttoc(Clique_shortcuts); //gttoc(Clique_shortcuts);
// Factor the shortcuts to be conditioned on the full root //// Factor the shortcuts to be conditioned on the full root
// Get the set of variables to eliminate, which is C1\B. //// Get the set of variables to eliminate, which is C1\B.
gttic(Full_root_factoring); //gttic(Full_root_factoring);
sharedConditional p_C1_B; { //sharedConditional p_C1_B; {
std::vector<Index> C1_minus_B; { // std::vector<Index> C1_minus_B; {
FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); // FastSet<Index> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) { // BOOST_FOREACH(const Index j, *B->conditional()) {
C1_minus_B_set.erase(j); } // C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); // C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
} // }
// Factor into C1\B | B. // // Factor into C1\B | B.
FactorGraph<FactorType> temp_remaining; // FactorGraph<FactorType> temp_remaining;
boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function); // boost::tie(p_C1_B, temp_remaining) = FactorGraph<FactorType>(p_C1_Bred).eliminate(C1_minus_B, function);
} //}
sharedConditional p_C2_B; { //sharedConditional p_C2_B; {
std::vector<Index> C2_minus_B; { // std::vector<Index> C2_minus_B; {
FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); // FastSet<Index> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Index j, *B->conditional()) { // BOOST_FOREACH(const Index j, *B->conditional()) {
C2_minus_B_set.erase(j); } // C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); // C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
} // }
// Factor into C2\B | B. // // Factor into C2\B | B.
FactorGraph<FactorType> temp_remaining; // FactorGraph<FactorType> temp_remaining;
boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function); // boost::tie(p_C2_B, temp_remaining) = FactorGraph<FactorType>(p_C2_Bred).eliminate(C2_minus_B, function);
} //}
gttoc(Full_root_factoring); //gttoc(Full_root_factoring);
gttic(Variable_joint); //gttic(Variable_joint);
// Build joint on all involved variables //// Build joint on all involved variables
FactorGraph<FactorType> p_BC1C2; //FactorGraph<FactorType> p_BC1C2;
p_BC1C2.push_back(p_B); //p_BC1C2.push_back(p_B);
p_BC1C2.push_back(p_C1_B->toFactor()); //p_BC1C2.push_back(p_C1_B->toFactor());
p_BC1C2.push_back(p_C2_B->toFactor()); //p_BC1C2.push_back(p_C2_B->toFactor());
if(C1 != B) //if(C1 != B)
p_BC1C2.push_back(C1->conditional()->toFactor()); // p_BC1C2.push_back(C1->conditional()->toFactor());
if(C2 != B) //if(C2 != B)
p_BC1C2.push_back(C2->conditional()->toFactor()); // p_BC1C2.push_back(C2->conditional()->toFactor());
// Reduce the variable indices to start at zero //// Reduce the variable indices to start at zero
gttic(Reduce); //gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys()); //const Permutation reduction = internal::createReducingPermutation(p_BC1C2.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction); //internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) { //BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_BC1C2) {
if(factor) factor->reduceWithInverse(inverseReduction); } // if(factor) factor->reduceWithInverse(inverseReduction); }
std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]); //std::vector<Index> js; js.push_back(inverseReduction[j1]); js.push_back(inverseReduction[j2]);
gttoc(Reduce); //gttoc(Reduce);
// now, marginalize out everything that is not variable j //// now, marginalize out everything that is not variable j
GenericSequentialSolver<FactorType> solver(p_BC1C2); //GenericSequentialSolver<FactorType> solver(p_BC1C2);
boost::shared_ptr<FactorGraph<FactorType> > result = solver.jointFactorGraph(js, function); //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;
//// 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> template<class CLIQUE>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL,CLIQUE>::jointBayesNet( typename BayesTreeUnordered<CLIQUE>::sharedBayesNet BayesTreeUnordered<CLIQUE>::jointBayesNet(
Index j1, Index j2, Eliminate function) const { Key j1, Key j2, Eliminate function) const
{
// eliminate factor graph marginal to a Bayes net return boost::make_shared<BayesNetType>();
return GenericSequentialSolver<FactorType> ( //// eliminate factor graph marginal to a Bayes net
*this->joint(j1, j2, function)).eliminate(function); //return GenericSequentialSolver<FactorType> (
// *this->joint(j1, j2, function)).eliminate(function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::clear() { void BayesTreeUnordered<CLIQUE>::clear() {
// Remove all nodes and clear the root pointer // Remove all nodes and clear the root pointer
nodes_.clear(); nodes_.clear();
root_.reset(); root_.reset();
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::removePath(sharedClique clique, void BayesTreeUnordered<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) { {
// base case is NULL, if so we do nothing and return empties above // base case is NULL, if so we do nothing and return empties above
if (clique!=NULL) { if (clique!=NULL) {
@ -732,13 +604,12 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
template<class CONTAINER> template<class CONTAINER>
void BayesTree<CONDITIONAL,CLIQUE>::removeTop(const CONTAINER& keys, void BayesTreeUnordered<CLIQUE>::removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans)
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL,CLIQUE>::Cliques& orphans) { {
// process each key of the new factor // process each key of the new factor
BOOST_FOREACH(const Index& j, keys) { BOOST_FOREACH(const Key& j, keys) {
// get the clique // get the clique
if(j < nodes_.size()) { if(j < nodes_.size()) {
@ -757,8 +628,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE> template<class CLIQUE>
typename BayesTree<CONDITIONAL,CLIQUE>::Cliques BayesTree<CONDITIONAL,CLIQUE>::removeSubtree( typename BayesTreeUnordered<CLIQUE>::Cliques BayesTreeUnordered<CLIQUE>::removeSubtree(
const sharedClique& subtree) const sharedClique& subtree)
{ {
// Result clique list // Result clique list
@ -782,8 +653,8 @@ namespace gtsam {
(*clique)->deleteCachedShortcutsNonRecursive(); (*clique)->deleteCachedShortcutsNonRecursive();
// Remove this node from the nodes index // Remove this node from the nodes index
BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) { BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) {
nodes_[j].reset(); } nodes_.erase(j); }
// Erase the parent and children pointers // Erase the parent and children pointers
(*clique)->parent_.reset(); (*clique)->parent_.reset();
@ -793,23 +664,5 @@ namespace gtsam {
return cliques; 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 /// namespace gtsam

229
gtsam/inference/BayesTreeUnordered.h
View File

@ -20,26 +20,14 @@
#pragma once #pragma once
#include <vector> #include <vector>
#include <stdexcept> #include <string>
#include <deque>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/base/FastMap.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam { namespace gtsam {
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
template<class CONDITIONAL> struct BayesTreeClique;
/** /**
* Bayes tree * Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, * @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 * \addtogroup Multifrontal
* \nosubgrouping * \nosubgrouping
*/ */
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> > template<class CLIQUE>
class BayesTree { class BayesTreeUnordered {
public: public:
typedef BayesTree<CONDITIONAL, CLIQUE> This; typedef BayesTreeUnordered<CLIQUE> This;
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
typedef CONDITIONAL ConditionalType;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique typedef 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 /** A convenience class for a list of shared cliques */
typedef boost::shared_ptr<Clique> sharedClique;
// A convenience class for a list of shared cliques
struct Cliques : public FastList<sharedClique> { struct Cliques : public FastList<sharedClique> {
void print(const std::string& s = "Cliques", 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; bool equals(const Cliques& other, double tol = 1e-9) const;
}; };
@ -91,16 +80,16 @@ namespace gtsam {
CliqueStats getStats() const; CliqueStats getStats() const;
}; };
/** Map from indices to Clique */ /** Map from keys to Clique */
typedef std::vector<sharedClique> Nodes; typedef FastMap<Key, sharedClique> Nodes;
protected: protected:
/** Map from indices to Clique */ /** Map from indices to Clique */
Nodes nodes_; Nodes nodes_;
/** Root clique */ /** Root cliques */
sharedClique root_; std::vector<sharedClique> roots_;
public: public:
@ -108,13 +97,10 @@ namespace gtsam {
/// @{ /// @{
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
BayesTree() {} BayesTreeUnordered() {}
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
explicit BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
/** Copy constructor */ /** Copy constructor */
BayesTree(const This& other); BayesTreeUnordered(const This& other);
/** Assignment operator */ /** Assignment operator */
This& operator=(const This& other); This& operator=(const This& other);
@ -123,37 +109,24 @@ namespace gtsam {
/// @name Advanced Constructors /// @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 */ /** Destructor */
virtual ~BayesTree() {} virtual ~BayesTreeUnordered() {}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{
/** check equality */ /** 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 */ /** print */
void print(const std::string& s = "", void print(const std::string& s = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @} /// @}
/// @name Standard Interface /// @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 */ /** number of cliques */
inline size_t size() const { inline size_t size() const {
if(root_) if(root_)
@ -170,17 +143,16 @@ namespace gtsam {
/** return nodes */ /** return nodes */
const Nodes& nodes() const { return nodes_; } const Nodes& nodes() const { return nodes_; }
/** return root clique */ /** return root cliques */
const sharedClique& root() const { return root_; } const std::vector<sharedClique>& roots() const { return roots_; }
/** find the clique that contains the variable with Index j */
inline sharedClique operator[](Index j) const {
return nodes_.at(j);
}
/** alternate syntax for matlab: find the clique that contains the variable with Index j */ /** alternate syntax for matlab: find the clique that contains the variable with Index j */
inline sharedClique clique(Index j) const { const sharedClique& clique(Key j) const {
return nodes_.at(j); 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 */ /** Gather data on all cliques */
@ -190,43 +162,44 @@ namespace gtsam {
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
/** return marginal on any variable */ /** 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 * return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net * NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function * 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 * return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root * 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 * return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root * 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 * Read only with side effects
*/ */
/** saves the Tree to a text file in GraphViz format */ /** 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 /// @name Advanced Interface
/// @{ /// @{
/** Access the root clique (non-const version) */ /**
sharedClique& root() { return root_; } * Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
/** Access the nodes (non-cost version) */ */
Nodes& nodes() { return nodes_; } template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** Remove all nodes */ /** Remove all nodes */
void clear(); void clear();
@ -236,21 +209,18 @@ namespace gtsam {
root_->deleteCachedShortcuts(); root_->deleteCachedShortcuts();
} }
/** Apply a permutation to the full tree - also updates the nodes structure */
void permuteWithInverse(const Permutation& inversePermutation);
/** /**
* Remove path from clique to root and return that path as factors * Remove path from clique to root and return that path as factors
* plus a list of orphaned subtree roots. Used in removeTop below. * 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. * 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. * Factors and orphans are added to the in/out arguments.
*/ */
template<class CONTAINER> 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. */ * Remove the requested subtree. */
@ -265,20 +235,6 @@ namespace gtsam {
*/ */
void insert(const sharedClique& subtree); 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 * Create a clone of this object as a shared pointer
* Necessary for inheritance in matlab interface * Necessary for inheritance in matlab interface
@ -290,7 +246,7 @@ namespace gtsam {
protected: protected:
/** private helper method for saving the Tree to a text file in GraphViz format */ /** 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; int parentnum = 0) const;
/** Gather data on a single clique */ /** Gather data on a single clique */
@ -308,32 +264,17 @@ namespace gtsam {
/** add a clique (bottom up) */ /** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques); 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 */ /** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree); void fillNodesIndex(const sharedClique& subtree);
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a /** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
* symbolic tree, used in the BT(BN) constructor. * symbolic tree, used in the BT(BN) constructor.
*/ */
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic, //void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals, // const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent); // const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent);
private: 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 */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -346,70 +287,4 @@ namespace gtsam {
}; // BayesTree }; // 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 } /// 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 { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE> template<class FACTOR, class FACTORGRAPH, class CONDITIONAL,
boost::shared_ptr<typename ELIMINATIONTREE::BayesNetType> class BAYESNET, class ELIMINATIONTREE, class BAYESTREE, class JUNCTIONTREE>
EliminateableFactorGraph<DERIVED,ELIMINATIONTREE,JUNCTIONTREE>::eliminateSequential( boost::shared_ptr<BAYESNET>
EliminateableFactorGraph<FACTOR, FACTORGRAPH, CONDITIONAL, BAYESNET, ELIMINATIONTREE, BAYESTREE, JUNCTIONTREE>::
eliminateSequential(
const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const
{ {
// Do elimination // 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) { if(ordering) {
// Do elimination with given ordering // Do elimination with given ordering
result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function); result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function);
@ -49,13 +51,15 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE> template<class FACTOR, class FACTORGRAPH, class CONDITIONAL,
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType> class BAYESNET, class ELIMINATIONTREE, class BAYESTREE, class JUNCTIONTREE>
EliminateableFactorGraph<DERIVED,ELIMINATIONTREE,JUNCTIONTREE>::eliminateMultifrontal( boost::shared_ptr<BAYESTREE>
EliminateableFactorGraph<FACTOR, FACTORGRAPH, CONDITIONAL, BAYESNET, ELIMINATIONTREE, BAYESTREE, JUNCTIONTREE>::
eliminateMultifrontal(
const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const
{ {
// Do elimination // 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) { if(ordering) {
// Do elimination with given ordering // Do elimination with given ordering
result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function); 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 * algorithms. Any factor graph holding eliminateable factors can derive from this class to
* expose functions for computing marginals, conditional marginals, doing multifrontal and * expose functions for computing marginals, conditional marginals, doing multifrontal and
* sequential elimination, etc. */ * 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 { class EliminateableFactorGraph {
public: public:
typedef EliminateableFactorGraph<DERIVED, ELIMINATIONTREE, JUNCTIONTREE> This; typedef EliminateableFactorGraph<FACTOR, FACTORGRAPH, CONDITIONAL, BAYESNET, ELIMINATIONTREE, BAYESTREE, 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 boost::optional<const OrderingUnordered&> OptionalOrdering; 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 /** 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. * 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); * boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, boost::none, varIndex);
* \endcode * \endcode
* */ * */
boost::shared_ptr<BayesNetType> boost::shared_ptr<BAYESNET>
eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none, eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const;
@ -92,7 +85,7 @@ namespace gtsam {
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex); * boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex);
* \endcode * \endcode
* */ * */
boost::shared_ptr<BayesTreeType> boost::shared_ptr<BAYESTREE>
eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none, eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const; 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$, * 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$ * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$
* B = X\backslash A \f$. */ * B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialSequential(const Eliminate& function, const Ordering& ordering, eliminatePartialSequential(const Eliminate& function, const OrderingUnordered& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to /** 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) * 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 * = 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$. */ * factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialSequential(const Eliminate& function, const std::vector& variables, eliminatePartialSequential(const Eliminate& function, const std::vector<Key>& variables,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to /** 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) * 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 * = 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$. */ * factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialMultifrontal(const Eliminate& function, const Ordering& ordering, eliminatePartialMultifrontal(const Eliminate& function, const OrderingUnordered& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes /** 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) * 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$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and
* \f$ B = X\backslash A \f$. */ * \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> >
eliminatePartialMultifrontal(const Eliminate& function, const std::vector& ordering, eliminatePartialMultifrontal(const Eliminate& function, const std::vector<Key>& variables,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)); const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
}; };

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

@ -59,6 +59,20 @@ namespace gtsam {
return eliminationResult.second; 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> template<class BAYESNET, class GRAPH>
@ -186,26 +200,11 @@ namespace gtsam {
return std::make_pair(result, allRemainingFactors); 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> template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const 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 <boost/shared_ptr.hpp>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
@ -59,9 +58,7 @@ namespace gtsam {
typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef boost::function<std::pair<sharedConditional,sharedFactor>( typedef typename GRAPH::Eliminate Eliminate;
std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
struct Node { struct Node {
typedef std::vector<sharedFactor> Factors; typedef std::vector<sharedFactor> Factors;
@ -73,6 +70,8 @@ namespace gtsam {
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output, sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const; 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 typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node

20
gtsam/inference/FactorGraphUnordered.h
View File

@ -22,6 +22,8 @@
#pragma once #pragma once
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -38,6 +40,7 @@ namespace gtsam {
public: public:
typedef FactorGraphUnordered<FACTOR> This;
typedef FACTOR FactorType; ///< factor type typedef FACTOR FactorType; ///< factor type
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional; ///< Shared pointer to a conditional
@ -103,12 +106,18 @@ namespace gtsam {
// TODO: are these needed? // TODO: are these needed?
/** Add a factor */ /** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) { void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(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 */ /** push back many factors */
void push_back(const This& factors) { void push_back(const This& factors) {
factors_.insert(end(), factors.begin(), factors.end()); factors_.insert(end(), factors.begin(), factors.end());
@ -120,6 +129,15 @@ namespace gtsam {
factors_.insert(end(), firstFactor, lastFactor); 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 * @brief Add a vector of derived factors
* @param factors to add * @param factors to add

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

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

8
gtsam/inference/Key.cpp
View File

@ -27,14 +27,6 @@
namespace gtsam { 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) { std::string _multirobotKeyFormatter(gtsam::Key key) {

13
gtsam/inference/Key.h
View File

@ -27,19 +27,6 @@
namespace gtsam { 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 // Helper function for Multi-robot Key Formatter
GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);

6
gtsam/inference/OrderingUnordered.h
View File

@ -22,6 +22,8 @@
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
class OrderingUnordered : class OrderingUnordered : std::vector<Key> {
typedef std::vector<Key> OrderingUnordered; 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 #pragma once
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h> #include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h> #include <gtsam/symbolic/SymbolicConditionalUnordered.h>
namespace gtsam { namespace gtsam {
@ -27,12 +27,14 @@ namespace gtsam {
/** Symbolic Bayes Net /** Symbolic Bayes Net
* \nosubgrouping * \nosubgrouping
*/ */
class SymbolicBayesNetUnordered: public SymbolicFactorGraphUnordered { class SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
public: public:
typedef SymbolicFactorGraphUnordered Base; typedef BayesNetUnordered<SymbolicConditionalUnordered> Base;
typedef SymbolicBayesNetUnordered This;
typedef SymbolicConditionalUnordered ConditionalType; typedef SymbolicConditionalUnordered ConditionalType;
typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -43,6 +45,11 @@ namespace gtsam {
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/// @}
private:
void noop() const; // Function defined in cpp file so that compiler instantiates the base class
}; };
} // namespace gtsam } // 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 <boost/make_shared.hpp>
#include <gtsam/inference/FactorGraphUnordered-inst.h> #include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h> #include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
#include <gtsam/symbolic/SymbolicJunctionTreeUnordered.h>
namespace gtsam { namespace gtsam {

23
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

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