Working on JunctionTree elimination / BayesTree
Richard Roberts
parent
b4d282f67e
commit
bbad690f65
|
|
@ -121,6 +121,17 @@ namespace gtsam {
|
|||
forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
|
||||
}
|
||||
|
||||
/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child
|
||||
* pointers for a clone of the original tree.
|
||||
* @param forest The forest of trees to clone. The method \c forest.roots() should exist and
|
||||
* return a collection of shared pointers to \c FOREST::Node.
|
||||
* @return The new collection of roots. */
|
||||
template<class FOREST>
|
||||
std::vector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 <boost/shared_ptr.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A BayesNet is a tree of conditionals, stored in elimination order.
|
||||
*
|
||||
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class CONDITIONAL>
|
||||
class BayesNetUnordered {
|
||||
|
||||
public:
|
||||
|
||||
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() {};
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -21,7 +21,6 @@
|
|||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/function.hpp>
|
||||
|
||||
#include <gtsam/base/FastList.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
|
|
@ -83,7 +82,7 @@ namespace gtsam {
|
|||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
FastList<sharedNode> roots_;
|
||||
std::vector<sharedNode> roots_;
|
||||
std::vector<sharedFactor> remainingFactors_;
|
||||
|
||||
public:
|
||||
|
|
@ -109,12 +108,12 @@ namespace gtsam {
|
|||
*/
|
||||
EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order);
|
||||
|
||||
/** TODO: Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
EliminationTreeUnordered(const This& other) { *this = other; }
|
||||
|
||||
/** TODO: Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
|
||||
* are copied, factors are not cloned. */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @}
|
||||
|
|
@ -144,7 +143,11 @@ namespace gtsam {
|
|||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
const FastList<sharedNode>& roots() const { return roots_; }
|
||||
/** Return the set of roots (one for a tree, multiple for a forest) */
|
||||
const std::vector<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
/** Return the remaining factors that are not pulled into elimination */
|
||||
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
|
||||
|
||||
/** Swap the data of this tree with another one, this operation is very fast. */
|
||||
void swap(This& other);
|
||||
|
|
|
|||
|
|
@ -14,74 +14,127 @@
|
|||
* @date Feb 4, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
* @brief The junction tree, template bodies
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
#include <gtsam/inference/EliminationTree.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/inference/JunctionTreeUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
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)
|
||||
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedFactor
|
||||
JunctionTreeUnordered<BAYESTREE,GRAPH>::Node::eliminate(
|
||||
const boost::shared_ptr<BayesTreeType>& output,
|
||||
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
|
||||
{
|
||||
// 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;
|
||||
// This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
|
||||
|
||||
assert(childrenResults.size() == children.size());
|
||||
|
||||
// Gather factors
|
||||
std::vector<sharedFactor> gatheredFactors;
|
||||
gatheredFactors.reserve(factors.size() + children.size());
|
||||
gatheredFactors.insert(gatheredFactors.end(), factors.begin(), factors.end());
|
||||
gatheredFactors.insert(gatheredFactors.end(), childrenResults.begin(), childrenResults.end());
|
||||
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
function(gatheredFactors, keys);
|
||||
|
||||
// Add conditional to BayesNet
|
||||
output->push_back(eliminationResult.first);
|
||||
|
||||
// Return result
|
||||
return eliminationResult.second;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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()) {}
|
||||
namespace {
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
struct ConstructorTraversalData {
|
||||
const ConstructorTraversalData* parentData;
|
||||
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode;
|
||||
std::vector<SymbolicConditionalUnordered::shared_ptr> childSymbolicConditionals;
|
||||
std::vector<SymbolicFactorUnordered::shared_ptr> childSymbolicFactors;
|
||||
ConstructorTraversalData(const ConstructorTraversalData* _parentData) : parentData(_parentData) {}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Pre-order visitor function
|
||||
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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Post-order visitor function
|
||||
void operator()(
|
||||
template<class BAYESTREE, class GRAPH, class ETREE_NODE>
|
||||
void ConstructorTraversalVisitorPost(
|
||||
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()
|
||||
}
|
||||
};
|
||||
// Do symbolic elimination for this node
|
||||
std::vector<SymbolicFactorUnordered::shared_ptr> symbolicFactors;
|
||||
symbolicFactors.reserve(node->factors.size() + myData.childSymbolicFactors.size());
|
||||
BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, node->factors) {
|
||||
symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(factor)); }
|
||||
symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
|
||||
std::vector<Key> keyAsVector(1); keyAsVector[0] = node->key;
|
||||
std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
|
||||
EliminateSymbolicUnordered(symbolicFactors, keyAsVector);
|
||||
|
||||
// Store symbolic elimination results
|
||||
myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
|
||||
myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second);
|
||||
|
||||
// Merge our children if they are in our clique - if our conditional has exactly one fewer
|
||||
// parent than our child's conditional.
|
||||
const size_t myNrParents = symbolicElimResult.first->nrParents();
|
||||
size_t nrMergedChildren = 0;
|
||||
assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size());
|
||||
// Loop over children
|
||||
for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) {
|
||||
// Check if we should merge the child
|
||||
if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
|
||||
// Get a reference to the child, adjusting the index to account for children previously
|
||||
// merged and removed from the child list.
|
||||
const typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node& childToMerge =
|
||||
*myData.myJTNode->children[child - nrMergedChildren];
|
||||
// Merge keys, factors, and children.
|
||||
myData.myJTNode->keys.insert(myData.myJTNode->keys.end(), childToMerge.keys.begin(), childToMerge.keys.end());
|
||||
myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end());
|
||||
myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end());
|
||||
// Remove child from list.
|
||||
myData.myJTNode->children.erase(myData.myJTNode->children.begin() + child - nrMergedChildren);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
template<class ETREE, class SYMBOLIC_CONDITIONAL>
|
||||
JunctionTreeUnordered(const ETREE& eliminationTree,
|
||||
const FactorGraphUnordered<SYMBOLIC_CONDITIONAL>& symbolicBayesNet)
|
||||
template<class ETREE>
|
||||
JunctionTreeUnordered<BAYESTREE,GRAPH>::JunctionTreeUnordered(const ETREE& eliminationTree)
|
||||
{
|
||||
// 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
|
||||
|
|
@ -89,180 +142,52 @@ namespace gtsam {
|
|||
// the same clique with its parent if it has exactly one more Bayes net conditional parent than
|
||||
// does its elimination tree parent.
|
||||
|
||||
// Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather
|
||||
// the created junction tree roots in a dummy Node.
|
||||
ConstructorTraversalData<BAYESTREE, GRAPH> rootData(0);
|
||||
rootData.myJTNode = boost::make_shared<Node>(); // Make a dummy node to gather the junction tree roots
|
||||
treeTraversal.DepthFirstForest(eliminationTree, rootData,
|
||||
ConstructorTraversalVisitorPre<BAYESREE,GRAPH>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH>);
|
||||
|
||||
}
|
||||
// Assign roots from the dummy node
|
||||
roots_ = rootData.myJTNode->children;
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {
|
||||
gttic(JT_symbolic_ET);
|
||||
const typename EliminationTree<IndexFactor>::shared_ptr symETree =
|
||||
EliminationTree<IndexFactor>::Create(fg, variableIndex);
|
||||
assert(symETree.get());
|
||||
gttoc(JT_symbolic_ET);
|
||||
gttic(JT_symbolic_eliminate);
|
||||
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
|
||||
assert(sbn.get());
|
||||
gttoc(JT_symbolic_eliminate);
|
||||
gttic(symbolic_BayesTree);
|
||||
SymbolicBayesTree sbt(*sbn);
|
||||
gttoc(symbolic_BayesTree);
|
||||
|
||||
// distribute factors
|
||||
gttic(distributeFactors);
|
||||
this->root_ = distributeFactors(fg, sbt.root());
|
||||
gttoc(distributeFactors);
|
||||
// Transfer remaining factors from elimination tree
|
||||
remainingFactors_ = eliminationTree.remainingFactors();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) {
|
||||
gttic(VariableIndex);
|
||||
VariableIndex varIndex(fg);
|
||||
gttoc(VariableIndex);
|
||||
construct(fg, varIndex);
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
JunctionTreeUnordered<BAYESTREE,GRAPH>::JunctionTreeUnordered(const This& other)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FG, class BTCLIQUE>
|
||||
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
|
||||
construct(fg, variableIndex);
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
JunctionTreeUnordered<BAYESTREE,GRAPH>& JunctionTreeUnordered<BAYESREE,GRAPH>::operator=(const This& other)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(
|
||||
const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) {
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
|
||||
JunctionTreeUnordered<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
|
||||
{
|
||||
// Allocate result
|
||||
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
|
||||
|
||||
// Build "target" index. This is an index for each variable of the factors
|
||||
// that involve this variable as their *lowest-ordered* variable. For each
|
||||
// factor, it is the lowest-ordered variable of that factor that pulls the
|
||||
// factor into elimination, after which all of the information in the
|
||||
// factor is contained in the eliminated factors that are passed up the
|
||||
// tree as elimination continues.
|
||||
// Run tree elimination algorithm
|
||||
std::vector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
|
||||
|
||||
// Two stages - first build an array of the lowest-ordered variable in each
|
||||
// factor and find the last variable to be eliminated.
|
||||
std::vector<Index> lowestOrdered(fg.size(), std::numeric_limits<Index>::max());
|
||||
Index maxVar = 0;
|
||||
for(size_t i=0; i<fg.size(); ++i)
|
||||
if(fg[i]) {
|
||||
typename FG::FactorType::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
|
||||
if(min != fg[i]->end()) {
|
||||
lowestOrdered[i] = *min;
|
||||
maxVar = std::max(maxVar, *min);
|
||||
}
|
||||
}
|
||||
// Add remaining factors that were not involved with eliminated variables
|
||||
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
|
||||
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
|
||||
allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
|
||||
|
||||
// Now add each factor to the list corresponding to its lowest-ordered
|
||||
// variable.
|
||||
std::vector<FastList<size_t> > targets(maxVar+1);
|
||||
for(size_t i=0; i<lowestOrdered.size(); ++i)
|
||||
if(lowestOrdered[i] != std::numeric_limits<Index>::max())
|
||||
targets[lowestOrdered[i]].push_back(i);
|
||||
|
||||
// Now call the recursive distributeFactors
|
||||
return distributeFactors(fg, targets, bayesClique);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
|
||||
const std::vector<FastList<size_t> >& targets,
|
||||
const SymbolicBayesTree::sharedClique& bayesClique) {
|
||||
|
||||
if(bayesClique) {
|
||||
// create a new clique in the junction tree
|
||||
sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(),
|
||||
(*bayesClique)->beginParents(), (*bayesClique)->endParents()));
|
||||
|
||||
// count the factors for this cluster to pre-allocate space
|
||||
{
|
||||
size_t nFactors = 0;
|
||||
BOOST_FOREACH(const Index frontal, clique->frontal) {
|
||||
// There may be less variables in "targets" than there really are if
|
||||
// some of the highest-numbered variables do not pull in any factors.
|
||||
if(frontal < targets.size())
|
||||
nFactors += targets[frontal].size(); }
|
||||
clique->reserve(nFactors);
|
||||
}
|
||||
// add the factors to this cluster
|
||||
BOOST_FOREACH(const Index frontal, clique->frontal) {
|
||||
if(frontal < targets.size()) {
|
||||
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
|
||||
clique->push_back(fg[factorI]); } } }
|
||||
|
||||
// recursively call the children
|
||||
BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
|
||||
sharedClique child = distributeFactors(fg, targets, bayesChild);
|
||||
clique->addChild(child);
|
||||
child->parent() = clique;
|
||||
}
|
||||
return clique;
|
||||
} else
|
||||
return sharedClique();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
std::pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr,
|
||||
typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique(
|
||||
typename FG::Eliminate function,
|
||||
const boost::shared_ptr<const Clique>& current) const {
|
||||
|
||||
FG fg; // factor graph will be assembled from local factors and marginalized children
|
||||
fg.reserve(current->size() + current->children().size());
|
||||
fg.push_back(*current); // add the local factors
|
||||
|
||||
// receive the factors from the child and its clique point
|
||||
std::list<typename BTClique::shared_ptr> children;
|
||||
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
|
||||
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> tree_factor(
|
||||
eliminateOneClique(function, child));
|
||||
children.push_back(tree_factor.first);
|
||||
fg.push_back(tree_factor.second);
|
||||
}
|
||||
|
||||
// eliminate the combined factors
|
||||
// warning: fg is being eliminated in-place and will contain marginal afterwards
|
||||
|
||||
// Now that we know which factors and variables, and where variables
|
||||
// come from and go to, create and eliminate the new joint factor.
|
||||
gttic(CombineAndEliminate);
|
||||
typename FG::EliminationResult eliminated(function(fg,
|
||||
current->frontal.size()));
|
||||
gttoc(CombineAndEliminate);
|
||||
|
||||
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
|
||||
|
||||
gttic(Update_tree);
|
||||
// create a new clique corresponding the combined factors
|
||||
typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated));
|
||||
new_clique->children_ = children;
|
||||
|
||||
BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) {
|
||||
childRoot->parent_ = new_clique;
|
||||
}
|
||||
gttoc(Update_tree);
|
||||
|
||||
return std::make_pair(new_clique, eliminated.second);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FG, class BTCLIQUE>
|
||||
typename BTCLIQUE::shared_ptr JunctionTree<FG,BTCLIQUE>::eliminate(
|
||||
typename FG::Eliminate function) const {
|
||||
if (this->root()) {
|
||||
gttic(JT_eliminate);
|
||||
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> ret =
|
||||
this->eliminateOneClique(function, this->root());
|
||||
if (ret.second->size() != 0) throw std::runtime_error(
|
||||
"JuntionTree::eliminate: elimination failed because of factors left over!");
|
||||
gttoc(JT_eliminate);
|
||||
return ret.first;
|
||||
} else
|
||||
return typename BTClique::shared_ptr();
|
||||
// Return result
|
||||
return std::make_pair(result, allRemainingFactors);
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
|
|||
|
|
@ -14,7 +14,8 @@
|
|||
* @date Feb 4, 2010
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
* @brief: The junction tree
|
||||
* @author Richard Roberts
|
||||
* @brief The junction tree
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -83,7 +84,7 @@ namespace gtsam {
|
|||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
FastList<sharedNode> roots_;
|
||||
std::vector<sharedNode>& roots_;
|
||||
std::vector<sharedFactor> remainingFactors_;
|
||||
|
||||
public:
|
||||
|
|
@ -92,10 +93,40 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** 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);
|
||||
template<class ETREE>
|
||||
JunctionTreeUnordered(const ETREE& eliminationTree);
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
JunctionTreeUnordered(const This& other) { *this = other; }
|
||||
|
||||
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
|
||||
* are copied, factors are not cloned. */
|
||||
This& operator=(const This& other);
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Eliminate the factors to a Bayes net and remaining factor graph
|
||||
* @param function The function to use to eliminate, see the namespace functions
|
||||
* 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;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Return the set of roots (one for a tree, multiple for a forest) */
|
||||
const std::vector<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
/** Return the remaining factors that are not pulled into elimination */
|
||||
const std::vector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 SymbolicJunctionTreeUnordered.cpp
|
||||
* @date Mar 29, 2013
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/JunctionTreeUnordered-inst.h>
|
||||
#include <gtsam/symbolic/SymbolicJunctionTreeUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
SymbolicJunctionTreeUnordered::SymbolicJunctionTreeUnordered() {
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicJunctionTreeUnordered.h
|
||||
* @date Mar 29, 2013
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
|
||||
#include <gtsam/inference/JunctionTreeUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class GTSAM_EXPORT SymbolicJunctionTreeUnordered :
|
||||
public JunctionTreeUnordered<SymbolicBayesTreeUnordered, SymbolicFactorGraphUnordered> {
|
||||
public:
|
||||
typedef JunctionTreeUnordered<SymbolicBayesTreeUnordered, SymbolicFactorGraphUnordered> Base; ///< Base class
|
||||
typedef SymbolicJunctionTreeUnordered This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
||||
/**
|
||||
* Build the elimination tree of a factor graph using pre-computed column structure.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
* @param structure The set of factors involving each variable. If this is not
|
||||
* precomputed, you can call the Create(const FactorGraph<DERIVEDFACTOR>&)
|
||||
* named constructor instead.
|
||||
* @return The elimination tree
|
||||
*/
|
||||
SymbolicJunctionTreeUnordered(const SymbolicEliminationTreeUnordered& eliminationTree) :
|
||||
Base(eliminationTree) {}
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
SymbolicJunctionTreeUnordered(const This& other) : Base(other) {}
|
||||
|
||||
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
This& operator=(const This& other) { (void) Base::operator=(other); return *this; }
|
||||
|
||||
private:
|
||||
|
||||
/// Private default constructor
|
||||
SymbolicJunctionTreeUnordered();
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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 testJunctionTree.cpp
|
||||
* @brief Unit tests for Junction Tree
|
||||
* @author Kai Ni
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <boost/assign/std/vector.hpp> // for operator +=
|
||||
#include <boost/assign/std/set.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/inference/ClusterTree.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
|
||||
|
||||
/* ************************************************************************* *
|
||||
* x1 - x2 - x3 - x4
|
||||
* x3 x4
|
||||
* x2 x1 : x3
|
||||
****************************************************************************/
|
||||
TEST( JunctionTree, constructor )
|
||||
{
|
||||
const Index x2=0, x1=1, x3=2, x4=3;
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(x2,x1);
|
||||
fg.push_factor(x2,x3);
|
||||
fg.push_factor(x3,x4);
|
||||
|
||||
SymbolicJunctionTree actual(fg);
|
||||
|
||||
vector<Index> frontal1; frontal1 += x3, x4;
|
||||
vector<Index> frontal2; frontal2 += x2, x1;
|
||||
vector<Index> sep1;
|
||||
vector<Index> sep2; sep2 += x3;
|
||||
CHECK(assert_equal(frontal1, actual.root()->frontal));
|
||||
CHECK(assert_equal(sep1, actual.root()->separator));
|
||||
LONGS_EQUAL(1, actual.root()->size());
|
||||
CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal));
|
||||
CHECK(assert_equal(sep2, actual.root()->children().front()->separator));
|
||||
LONGS_EQUAL(2, actual.root()->children().front()->size());
|
||||
CHECK(assert_equal(*fg[2], *(*actual.root())[0]));
|
||||
CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0]));
|
||||
CHECK(assert_equal(*fg[1], *(*actual.root()->children().front())[1]));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
* x1 - x2 - x3 - x4
|
||||
* x3 x4
|
||||
* x2 x1 : x3
|
||||
****************************************************************************/
|
||||
TEST( JunctionTree, eliminate)
|
||||
{
|
||||
const Index x2=0, x1=1, x3=2, x4=3;
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(x2,x1);
|
||||
fg.push_factor(x2,x3);
|
||||
fg.push_factor(x3,x4);
|
||||
|
||||
SymbolicJunctionTree jt(fg);
|
||||
SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic);
|
||||
|
||||
BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
|
||||
SymbolicBayesTree expected(bn);
|
||||
|
||||
// cout << "BT from JT:\n";
|
||||
// actual->printTree("");
|
||||
|
||||
CHECK(assert_equal(*expected.root(), *actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue