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

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

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

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

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

11
gtsam/base/types.cpp
View File

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

16
gtsam/base/types.h
View File

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

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

@ -0,0 +1,53 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesNet.h
* @brief Bayes network
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const
{
Base::print(s, formatter);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::saveGraph(
const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter)
{
std::ofstream of(s.c_str());
of << "digraph G{\n";
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Index me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
BOOST_FOREACH(Index p, parents)
of << p << "->" << me << std::endl;
}
of << "}";
of.close();
}
}

35
gtsam/inference/BayesNetUnordered.h
View File

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

14
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

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

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

@ -0,0 +1,60 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file BayesTreeCliqueDefault.h
* @brief A standard BayesTree clique. iSAM2 uses a specialized clique.
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
namespace gtsam {
/* ************************************************************************* */
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*
* This is the default clique type in a BayesTree, but some algorithms, like
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
* extra data along with the clique.
*/
template<class FACTORGRAPH, class BAYESNET>
struct BayesTreeCliqueDefaultUnordered :
public BayesTreeCliqueBaseUnordered<BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET>, FACTORGRAPH, BAYESNET> {
public:
typedef typename BAYESNET::ConditionalType ConditionalType;
typedef BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET> This;
typedef BayesTreeCliqueBaseUnordered<This, FACTORGRAPH, BAYESNET> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeCliqueDefaultUnordered() {}
BayesTreeCliqueDefaultUnordered(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeCliqueDefaultUnordered(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
}

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

File diff suppressed because it is too large Load Diff

231
gtsam/inference/BayesTreeUnordered.h
View File

@ -20,26 +20,14 @@
#pragma once
#include <vector>
#include <stdexcept>
#include <deque>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
#include <string>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/FastMap.h>
namespace gtsam {
// Forward declaration of BayesTreeClique which is defined below BayesTree in this file
template<class CONDITIONAL> struct BayesTreeClique;
/**
* Bayes tree
* @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain,
@ -50,28 +38,29 @@ namespace gtsam {
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTree {
template<class CLIQUE>
class BayesTreeUnordered {
public:
typedef BayesTree<CONDITIONAL, CLIQUE> This;
typedef boost::shared_ptr<BayesTree<CONDITIONAL, CLIQUE> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
typedef CONDITIONAL ConditionalType;
typedef typename CONDITIONAL::FactorType FactorType;
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
typedef BayesTreeUnordered<CLIQUE> This;
typedef boost::shared_ptr<This> shared_ptr;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
typedef typename CLIQUE::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType;
typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
typedef typename CLIQUE::FactorType FactorType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef typename CLIQUE::FactorGraphType FactorGraphType;
typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
typedef typename FactorGraphType::Eliminate Eliminate;
// typedef for shared pointers to cliques
typedef boost::shared_ptr<Clique> sharedClique;
// A convenience class for a list of shared cliques
/** A convenience class for a list of shared cliques */
struct Cliques : public FastList<sharedClique> {
void print(const std::string& s = "Cliques",
const IndexFormatter& indexFormatter = DefaultIndexFormatter) const;
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
bool equals(const Cliques& other, double tol = 1e-9) const;
};
@ -91,16 +80,16 @@ namespace gtsam {
CliqueStats getStats() const;
};
/** Map from indices to Clique */
typedef std::vector<sharedClique> Nodes;
/** Map from keys to Clique */
typedef FastMap<Key, sharedClique> Nodes;
protected:
/** Map from indices to Clique */
Nodes nodes_;
/** Root clique */
sharedClique root_;
/** Root cliques */
std::vector<sharedClique> roots_;
public:
@ -108,13 +97,10 @@ namespace gtsam {
/// @{
/** Create an empty Bayes Tree */
BayesTree() {}
/** Create a Bayes Tree from a Bayes Net (requires CONDITIONAL is IndexConditional *or* CONDITIONAL::Combine) */
explicit BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
BayesTreeUnordered() {}
/** Copy constructor */
BayesTree(const This& other);
BayesTreeUnordered(const This& other);
/** Assignment operator */
This& operator=(const This& other);
@ -123,37 +109,24 @@ namespace gtsam {
/// @name Advanced Constructors
/// @{
/**
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
* new root clique and the subtrees are connected to the root clique.
*/
BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL,CLIQUE> > subtrees);
/** Destructor */
virtual ~BayesTree() {}
virtual ~BayesTreeUnordered() {}
/// @}
/// @name Testable
/// @{
/** check equality */
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
bool equals(const This& other, double tol = 1e-9) const;
/** print */
void print(const std::string& s = "",
const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
*/
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** number of cliques */
inline size_t size() const {
if(root_)
@ -170,17 +143,16 @@ namespace gtsam {
/** return nodes */
const Nodes& nodes() const { return nodes_; }
/** return root clique */
const sharedClique& root() const { return root_; }
/** find the clique that contains the variable with Index j */
inline sharedClique operator[](Index j) const {
return nodes_.at(j);
}
/** return root cliques */
const std::vector<sharedClique>& roots() const { return roots_; }
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
inline sharedClique clique(Index j) const {
return nodes_.at(j);
const sharedClique& clique(Key j) const {
Nodes::const_iterator c = nodes_.find(j);
if(c == nodes_.end())
throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
else
return *c;
}
/** Gather data on all cliques */
@ -190,43 +162,44 @@ namespace gtsam {
size_t numCachedSeparatorMarginals() const;
/** return marginal on any variable */
typename FactorType::shared_ptr marginalFactor(Index j, Eliminate function) const;
sharedFactor marginalFactor(Key j, Eliminate function) const;
/**
* return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function
*/
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index j, Eliminate function) const;
sharedBayesNet marginalBayesNet(Key j, Eliminate function) const;
/**
* return joint on two variables
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
typename FactorGraph<FactorType>::shared_ptr joint(Index j1, Index j2, Eliminate function) const;
sharedFactorGraph joint(Index j1, Index j2, Eliminate function) const;
/**
* return joint on two variables as a BayesNet
* Limitation: can only calculate joint if cliques are disjoint or one of them is root
*/
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index j1, Index j2, Eliminate function) const;
sharedBayesNet jointBayesNet(Index j1, Index j2, Eliminate function) const;
/**
* Read only with side effects
*/
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s, const IndexFormatter& indexFormatter = DefaultIndexFormatter ) const;
void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Advanced Interface
/// @{
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** Access the nodes (non-cost version) */
Nodes& nodes() { return nodes_; }
/**
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
*/
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** Remove all nodes */
void clear();
@ -236,21 +209,18 @@ namespace gtsam {
root_->deleteCachedShortcuts();
}
/** Apply a permutation to the full tree - also updates the nodes structure */
void permuteWithInverse(const Permutation& inversePermutation);
/**
* Remove path from clique to root and return that path as factors
* plus a list of orphaned subtree roots. Used in removeTop below.
*/
void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
void removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans);
/**
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class CONTAINER>
void removeTop(const CONTAINER& indices, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
void removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans);
/**
* Remove the requested subtree. */
@ -265,20 +235,6 @@ namespace gtsam {
*/
void insert(const sharedClique& subtree);
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
/**
* Create a clone of this object as a shared pointer
* Necessary for inheritance in matlab interface
@ -290,7 +246,7 @@ namespace gtsam {
protected:
/** private helper method for saving the Tree to a text file in GraphViz format */
void saveGraph(std::ostream &s, sharedClique clique, const IndexFormatter& indexFormatter,
void saveGraph(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
int parentnum = 0) const;
/** Gather data on a single clique */
@ -308,32 +264,17 @@ namespace gtsam {
/** add a clique (bottom up) */
sharedClique addClique(const sharedConditional& conditional, std::list<sharedClique>& child_cliques);
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
static void addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& bayesTree,
const sharedConditional& conditional, const sharedClique& clique);
/** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree);
/** Helper function to build a non-symbolic tree (e.g. Gaussian) using a
* symbolic tree, used in the BT(BN) constructor.
*/
void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent);
//void recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
// const std::vector<boost::shared_ptr<CONDITIONAL> >& conditionals,
// const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& parent);
private:
/** deep copy to another tree */
void cloneTo(This& newTree) const;
/** deep copy to another tree */
void cloneTo(This& newTree, const sharedClique& subtree, const sharedClique& parent) const;
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
@ -346,70 +287,4 @@ namespace gtsam {
}; // BayesTree
/* ************************************************************************* */
template<class CONDITIONAL, class CLIQUE>
void _BayesTree_dim_adder(
std::vector<size_t>& dims,
const typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique& clique) {
if(clique) {
// Add dims from this clique
for(typename CONDITIONAL::const_iterator it = (*clique)->beginFrontals(); it != (*clique)->endFrontals(); ++it)
dims[*it] = (*clique)->dim(it);
// Traverse children
typedef typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, clique->children()) {
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dims, child);
}
}
}
/* ************************************************************************* */
template<class CONDITIONAL,class CLIQUE>
boost::shared_ptr<VectorValues> allocateVectorValues(const BayesTree<CONDITIONAL,CLIQUE>& bt) {
std::vector<size_t> dimensions(bt.nodes().size(), 0);
_BayesTree_dim_adder<CONDITIONAL,CLIQUE>(dimensions, bt.root());
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
}
/* ************************************************************************* */
/**
* A Clique in the tree is an incomplete Bayes net: the variables
* in the Bayes net are the frontal nodes, and the variables conditioned
* on are the separator. We also have pointers up and down the tree.
*
* Since our Conditional class already handles multiple frontal variables,
* this Clique contains exactly 1 conditional.
*
* This is the default clique type in a BayesTree, but some algorithms, like
* iSAM2 (see ISAM2Clique), use a different clique type in order to store
* extra data along with the clique.
*/
template<class CONDITIONAL>
struct BayesTreeClique : public BayesTreeCliqueBase<BayesTreeClique<CONDITIONAL>, CONDITIONAL> {
public:
typedef CONDITIONAL ConditionalType;
typedef BayesTreeClique<CONDITIONAL> This;
typedef BayesTreeCliqueBase<This, CONDITIONAL> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeClique() {}
BayesTreeClique(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
} /// namespace gtsam
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/BayesTreeCliqueBase-inl.h>

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

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

41
gtsam/inference/EliminateableFactorGraph.h
View File

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

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

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

7
gtsam/inference/EliminationTreeUnordered.h
View File

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

20
gtsam/inference/FactorGraphUnordered.h
View File

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

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

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

13
gtsam/inference/JunctionTreeUnordered.h
View File

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

8
gtsam/inference/Key.cpp
View File

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

13
gtsam/inference/Key.h
View File

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

6
gtsam/inference/OrderingUnordered.h
View File

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

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

@ -0,0 +1,28 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicBayesNet.cpp
* @date Oct 29, 2009
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
namespace gtsam {
/* ************************************************************************* */
void SymbolicBayesNetUnordered::noop() const {
}
}

13
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

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

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

@ -0,0 +1,98 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicBayesTree.h
* @date Oct 29, 2009
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <boost/foreach.hpp>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
namespace gtsam {
/* ************************************************************************* */
void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional)
{
static const bool debug = false;
// get indices and parents
const typename CONDITIONAL::Parents& parents = conditional->parents();
if(debug) conditional->print("Adding conditional ");
// if no parents, start a new root clique
if (parents.empty()) {
if(debug) std::cout << "No parents so making root" << std::endl;
bayesTree.root_ = bayesTree.addClique(conditional);
return;
}
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Index parentRepresentative = bayesTree.findParentClique(parents);
if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
sharedClique parent_clique = bayesTree[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");
// if the parents and parent clique have the same size, add to parent clique
if ((*parent_clique)->size() == size_t(parents.size())) {
if(debug) std::cout << "Adding to parent clique" << std::endl;
#ifndef NDEBUG
// Debug check that the parent indices of the new conditional match the indices
// currently in the clique.
// list<Index>::const_iterator parent = parents.begin();
// typename Clique::const_iterator cond = parent_clique->begin();
// while(parent != parents.end()) {
// assert(cond != parent_clique->end());
// assert(*parent == (*cond)->key());
// ++ parent;
// ++ cond;
// }
#endif
addToCliqueFront(bayesTree, conditional, parent_clique);
} else {
if(debug) std::cout << "Starting new clique" << std::endl;
// otherwise, start a new clique and add it to the tree
bayesTree.addClique(conditional,parent_clique);
}
}
/* ************************************************************************* */
void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false;
#ifndef NDEBUG
// Debug check to make sure the conditional variable is ordered lower than
// its parents and that all of its parents are present either in this
// clique or its separator.
BOOST_FOREACH(Key parent, conditional->parents()) {
assert(parent > conditional->lastFrontalKey());
const Clique& cliquer(*clique);
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
}
#endif
if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique ");
Index j = conditional->lastFrontalKey();
this->nodes_.resize(std::max(j+1, this->nodes_.size()));
this->nodes_[j] = clique;
FastVector<Index> newIndices(clique->conditional()->size() + 1);
newIndices[0] = j;
std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1);
clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
}

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

@ -0,0 +1,46 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicBayesTree.h
* @date Oct 29, 2009
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
namespace gtsam {
class SymbolicBayesTreeUnordered :
public BayesTreeUnordered<BayesTreeCliqueBaseUnordered<>
{
public:
/** Insert a new conditional */
void insert(const sharedConditional& conditional);
protected:
/**
* Add a conditional to the front of a clique, i.e. a conditional whose
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
};
}

3
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

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

23
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

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

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

@ -0,0 +1,99 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testSymbolicBayesNet.cpp
* @brief Unit tests for a symbolic Bayes chain
* @author Frank Dellaert
*/
#include <boost/make_shared.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
using namespace std;
using namespace gtsam;
static const Key _L_ = 0;
static const Key _A_ = 1;
static const Key _B_ = 2;
static const Key _C_ = 3;
static const Key _D_ = 4;
static const Key _E_ = 5;
static SymbolicConditionalUnordered::shared_ptr
B(new SymbolicConditionalUnordered(_B_)),
L(new SymbolicConditionalUnordered(_L_, _B_));
/* ************************************************************************* */
TEST( SymbolicBayesNet, equals )
{
SymbolicBayesNetUnordered f1;
f1.push_back(B);
f1.push_back(L);
SymbolicBayesNetUnordered f2;
f2.push_back(L);
f2.push_back(B);
CHECK(f1.equals(f1));
CHECK(!f1.equals(f2));
}
/* ************************************************************************* */
TEST( SymbolicBayesNet, combine )
{
SymbolicConditionalUnordered::shared_ptr
A(new SymbolicConditionalUnordered(_A_,_B_,_C_)),
B(new SymbolicConditionalUnordered(_B_,_C_)),
C(new SymbolicConditionalUnordered(_C_));
// p(A|BC)
SymbolicBayesNetUnordered p_ABC;
p_ABC.push_back(A);
// P(BC)=P(B|C)P(C)
SymbolicBayesNetUnordered p_BC;
p_BC.push_back(B);
p_BC.push_back(C);
// P(ABC) = P(A|BC) P(BC)
p_ABC.push_back(p_BC);
SymbolicBayesNetUnordered expected;
expected.push_back(A);
expected.push_back(B);
expected.push_back(C);
CHECK(assert_equal(expected,p_ABC));
}
/* ************************************************************************* */
TEST(SymbolicBayesNet, saveGraph) {
SymbolicBayesNetUnordered bn;
bn.add(SymbolicConditionalUnordered(_A_, _B_));
std::vector<Index> keys;
keys.push_back(_B_);
keys.push_back(_C_);
keys.push_back(_D_);
bn.add(SymbolicConditionalUnordered::FromKeys(keys,2));
bn.add(SymbolicConditionalUnordered(_D_));
bn.saveGraph("SymbolicBayesNet.dot");
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -0,0 +1,116 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testSymbolicFactorGraph.cpp
* @brief Unit tests for symbolic factor graphs
* @author Christian Potthast
**/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
//TEST(SymbolicFactorGraph, eliminateFrontals) {
//
// SymbolicFactorGraph sfgOrig;
// sfgOrig.push_factor(0,1);
// sfgOrig.push_factor(0,2);
// sfgOrig.push_factor(1,3);
// sfgOrig.push_factor(1,4);
// sfgOrig.push_factor(2,3);
// sfgOrig.push_factor(4,5);
//
// SymbolicConditionalUnordered::shared_ptr actualCond;
// SymbolicFactorGraph::shared_ptr actualSfg;
// boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2);
//
// vector<Index> condIndices;
// condIndices += 0,1,2,3,4;
// IndexConditional expectedCond(condIndices, 2);
//
// SymbolicFactorGraph expectedSfg;
// expectedSfg.push_factor(2,3);
// expectedSfg.push_factor(4,5);
// expectedSfg.push_factor(2,3,4);
//
// EXPECT(assert_equal(expectedSfg, actualSfg));
// EXPECT(assert_equal(expectedCond, *actualCond));
//}
///* ************************************************************************* */
//TEST( SymbolicFactorGraph, EliminateOne )
//{
// // create a test graph
// SymbolicFactorGraph fg;
// fg.push_factor(vx2, vx1);
//
// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1);
// SymbolicFactorGraph expected;
// expected.push_back(boost::shared_ptr<IndexFactor>());
// expected.push_factor(vx1);
//
// CHECK(assert_equal(expected, fg));
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, constructFromBayesNet )
{
// create expected factor graph
SymbolicFactorGraphUnordered expected;
expected.push_factor(0, 1, 2);
expected.push_factor(1, 2);
expected.push_factor(1);
// create Bayes Net
SymbolicBayesNetUnordered bayesNet;
bayesNet.add(SymbolicConditionalUnordered(0, 1, 2));
bayesNet.add(SymbolicConditionalUnordered(1, 2));
bayesNet.add(SymbolicConditionalUnordered(1));
// create actual factor graph from a Bayes Net
SymbolicFactorGraphUnordered actual(bayesNet);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, push_back )
{
// Create two factor graphs and expected combined graph
SymbolicFactorGraphUnordered fg1, fg2, expected;
fg1.push_factor(1);
fg1.push_factor(0, 1);
fg2.push_factor(1, 2);
fg2.push_factor(0, 2);
expected.push_factor(1);
expected.push_factor(0, 1);
expected.push_factor(1, 2);
expected.push_factor(0, 2);
// combine
SymbolicFactorGraphUnordered actual;
actual.push_back(fg1);
actual.push_back(fg2);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */