Restored ClusterTree base class of JunctionTree
parent
121e71431a
commit
69c7f6d137
|
|
@ -30,7 +30,7 @@ namespace gtsam {
|
||||||
|
|
||||||
// Forward declarations
|
// Forward declarations
|
||||||
template<class FACTOR> class FactorGraph;
|
template<class FACTOR> class FactorGraph;
|
||||||
template<class BAYESTREE, class GRAPH> class JunctionTree;
|
template<class BAYESTREE, class GRAPH> class ClusterTree;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
/** clique statistics */
|
/** clique statistics */
|
||||||
|
|
@ -244,7 +244,7 @@ namespace gtsam {
|
||||||
void fillNodesIndex(const sharedClique& subtree);
|
void fillNodesIndex(const sharedClique& subtree);
|
||||||
|
|
||||||
// Friend JunctionTree because it directly fills roots and nodes index.
|
// 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:
|
private:
|
||||||
/** Serialization function */
|
/** 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
|
#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/JunctionTree.h>
|
||||||
|
#include <gtsam/inference/ClusterTree-inst.h>
|
||||||
#include <gtsam/symbolic/SymbolicConditional.h>
|
#include <gtsam/symbolic/SymbolicConditional.h>
|
||||||
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
||||||
|
|
||||||
#include <boost/foreach.hpp>
|
|
||||||
#include <boost/bind.hpp>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
@ -119,121 +114,6 @@ namespace gtsam {
|
||||||
}
|
}
|
||||||
myData.myJTNode->problemSize_ = combinedProblemSize;
|
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.
|
// the created junction tree roots in a dummy Node.
|
||||||
typedef typename ETREE::Node ETreeNode;
|
typedef typename ETREE::Node ETreeNode;
|
||||||
ConstructorTraversalData<BAYESTREE, GRAPH> rootData(0);
|
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,
|
treeTraversal::DepthFirstForest(eliminationTree, rootData,
|
||||||
ConstructorTraversalVisitorPre<BAYESTREE,GRAPH,ETreeNode>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH,ETreeNode>);
|
ConstructorTraversalVisitorPre<BAYESTREE,GRAPH,ETreeNode>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH,ETreeNode>);
|
||||||
|
|
||||||
// Assign roots from the dummy node
|
// Assign roots from the dummy node
|
||||||
roots_ = rootData.myJTNode->children;
|
Base::roots_ = rootData.myJTNode->children;
|
||||||
|
|
||||||
// Transfer remaining factors from elimination tree
|
// Transfer remaining factors from elimination tree
|
||||||
remainingFactors_ = eliminationTree.remainingFactors();
|
Base::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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} //namespace gtsam
|
} //namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -20,15 +20,14 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/inference/ClusterTree.h>
|
||||||
#include <gtsam/base/FastVector.h>
|
|
||||||
#include <gtsam/inference/Key.h>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* A ClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with
|
* A JunctionTree is a ClusterTree, i.e., a set of variable clusters with factors, arranged
|
||||||
* the additional property that it represents the clique tree associated with a Bayes net.
|
* 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
|
* In GTSAM a junction tree is an intermediate data structure in multifrontal
|
||||||
* variable elimination. Each node is a cluster of factors, along with a
|
* variable elimination. Each node is a cluster of factors, along with a
|
||||||
|
|
@ -47,45 +46,13 @@ namespace gtsam {
|
||||||
* \nosubgrouping
|
* \nosubgrouping
|
||||||
*/
|
*/
|
||||||
template<class BAYESTREE, class GRAPH>
|
template<class BAYESTREE, class GRAPH>
|
||||||
class JunctionTree {
|
class JunctionTree : public ClusterTree<BAYESTREE, GRAPH> {
|
||||||
|
|
||||||
public:
|
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 JunctionTree<BAYESTREE, GRAPH> This; ///< This class
|
||||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||||
typedef boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
|
typedef ClusterTree<BAYESTREE, GRAPH> Base; ///< Our base class
|
||||||
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_;
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
|
|
@ -100,43 +67,6 @@ namespace gtsam {
|
||||||
template<class ETREE>
|
template<class ETREE>
|
||||||
JunctionTree(const ETREE& eliminationTree);
|
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_; }
|
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
||||||
|
|
@ -22,7 +22,8 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
// Instantiate base class
|
// Instantiate base classes
|
||||||
|
template class ClusterTree<GaussianBayesTree, GaussianFactorGraph>;
|
||||||
template class JunctionTree<GaussianBayesTree, GaussianFactorGraph>;
|
template class JunctionTree<GaussianBayesTree, GaussianFactorGraph>;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -23,6 +23,7 @@
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
// Instantiate base class
|
// Instantiate base class
|
||||||
|
template class ClusterTree<SymbolicBayesTree, SymbolicFactorGraph>;
|
||||||
template class JunctionTree<SymbolicBayesTree, SymbolicFactorGraph>;
|
template class JunctionTree<SymbolicBayesTree, SymbolicFactorGraph>;
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue