Started implementing junction tree construction
Richard Roberts
parent
5b1ac91c85
commit
b4d282f67e
|
|
@ -28,7 +28,7 @@ namespace gtsam {
|
|||
template<typename NODE, typename DATA>
|
||||
struct TraversalNode {
|
||||
bool expanded;
|
||||
NODE* const treeNode;
|
||||
NODE& const treeNode;
|
||||
DATA data;
|
||||
TraversalNode(NODE* _treeNode, const DATA& _data) :
|
||||
expanded(false), treeNode(_treeNode), data(_data) {}
|
||||
|
|
@ -38,16 +38,14 @@ namespace gtsam {
|
|||
template<typename NODE, typename VISITOR, typename DATA, typename STACK>
|
||||
struct PreOrderExpand {
|
||||
VISITOR& visitor_;
|
||||
DATA* parentData_;
|
||||
DATA& parentData_;
|
||||
STACK& stack_;
|
||||
PreOrderExpand(VISITOR& visitor, DATA* parentData, STACK& stack) :
|
||||
PreOrderExpand(VISITOR& visitor, DATA& parentData, STACK& stack) :
|
||||
visitor_(visitor), parentData_(parentData), stack_(stack) {}
|
||||
template<typename P>
|
||||
void operator()(const P& nodePointer) {
|
||||
// Get raw pointer (in case nodePointer is a shared_ptr)
|
||||
NODE* rawNodePointer = &(*nodePointer);
|
||||
// Add node
|
||||
stack_.push(TraversalNode<NODE,DATA>(rawNodePointer, visitor_(nodePointer, *parentData_)));
|
||||
stack_.push(TraversalNode<NODE,DATA>(*nodePointer, visitor_(nodePointer, parentData_)));
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -82,7 +80,7 @@ namespace gtsam {
|
|||
// Add roots to stack (use reverse iterators so children are processed in the order they
|
||||
// appear)
|
||||
(void) std::for_each(forest.roots().rbegin(), forest.roots().rend(),
|
||||
Expander(visitorPre, &rootData, stack));
|
||||
Expander(visitorPre, rootData, stack));
|
||||
|
||||
// Traverse
|
||||
while(!stack.empty())
|
||||
|
|
@ -93,13 +91,13 @@ namespace gtsam {
|
|||
if(node.expanded) {
|
||||
// If already expanded, then the data stored in the node is no longer needed, so visit
|
||||
// then delete it.
|
||||
(void) visitorPost(*node.treeNode, node.data);
|
||||
(void) visitorPost(node.treeNode, node.data);
|
||||
stack.pop();
|
||||
} 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(),
|
||||
Expander(visitorPre, &node.data, stack));
|
||||
Expander(visitorPre, node.data, stack));
|
||||
node.expanded = true;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,6 +37,8 @@ namespace gtsam {
|
|||
const boost::shared_ptr<BayesNetType>& output,
|
||||
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
|
||||
{
|
||||
// This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
|
||||
|
||||
assert(childrenResults.size() == children.size());
|
||||
|
||||
// Gather factors
|
||||
|
|
@ -146,8 +148,7 @@ namespace gtsam {
|
|||
// Build variable index first
|
||||
const VariableIndexUnordered variableIndex(factorGraph);
|
||||
This temp(factorGraph, variableIndex, order);
|
||||
roots_.swap(temp.roots_); // Swap in the tree, and temp will be deleted
|
||||
remainingFactors_.swap(temp.remainingFactors_);
|
||||
this->swap(temp); // Swap in the tree, and temp will be deleted
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -295,4 +296,12 @@ namespace gtsam {
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESNET, class GRAPH>
|
||||
void EliminationTreeUnordered<BAYESNET,GRAPH>::swap(This& other) {
|
||||
roots_.swap(other.roots_);
|
||||
remainingFactors_.swap(other.remainingFactors_);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -65,8 +65,8 @@ namespace gtsam {
|
|||
Eliminate; ///< Typedef for an eliminate subroutine
|
||||
|
||||
struct Node {
|
||||
typedef FastList<sharedFactor> Factors;
|
||||
typedef FastList<boost::shared_ptr<Node> > Children;
|
||||
typedef std::vector<sharedFactor> Factors;
|
||||
typedef std::vector<boost::shared_ptr<Node> > Children;
|
||||
|
||||
Key key; ///< key associated with root
|
||||
Factors factors; ///< factors associated with root
|
||||
|
|
@ -146,6 +146,9 @@ namespace gtsam {
|
|||
|
||||
const FastList<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
/** Swap the data of this tree with another one, this operation is very fast. */
|
||||
void swap(This& other);
|
||||
|
||||
protected:
|
||||
/// Protected default constructor
|
||||
EliminationTreeUnordered() {}
|
||||
|
|
|
|||
|
|
@ -28,6 +28,71 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
namespace {
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
struct ConstructorTraversalData {
|
||||
const ConstructorTraversalData& parentData;
|
||||
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode;
|
||||
ConstructorTraversalData(const ConstructorTraversalData& _parentData) : parentData(_parentData) {}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH, class ETREE_NODE>
|
||||
ConstructorTraversalData<BAYESTREE,GRAPH> ConstructorTraversalVisitorPre(
|
||||
const boost::shared_ptr<ETREE_NODE>& node,
|
||||
const ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
|
||||
{
|
||||
// On the pre-order pass, before children have been visited, we just set up a traversal data
|
||||
// structure with its own JT node, and create a child pointer in its parent.
|
||||
ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(parentData);
|
||||
myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node>();
|
||||
myData.myJTNode->keys.push_back(node->key);
|
||||
myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
|
||||
parentData.myJTNode->children.push_back(myData.myJTNode);
|
||||
return myData;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH, class ETREE_NODE, class SYMBOLIC_CONDITIONAL>
|
||||
struct ConstructorTraversalVisitorPost
|
||||
{
|
||||
// Store and increment an iterator into the Bayes net during the post-order step
|
||||
FactorGraphUnordered<SYMBOLIC_CONDITIONAL>::const_iterator symbolicBayesNetIterator;
|
||||
|
||||
// Constructor that starts at the beginning of the Bayes net
|
||||
ConstructorTraversalVisitorPost(const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet) :
|
||||
symbolicBayesNetIterator(symbolicBayesNet.begin()) {}
|
||||
|
||||
// Post-order visitor function
|
||||
void operator()(
|
||||
const boost::shared_ptr<ETREE_NODE>& node,
|
||||
const ConstructorTraversalData<BAYESTREE,GRAPH>& myData)
|
||||
{
|
||||
// In the post-order step, we check if we are in the same clique with our parent. If so, we
|
||||
// merge our JT nodes.
|
||||
if()
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
template<class ETREE, class SYMBOLIC_CONDITIONAL>
|
||||
JunctionTreeUnordered(const ETREE& eliminationTree,
|
||||
const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet)
|
||||
{
|
||||
// Here we rely on the BayesNet having been produced by this elimination tree, such that the
|
||||
// conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect
|
||||
// the symbolic conditional corresponding to each node. The elimination tree node is added to
|
||||
// the same clique with its parent if it has exactly one more Bayes net conditional parent than
|
||||
// does its elimination tree parent.
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {
|
||||
|
|
|
|||
|
|
@ -21,51 +21,11 @@
|
|||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/FactorGraphUnordered.h>
|
||||
#include <gtsam/inference/EliminationTreeUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
class JunctionTreeUnordered {
|
||||
|
||||
public:
|
||||
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
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 BAYESTREE BayesTreeType; ///< The BayesNet corresponding to FACTOR
|
||||
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
|
||||
|
||||
struct Node {
|
||||
typedef FastList<sharedFactor> Factors;
|
||||
typedef FastList<boost::shared_ptr<Node> > Children;
|
||||
|
||||
Key key; ///< key associated with root
|
||||
Factors factors; ///< factors associated with root
|
||||
Children children; ///< sub-trees
|
||||
|
||||
sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
|
||||
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
|
||||
|
||||
private:
|
||||
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
FastList<sharedNode> roots_;
|
||||
std::vector<sharedFactor> remainingFactors_;
|
||||
|
||||
public:
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with
|
||||
* the additional property that it represents the clique tree associated with a Bayes net.
|
||||
|
|
@ -87,86 +47,58 @@ namespace gtsam {
|
|||
* \addtogroup Multifrontal
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
|
||||
class JunctionTree: public ClusterTree<FG> {
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
class JunctionTreeUnordered {
|
||||
|
||||
public:
|
||||
|
||||
/// In a junction tree each cluster is associated with a clique
|
||||
typedef typename ClusterTree<FG>::Cluster Clique;
|
||||
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
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 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
|
||||
|
||||
/// The BayesTree type produced by elimination
|
||||
typedef BTCLIQUE BTClique;
|
||||
struct Node {
|
||||
typedef std::vector<Key> Keys;
|
||||
typedef std::vector<sharedFactor> Factors;
|
||||
typedef std::vector<boost::shared_ptr<Node> > Children;
|
||||
|
||||
/// Shared pointer to this class
|
||||
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
|
||||
Keys keys; ///< Frontal keys of this node
|
||||
Factors factors; ///< Factors associated with this node
|
||||
Children children; ///< sub-trees
|
||||
|
||||
/// We will frequently refer to a symbolic Bayes tree, used to find the clique structure
|
||||
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
|
||||
sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
|
||||
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
|
||||
|
||||
private:
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
/// distribute the factors along the cluster tree
|
||||
sharedClique distributeFactors(const FG& fg,
|
||||
const SymbolicBayesTree::sharedClique& clique);
|
||||
|
||||
/// distribute the factors along the cluster tree
|
||||
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
|
||||
const SymbolicBayesTree::sharedClique& clique);
|
||||
|
||||
/// recursive elimination function
|
||||
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
|
||||
eliminateOneClique(typename FG::Eliminate function,
|
||||
const boost::shared_ptr<const Clique>& clique) const;
|
||||
|
||||
/// internal constructor
|
||||
void construct(const FG& fg, const VariableIndex& variableIndex);
|
||||
|
||||
/// @}
|
||||
FastList<sharedNode> roots_;
|
||||
std::vector<sharedFactor> remainingFactors_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor */
|
||||
JunctionTree() {}
|
||||
|
||||
/** Named constructor to build the junction tree of a factor graph. Note
|
||||
* that this has to compute the column structure as a VariableIndex, so if you
|
||||
* already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&)
|
||||
* constructor instead.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
*/
|
||||
JunctionTree(const FG& factorGraph);
|
||||
|
||||
/** Construct from a factor graph and pre-computed variable index.
|
||||
* @param fg The factor graph for which to build the junction tree
|
||||
* @param structure The set of factors involving each variable. If this is not
|
||||
* precomputed, you can call the JunctionTree(const FG&)
|
||||
* constructor instead.
|
||||
*/
|
||||
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Eliminate the factors in the subgraphs to produce a BayesTree.
|
||||
* @param function The function used to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The BayesTree resulting from elimination
|
||||
*/
|
||||
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
|
||||
/** Build the junction tree from an elimination tree and a symbolic Bayes net. */
|
||||
template<class ETREE, class SYMBOLIC_CONDITIONAL>
|
||||
JunctionTreeUnordered(
|
||||
const ETREE& eliminationTree,
|
||||
const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet);
|
||||
|
||||
/// @}
|
||||
|
||||
}; // JunctionTree
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/JunctionTree-inl.h>
|
||||
}
|
||||
Loading…
Reference in New Issue