Restored ClusterTree base class of JunctionTree
Richard Roberts
parent
121e71431a
commit
69c7f6d137
|
|
@ -30,7 +30,7 @@ namespace gtsam {
|
|||
|
||||
// Forward declarations
|
||||
template<class FACTOR> class FactorGraph;
|
||||
template<class BAYESTREE, class GRAPH> class JunctionTree;
|
||||
template<class BAYESTREE, class GRAPH> class ClusterTree;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** clique statistics */
|
||||
|
|
@ -244,7 +244,7 @@ namespace gtsam {
|
|||
void fillNodesIndex(const sharedClique& subtree);
|
||||
|
||||
// Friend JunctionTree because it directly fills roots and nodes index.
|
||||
template<class BAYESRTEE, class GRAPH> friend class JunctionTree;
|
||||
template<class BAYESRTEE, class GRAPH> friend class ClusterTree;
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,183 @@
|
|||
/**
|
||||
* @file ClusterTree-inst.h
|
||||
* @date Oct 8, 2013
|
||||
* @author Kai Ni
|
||||
* @author Richard Roberts
|
||||
* @author Frank Dellaert
|
||||
* @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree
|
||||
*/
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/inference/ClusterTree.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace gtsam
|
||||
{
|
||||
namespace
|
||||
{
|
||||
/* ************************************************************************* */
|
||||
// Elimination traversal data - stores a pointer to the parent data and collects the factors
|
||||
// resulting from elimination of the children. Also sets up BayesTree cliques with parent and
|
||||
// child pointers.
|
||||
template<class CLUSTERTREE>
|
||||
struct EliminationData {
|
||||
EliminationData* const parentData;
|
||||
size_t myIndexInParent;
|
||||
FastVector<typename CLUSTERTREE::sharedFactor> childFactors;
|
||||
boost::shared_ptr<typename CLUSTERTREE::BayesTreeType::Node> bayesTreeNode;
|
||||
EliminationData(EliminationData* _parentData, size_t nChildren) :
|
||||
parentData(_parentData),
|
||||
bayesTreeNode(boost::make_shared<typename CLUSTERTREE::BayesTreeType::Node>())
|
||||
{
|
||||
if(parentData) {
|
||||
myIndexInParent = parentData->childFactors.size();
|
||||
parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor());
|
||||
} else {
|
||||
myIndexInParent = 0;
|
||||
}
|
||||
// Set up BayesTree parent and child pointers
|
||||
if(parentData) {
|
||||
if(parentData->parentData) // If our parent is not the dummy node
|
||||
bayesTreeNode->parent_ = parentData->bayesTreeNode;
|
||||
parentData->bayesTreeNode->children.push_back(bayesTreeNode);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination pre-order visitor - just creates the EliminationData structure for the visited
|
||||
// node.
|
||||
template<class CLUSTERTREE>
|
||||
EliminationData<CLUSTERTREE> eliminationPreOrderVisitor(
|
||||
const typename CLUSTERTREE::sharedNode& node, EliminationData<CLUSTERTREE>& parentData)
|
||||
{
|
||||
EliminationData<CLUSTERTREE> myData(&parentData, node->children.size());
|
||||
myData.bayesTreeNode->problemSize_ = node->problemSize();
|
||||
return myData;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination post-order visitor - combine the child factors with our own factors, add the
|
||||
// resulting conditional to the BayesTree, and add the remaining factor to the parent.
|
||||
template<class CLUSTERTREE>
|
||||
struct EliminationPostOrderVisitor
|
||||
{
|
||||
const typename CLUSTERTREE::Eliminate& eliminationFunction;
|
||||
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex;
|
||||
EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction,
|
||||
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
|
||||
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {}
|
||||
void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData<CLUSTERTREE>& myData)
|
||||
{
|
||||
// Typedefs
|
||||
typedef typename CLUSTERTREE::sharedFactor sharedFactor;
|
||||
typedef typename CLUSTERTREE::FactorType FactorType;
|
||||
typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
|
||||
typedef typename CLUSTERTREE::ConditionalType ConditionalType;
|
||||
typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
|
||||
|
||||
// Gather factors
|
||||
FactorGraphType gatheredFactors;
|
||||
gatheredFactors.reserve(node->factors.size() + node->children.size());
|
||||
gatheredFactors += node->factors;
|
||||
gatheredFactors += myData.childFactors;
|
||||
|
||||
// Check for Bayes tree orphan subtrees, and add them to our children
|
||||
BOOST_FOREACH(const sharedFactor& f, node->factors)
|
||||
{
|
||||
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
|
||||
{
|
||||
myData.bayesTreeNode->children.push_back(asSubtree->clique);
|
||||
asSubtree->clique->parent_ = myData.bayesTreeNode;
|
||||
}
|
||||
}
|
||||
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
eliminationFunction(gatheredFactors, Ordering(node->keys));
|
||||
|
||||
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
|
||||
myData.bayesTreeNode->setEliminationResult(eliminationResult);
|
||||
|
||||
// Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
|
||||
// putting orphan subtrees in the index - they'll already be in the index of the ISAM2
|
||||
// object they're added to.
|
||||
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
|
||||
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode));
|
||||
|
||||
// Store remaining factor in parent's gathered factors
|
||||
if(!eliminationResult.second->empty())
|
||||
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
void ClusterTree<BAYESTREE,GRAPH>::Cluster::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
std::cout << s;
|
||||
BOOST_FOREACH(Key j, keys)
|
||||
std::cout << j << " ";
|
||||
std::cout << "problemSize = " << problemSize_ << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
void ClusterTree<BAYESTREE,GRAPH>::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
treeTraversal::PrintForest(*this, s, keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
ClusterTree<BAYESTREE,GRAPH>& ClusterTree<BAYESTREE,GRAPH>::operator=(const This& other)
|
||||
{
|
||||
// Start by duplicating the tree.
|
||||
roots_ = treeTraversal::CloneForest(other);
|
||||
|
||||
// Assign the remaining factors - these are pointers to factors in the original factor graph and
|
||||
// we do not clone them.
|
||||
remainingFactors_ = other.remainingFactors_;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
|
||||
ClusterTree<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
|
||||
{
|
||||
gttic(ClusterTree_eliminate);
|
||||
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
|
||||
// that contains all of the roots as its children. rootsContainer also stores the remaining
|
||||
// uneliminated factors passed up from the roots.
|
||||
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
|
||||
EliminationData<This> rootsContainer(0, roots_.size());
|
||||
EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
|
||||
treeTraversal::DepthFirstForestParallel(*this, rootsContainer,
|
||||
eliminationPreOrderVisitor<This>, visitorPost, 10);
|
||||
|
||||
// Create BayesTree from roots stored in the dummy BayesTree node.
|
||||
result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end());
|
||||
|
||||
// Add remaining factors that were not involved with eliminated variables
|
||||
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
|
||||
allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
|
||||
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
|
||||
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors)
|
||||
if(factor)
|
||||
allRemainingFactors->push_back(factor);
|
||||
|
||||
// Return result
|
||||
return std::make_pair(result, allRemainingFactors);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
/**
|
||||
* @file ClusterTree.h
|
||||
* @date Oct 8, 2013
|
||||
* @author Kai Ni
|
||||
* @author Richard Roberts
|
||||
* @author Frank Dellaert
|
||||
* @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
namespace gtsam
|
||||
{
|
||||
|
||||
/**
|
||||
* A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman:
|
||||
* each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that
|
||||
* each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
class ClusterTree
|
||||
{
|
||||
public:
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
|
||||
typedef ClusterTree<BAYESTREE, GRAPH> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
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 boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
|
||||
|
||||
struct Cluster {
|
||||
typedef FastVector<Key> Keys;
|
||||
typedef FastVector<sharedFactor> Factors;
|
||||
typedef FastVector<boost::shared_ptr<Cluster> > Children;
|
||||
|
||||
Keys keys; ///< Frontal keys of this node
|
||||
Factors factors; ///< Factors associated with this node
|
||||
Children children; ///< sub-trees
|
||||
int problemSize_;
|
||||
|
||||
int problemSize() const { return problemSize_; }
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Cluster> sharedCluster; ///< Shared pointer to Cluster
|
||||
typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions
|
||||
typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions
|
||||
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
protected:
|
||||
FastVector<sharedNode> roots_;
|
||||
FastVector<sharedFactor> remainingFactors_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
ClusterTree(const This& other) { *this = other; }
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print the cluster tree */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Eliminate the factors to a Bayes tree and remaining factor graph
|
||||
* @param function The function to use to eliminate, see the namespace functions
|
||||
* in GaussianFactorGraph.h
|
||||
* @return The Bayes tree and factor graph resulting from elimination
|
||||
*/
|
||||
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminate(const Eliminate& function) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Return the set of roots (one for a tree, multiple for a forest) */
|
||||
const FastVector<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
/** Return the remaining factors that are not pulled into elimination */
|
||||
const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
|
||||
|
||||
/// @}
|
||||
|
||||
protected:
|
||||
/// @name Details
|
||||
|
||||
/// 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);
|
||||
|
||||
/// Default constructor to be used in derived classes
|
||||
ClusterTree() {}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -20,16 +20,11 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/treeTraversal-inst.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/inference/ClusterTree-inst.h>
|
||||
#include <gtsam/symbolic/SymbolicConditional.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace {
|
||||
|
|
@ -119,121 +114,6 @@ namespace gtsam {
|
|||
}
|
||||
myData.myJTNode->problemSize_ = combinedProblemSize;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination traversal data - stores a pointer to the parent data and collects the factors
|
||||
// resulting from elimination of the children. Also sets up BayesTree cliques with parent and
|
||||
// child pointers.
|
||||
template<class JUNCTIONTREE>
|
||||
struct EliminationData {
|
||||
EliminationData* const parentData;
|
||||
size_t myIndexInParent;
|
||||
FastVector<typename JUNCTIONTREE::sharedFactor> childFactors;
|
||||
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType::Node> bayesTreeNode;
|
||||
EliminationData(EliminationData* _parentData, size_t nChildren) :
|
||||
parentData(_parentData),
|
||||
bayesTreeNode(boost::make_shared<typename JUNCTIONTREE::BayesTreeType::Node>())
|
||||
{
|
||||
if(parentData) {
|
||||
myIndexInParent = parentData->childFactors.size();
|
||||
parentData->childFactors.push_back(typename JUNCTIONTREE::sharedFactor());
|
||||
} else {
|
||||
myIndexInParent = 0;
|
||||
}
|
||||
// Set up BayesTree parent and child pointers
|
||||
if(parentData) {
|
||||
if(parentData->parentData) // If our parent is not the dummy node
|
||||
bayesTreeNode->parent_ = parentData->bayesTreeNode;
|
||||
parentData->bayesTreeNode->children.push_back(bayesTreeNode);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination pre-order visitor - just creates the EliminationData structure for the visited
|
||||
// node.
|
||||
template<class JUNCTIONTREE>
|
||||
EliminationData<JUNCTIONTREE> eliminationPreOrderVisitor(
|
||||
const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& parentData)
|
||||
{
|
||||
EliminationData<JUNCTIONTREE> myData(&parentData, node->children.size());
|
||||
myData.bayesTreeNode->problemSize_ = node->problemSize();
|
||||
return myData;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination post-order visitor - combine the child factors with our own factors, add the
|
||||
// resulting conditional to the BayesTree, and add the remaining factor to the parent.
|
||||
template<class JUNCTIONTREE>
|
||||
struct EliminationPostOrderVisitor
|
||||
{
|
||||
const typename JUNCTIONTREE::Eliminate& eliminationFunction;
|
||||
typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex;
|
||||
EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction,
|
||||
typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex) :
|
||||
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {}
|
||||
void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData)
|
||||
{
|
||||
// Typedefs
|
||||
typedef typename JUNCTIONTREE::sharedFactor sharedFactor;
|
||||
typedef typename JUNCTIONTREE::FactorType FactorType;
|
||||
typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType;
|
||||
typedef typename JUNCTIONTREE::ConditionalType ConditionalType;
|
||||
typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode;
|
||||
|
||||
// Gather factors
|
||||
FactorGraphType gatheredFactors;
|
||||
gatheredFactors.reserve(node->factors.size() + node->children.size());
|
||||
gatheredFactors += node->factors;
|
||||
gatheredFactors += myData.childFactors;
|
||||
|
||||
// Check for Bayes tree orphan subtrees, and add them to our children
|
||||
BOOST_FOREACH(const sharedFactor& f, node->factors)
|
||||
{
|
||||
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
|
||||
{
|
||||
myData.bayesTreeNode->children.push_back(asSubtree->clique);
|
||||
asSubtree->clique->parent_ = myData.bayesTreeNode;
|
||||
}
|
||||
}
|
||||
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
eliminationFunction(gatheredFactors, Ordering(node->keys));
|
||||
|
||||
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
|
||||
myData.bayesTreeNode->setEliminationResult(eliminationResult);
|
||||
|
||||
// Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
|
||||
// putting orphan subtrees in the index - they'll already be in the index of the ISAM2
|
||||
// object they're added to.
|
||||
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
|
||||
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode));
|
||||
|
||||
// Store remaining factor in parent's gathered factors
|
||||
if(!eliminationResult.second->empty())
|
||||
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
void JunctionTree<BAYESTREE,GRAPH>::Node::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
std::cout << s;
|
||||
BOOST_FOREACH(Key j, keys)
|
||||
std::cout << j << " ";
|
||||
std::cout << "problemSize = " << problemSize_ << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
void JunctionTree<BAYESTREE,GRAPH>::print(
|
||||
const std::string& s, const KeyFormatter& keyFormatter) const
|
||||
{
|
||||
treeTraversal::PrintForest(*this, s, keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -252,59 +132,15 @@ namespace gtsam {
|
|||
// the created junction tree roots in a dummy Node.
|
||||
typedef typename ETREE::Node ETreeNode;
|
||||
ConstructorTraversalData<BAYESTREE, GRAPH> rootData(0);
|
||||
rootData.myJTNode = boost::make_shared<Node>(); // Make a dummy node to gather the junction tree roots
|
||||
rootData.myJTNode = boost::make_shared<typename Base::Node>(); // Make a dummy node to gather the junction tree roots
|
||||
treeTraversal::DepthFirstForest(eliminationTree, rootData,
|
||||
ConstructorTraversalVisitorPre<BAYESTREE,GRAPH,ETreeNode>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH,ETreeNode>);
|
||||
|
||||
// Assign roots from the dummy node
|
||||
roots_ = rootData.myJTNode->children;
|
||||
Base::roots_ = rootData.myJTNode->children;
|
||||
|
||||
// Transfer remaining factors from elimination tree
|
||||
remainingFactors_ = eliminationTree.remainingFactors();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
JunctionTree<BAYESTREE,GRAPH>& JunctionTree<BAYESTREE,GRAPH>::operator=(const This& other)
|
||||
{
|
||||
// Start by duplicating the tree.
|
||||
roots_ = treeTraversal::CloneForest(other);
|
||||
|
||||
// Assign the remaining factors - these are pointers to factors in the original factor graph and
|
||||
// we do not clone them.
|
||||
remainingFactors_ = other.remainingFactors_;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
|
||||
JunctionTree<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
|
||||
{
|
||||
gttic(JunctionTree_eliminate);
|
||||
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
|
||||
// that contains all of the roots as its children. rootsContainer also stores the remaining
|
||||
// uneliminated factors passed up from the roots.
|
||||
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
|
||||
EliminationData<This> rootsContainer(0, roots_.size());
|
||||
EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
|
||||
treeTraversal::DepthFirstForestParallel(*this, rootsContainer,
|
||||
eliminationPreOrderVisitor<This>, visitorPost, 10);
|
||||
|
||||
// Create BayesTree from roots stored in the dummy BayesTree node.
|
||||
result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end());
|
||||
|
||||
// Add remaining factors that were not involved with eliminated variables
|
||||
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
|
||||
allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
|
||||
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
|
||||
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors)
|
||||
if(factor)
|
||||
allRemainingFactors->push_back(factor);
|
||||
|
||||
// Return result
|
||||
return std::make_pair(result, allRemainingFactors);
|
||||
Base::remainingFactors_ = eliminationTree.remainingFactors();
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
|
|||
|
|
@ -20,15 +20,14 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/inference/ClusterTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* A JunctionTree is 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.
|
||||
*
|
||||
* In GTSAM a junction tree is an intermediate data structure in multifrontal
|
||||
* variable elimination. Each node is a cluster of factors, along with a
|
||||
|
|
@ -47,45 +46,13 @@ namespace gtsam {
|
|||
* \nosubgrouping
|
||||
*/
|
||||
template<class BAYESTREE, class GRAPH>
|
||||
class JunctionTree {
|
||||
class JunctionTree : public ClusterTree<BAYESTREE, GRAPH> {
|
||||
|
||||
public:
|
||||
|
||||
typedef GRAPH FactorGraphType; ///< The factor graph type
|
||||
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
|
||||
typedef JunctionTree<BAYESTREE, GRAPH> This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
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 boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
|
||||
typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
|
||||
|
||||
struct Node {
|
||||
typedef FastVector<Key> Keys;
|
||||
typedef FastVector<sharedFactor> Factors;
|
||||
typedef FastVector<boost::shared_ptr<Node> > Children;
|
||||
|
||||
Keys keys; ///< Frontal keys of this node
|
||||
Factors factors; ///< Factors associated with this node
|
||||
Children children; ///< sub-trees
|
||||
int problemSize_;
|
||||
|
||||
int problemSize() const { return problemSize_; }
|
||||
|
||||
/** print this node */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
|
||||
|
||||
private:
|
||||
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
FastVector<sharedNode> roots_;
|
||||
FastVector<sharedFactor> remainingFactors_;
|
||||
typedef ClusterTree<BAYESTREE, GRAPH> Base; ///< Our base class
|
||||
|
||||
protected:
|
||||
|
||||
|
|
@ -99,43 +66,6 @@ namespace gtsam {
|
|||
/** Build the junction tree from an elimination tree. */
|
||||
template<class ETREE>
|
||||
JunctionTree(const ETREE& eliminationTree);
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
JunctionTree(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);
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
|
||||
/// @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<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
|
||||
eliminate(const Eliminate& function) const;
|
||||
|
||||
/** Print the junction tree */
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Return the set of roots (one for a tree, multiple for a forest) */
|
||||
const FastVector<sharedNode>& roots() const { return roots_; }
|
||||
|
||||
/** Return the remaining factors that are not pulled into elimination */
|
||||
const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -146,4 +76,4 @@ namespace gtsam {
|
|||
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,7 +22,8 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
// Instantiate base classes
|
||||
template class ClusterTree<GaussianBayesTree, GaussianFactorGraph>;
|
||||
template class JunctionTree<GaussianBayesTree, GaussianFactorGraph>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
namespace gtsam {
|
||||
|
||||
// Instantiate base class
|
||||
template class ClusterTree<SymbolicBayesTree, SymbolicFactorGraph>;
|
||||
template class JunctionTree<SymbolicBayesTree, SymbolicFactorGraph>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue