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

Merge branch 'jtree_mania' into feature/CeresDefaults-2

release/4.3a0
dellaert 2015-06-21 14:39:46 -07:00
parent 5237c74928 ef829c333e
commit 786b762907
4 changed files with 558 additions and 507 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,19 +1,19 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file treeTraversal-inst.h * @file treeTraversal-inst.h
* @author Richard Roberts * @author Richard Roberts
* @date April 9, 2013 * @date April 9, 2013
*/ */
#pragma once #pragma once
#include <gtsam/base/treeTraversal/parallelTraversalTasks.h> #include <gtsam/base/treeTraversal/parallelTraversalTasks.h>
@ -33,31 +33,33 @@
namespace gtsam { namespace gtsam {
/** Internal functions used for traversing trees */ /** Internal functions used for traversing trees */
namespace treeTraversal { namespace treeTraversal {
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
// Internal node used in DFS preorder stack // Internal node used in DFS preorder stack
template<typename NODE, typename DATA> template<typename NODE, typename DATA>
struct TraversalNode { struct TraversalNode {
bool expanded; bool expanded;
const boost::shared_ptr<NODE>& treeNode; const boost::shared_ptr<NODE>& treeNode;
DATA& parentData; DATA& parentData;
typename FastList<DATA>::iterator dataPointer; typename FastList<DATA>::iterator dataPointer;
TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) : TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
expanded(false), treeNode(_treeNode), parentData(_parentData) {} expanded(false), treeNode(_treeNode), parentData(_parentData) {
};
// Do nothing - default argument for post-visitor for tree traversal
struct no_op {
template<typename NODE, typename DATA>
void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {}
};
} }
};
/** Traverse a forest depth-first with pre-order and post-order visits. // Do nothing - default argument for post-visitor for tree traversal
struct no_op {
template<typename NODE, typename DATA>
void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {
}
};
}
/** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist * @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node. * and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
@ -71,9 +73,10 @@ namespace gtsam {
* call to \c visitorPre (the \c DATA object may be modified by visiting the children). * call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each * @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */ * root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST> template<class FOREST, typename DATA, typename VISITOR_PRE,
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) typename VISITOR_POST>
{ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
VISITOR_POST& visitorPost) {
// Typedefs // Typedefs
typedef typename FOREST::Node Node; typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode; typedef boost::shared_ptr<Node> sharedNode;
@ -92,12 +95,11 @@ namespace gtsam {
} }
// Traverse // Traverse
while(!stack.empty()) while (!stack.empty()) {
{
// Get next node // Get next node
TraversalNode& node = stack.front(); TraversalNode& node = stack.front();
if(node.expanded) { if (node.expanded) {
// If already expanded, then the data stored in the node is no longer needed, so visit // If already expanded, then the data stored in the node is no longer needed, so visit
// then delete it. // then delete it.
(void) visitorPost(node.treeNode, *node.dataPointer); (void) visitorPost(node.treeNode, *node.dataPointer);
@ -106,7 +108,8 @@ namespace gtsam {
} else { } else {
// If not already visited, visit the node and add its children (use reverse iterators so // If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear) // children are processed in the order they appear)
node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); node.dataPointer = dataList.insert(dataList.end(),
visitorPre(node.treeNode, node.parentData));
typename Stack::iterator insertLocation = stack.begin(); typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& child, node.treeNode->children) BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
@ -114,9 +117,9 @@ namespace gtsam {
} }
} }
assert(dataList.empty()); assert(dataList.empty());
} }
/** Traverse a forest depth-first, with a pre-order visit but no post-order visit. /** Traverse a forest depth-first, with a pre-order visit but no post-order visit.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist * @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node. * and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
@ -127,14 +130,13 @@ namespace gtsam {
* Regarding efficiency, this copy-on-return is usually optimized out by the compiler. * Regarding efficiency, this copy-on-return is usually optimized out by the compiler.
* @param rootData The data to pass by reference to \c visitorPre when it is called on each * @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */ * root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE> template<class FOREST, typename DATA, typename VISITOR_PRE>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
{
no_op visitorPost; no_op visitorPost;
DepthFirstForest(forest, rootData, visitorPre, visitorPost); DepthFirstForest(forest, rootData, visitorPre, visitorPost);
} }
/** Traverse a forest depth-first with pre-order and post-order visits. /** Traverse a forest depth-first with pre-order and post-order visits.
* @param forest The forest of trees to traverse. The method \c forest.roots() should exist * @param forest The forest of trees to traverse. The method \c forest.roots() should exist
* and return a collection of (shared) pointers to \c FOREST::Node. * and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
@ -148,77 +150,80 @@ namespace gtsam {
* call to \c visitorPre (the \c DATA object may be modified by visiting the children). * call to \c visitorPre (the \c DATA object may be modified by visiting the children).
* @param rootData The data to pass by reference to \c visitorPre when it is called on each * @param rootData The data to pass by reference to \c visitorPre when it is called on each
* root node. */ * root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST> template<class FOREST, typename DATA, typename VISITOR_PRE,
void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, typename VISITOR_POST>
int problemSizeThreshold = 10) void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
{ VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
int problemSizeThreshold = 10) {
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
// Typedefs // Typedefs
typedef typename FOREST::Node Node; typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode; typedef boost::shared_ptr<Node> sharedNode;
tbb::task::spawn_root_and_wait(internal::CreateRootTask<Node>( tbb::task::spawn_root_and_wait(
forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
visitorPost, problemSizeThreshold));
#else #else
DepthFirstForest(forest, rootData, visitorPre, visitorPost); DepthFirstForest(forest, rootData, visitorPre, visitorPost);
#endif #endif
} }
/* ************************************************************************* */
/* ************************************************************************* */ /** Traversal function for CloneForest */
/** Traversal function for CloneForest */ namespace {
namespace { template<typename NODE>
template<typename NODE> boost::shared_ptr<NODE> CloneForestVisitorPre(
boost::shared_ptr<NODE> const boost::shared_ptr<NODE>& node,
CloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer) const boost::shared_ptr<NODE>& parentPointer) {
{
// Clone the current node and add it to its cloned parent // Clone the current node and add it to its cloned parent
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node); boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
clone->children.clear(); clone->children.clear();
parentPointer->children.push_back(clone); parentPointer->children.push_back(clone);
return clone; return clone;
} }
} }
/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child
* pointers for a clone of the original tree. * pointers for a clone of the original tree.
* @param forest The forest of trees to clone. The method \c forest.roots() should exist and * @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 a collection of shared pointers to \c FOREST::Node.
* @return The new collection of roots. */ * @return The new collection of roots. */
template<class FOREST> template<class FOREST>
FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest) FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(
{ const FOREST& forest) {
typedef typename FOREST::Node Node; typedef typename FOREST::Node Node;
boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>(); boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>); DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end()); return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(),
} rootContainer->children.end());
}
/* ************************************************************************* */
/* ************************************************************************* */ /** Traversal function for PrintForest */
/** Traversal function for PrintForest */ namespace {
namespace { struct PrintForestVisitorPre {
struct PrintForestVisitorPre
{
const KeyFormatter& formatter; const KeyFormatter& formatter;
PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} PrintForestVisitorPre(const KeyFormatter& formatter) :
template<typename NODE> std::string operator()(const boost::shared_ptr<NODE>& node, const std::string& parentString) formatter(formatter) {
{ }
template<typename NODE> std::string operator()(
const boost::shared_ptr<NODE>& node, const std::string& parentString) {
// Print the current node // Print the current node
node->print(parentString + "-", formatter); node->print(parentString + "-", formatter);
// Increment the indentation // Increment the indentation
return parentString + "| "; return parentString + "| ";
} }
}; };
} }
/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
* To print each node, this function calls the \c print function of the tree nodes. */ * To print each node, this function calls the \c print function of the tree nodes. */
template<class FOREST> template<class FOREST>
void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { void PrintForest(const FOREST& forest, std::string str,
const KeyFormatter& keyFormatter) {
PrintForestVisitorPre visitor(keyFormatter); PrintForestVisitorPre visitor(keyFormatter);
DepthFirstForest(forest, str, visitor); DepthFirstForest(forest, str, visitor);
} }
} }
} }

253
gtsam/inference/ClusterTree-inst.h
View File

@ -7,73 +7,23 @@
* @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree * @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/ClusterTree.h>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
namespace gtsam 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 // Elimination traversal data - stores a pointer to the parent data and collects the factors
// node. // resulting from elimination of the children. Also sets up BayesTree cliques with parent and
template<class CLUSTERTREE> // child pointers.
EliminationData<CLUSTERTREE> eliminationPreOrderVisitor( template<class CLUSTERTREE>
const typename CLUSTERTREE::sharedNode& node, EliminationData<CLUSTERTREE>& parentData) struct EliminationData {
{
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 // Typedefs
typedef typename CLUSTERTREE::sharedFactor sharedFactor; typedef typename CLUSTERTREE::sharedFactor sharedFactor;
typedef typename CLUSTERTREE::FactorType FactorType; typedef typename CLUSTERTREE::FactorType FactorType;
@ -81,6 +31,50 @@ namespace gtsam
typedef typename CLUSTERTREE::ConditionalType ConditionalType; typedef typename CLUSTERTREE::ConditionalType ConditionalType;
typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
EliminationData* const parentData;
size_t myIndexInParent;
FastVector<sharedFactor> childFactors;
boost::shared_ptr<BTNode> bayesTreeNode;
EliminationData(EliminationData* _parentData, size_t nChildren) :
parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>()) {
if (parentData) {
myIndexInParent = parentData->childFactors.size();
parentData->childFactors.push_back(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 - creates the EliminationData structure for the visited node.
static EliminationData EliminationPreOrderVisitor(
const typename CLUSTERTREE::sharedNode& node,
EliminationData& parentData) {
assert(node);
EliminationData 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.
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& myData) {
assert(node);
// Gather factors // Gather factors
FactorGraphType gatheredFactors; FactorGraphType gatheredFactors;
gatheredFactors.reserve(node->factors.size() + node->children.size()); gatheredFactors.reserve(node->factors.size() + node->children.size());
@ -88,17 +82,17 @@ namespace gtsam
gatheredFactors += myData.childFactors; gatheredFactors += myData.childFactors;
// Check for Bayes tree orphan subtrees, and add them to our children // Check for Bayes tree orphan subtrees, and add them to our children
BOOST_FOREACH(const sharedFactor& f, node->factors) BOOST_FOREACH(const sharedFactor& f, node->factors) {
{ if (const BayesTreeOrphanWrapper<BTNode>* asSubtree =
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) {
{
myData.bayesTreeNode->children.push_back(asSubtree->clique); myData.bayesTreeNode->children.push_back(asSubtree->clique);
asSubtree->clique->parent_ = myData.bayesTreeNode; asSubtree->clique->parent_ = myData.bayesTreeNode;
} }
} }
// Do dense elimination step // Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = std::pair<boost::shared_ptr<ConditionalType>,
boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, node->orderedFrontalKeys); eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
@ -111,33 +105,83 @@ namespace gtsam
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode));
// Store remaining factor in parent's gathered factors // Store remaining factor in parent's gathered factors
if(!eliminationResult.second->empty()) if (!eliminationResult.second->empty())
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; myData.parentData->childFactors[myData.myIndexInParent] =
eliminationResult.second;
} }
}; };
} };
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
void ClusterTree<BAYESTREE,GRAPH>::Cluster::print( void ClusterTree<BAYESTREE, GRAPH>::Cluster::print(const std::string& s,
const std::string& s, const KeyFormatter& keyFormatter) const const KeyFormatter& keyFormatter) const {
{ std::cout << s << " (" << problemSize_ << ")";
std::cout << s << " (" << problemSize_ << ")" ;
PrintKeyVector(orderedFrontalKeys); PrintKeyVector(orderedFrontalKeys);
}
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
const std::vector<bool>& merge) {
gttic(Cluster::mergeChildren);
// Count how many keys, factors and children we'll end up with
size_t nrKeys = orderedFrontalKeys.size();
size_t nrFactors = factors.size();
size_t nrNewChildren = 0;
// Loop over children
size_t i = 0;
BOOST_FOREACH(const sharedNode& child, children) {
if (merge[i]) {
nrKeys += child->orderedFrontalKeys.size();
nrFactors += child->factors.size();
nrNewChildren += child->children.size();
} else {
nrNewChildren += 1; // we keep the child
}
++i;
} }
/* ************************************************************************* */ // now reserve space, and really merge
template<class BAYESTREE, class GRAPH> orderedFrontalKeys.reserve(nrKeys);
void ClusterTree<BAYESTREE,GRAPH>::print( factors.reserve(nrFactors);
const std::string& s, const KeyFormatter& keyFormatter) const typename Node::Children newChildren;
{ newChildren.reserve(nrNewChildren);
i = 0;
BOOST_FOREACH(const sharedNode& child, children) {
if (merge[i]) {
// Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
orderedFrontalKeys.insert(orderedFrontalKeys.end(),
child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend());
// Merge keys, factors, and children.
factors.insert(factors.end(), child->factors.begin(),
child->factors.end());
newChildren.insert(newChildren.end(), child->children.begin(),
child->children.end());
// Increment problem size
problemSize_ = std::max(problemSize_, child->problemSize_);
// Increment number of frontal variables
} else {
newChildren.push_back(child); // we keep the child
}
++i;
}
children = newChildren;
std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end());
}
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
void ClusterTree<BAYESTREE, GRAPH>::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
treeTraversal::PrintForest(*this, s, keyFormatter); treeTraversal::PrintForest(*this, s, keyFormatter);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
ClusterTree<BAYESTREE,GRAPH>& ClusterTree<BAYESTREE,GRAPH>::operator=(const This& other) ClusterTree<BAYESTREE, GRAPH>& ClusterTree<BAYESTREE, GRAPH>::operator=(
{ const This& other) {
// Start by duplicating the tree. // Start by duplicating the tree.
roots_ = treeTraversal::CloneForest(other); roots_ = treeTraversal::CloneForest(other);
@ -146,39 +190,44 @@ namespace gtsam
remainingFactors_ = other.remainingFactors_; remainingFactors_ = other.remainingFactors_;
return *this; return *this;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > ClusterTree<
ClusterTree<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const {
{
gttic(ClusterTree_eliminate); gttic(ClusterTree_eliminate);
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node // 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 // that contains all of the roots as its children. rootsContainer also stores the remaining
// uneliminated factors passed up from the roots. // uneliminated factors passed up from the roots.
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>(); boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
EliminationData<This> rootsContainer(0, roots_.size()); typedef EliminationData<This> Data;
EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_); Data rootsContainer(0, roots_.size());
typename Data::EliminationPostOrderVisitor visitorPost(function,
result->nodes_);
{ {
TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
treeTraversal::DepthFirstForestParallel(*this, rootsContainer, treeTraversal::DepthFirstForestParallel(*this, rootsContainer,
eliminationPreOrderVisitor<This>, visitorPost, 10); Data::EliminationPreOrderVisitor, visitorPost, 10);
} }
// Create BayesTree from roots stored in the dummy BayesTree node. // Create BayesTree from roots stored in the dummy BayesTree node.
result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); result->roots_.insert(result->roots_.end(),
rootsContainer.bayesTreeNode->children.begin(),
rootsContainer.bayesTreeNode->children.end());
// Add remaining factors that were not involved with eliminated variables // Add remaining factors that were not involved with eliminated variables
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>(); boost::shared_ptr<FactorGraphType> remaining = boost::make_shared<
allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); FactorGraphType>();
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); remaining->reserve(
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) remainingFactors_.size() + rootsContainer.childFactors.size());
if(factor) remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
allRemainingFactors->push_back(factor); BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) {
if (factor)
// Return result remaining->push_back(factor);
return std::make_pair(result, allRemainingFactors);
} }
// Return result
return std::make_pair(result, remaining);
}
} }

46
gtsam/inference/ClusterTree.h
View File

@ -13,19 +13,17 @@
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
namespace gtsam namespace gtsam {
{
/** /**
* A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: * 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 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$. * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$.
* \nosubgrouping * \nosubgrouping
*/ */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
class ClusterTree class ClusterTree {
{ public:
public:
typedef GRAPH FactorGraphType; ///< The factor graph type typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef ClusterTree<BAYESTREE, GRAPH> This; ///< This class typedef ClusterTree<BAYESTREE, GRAPH> This; ///< This class
@ -41,8 +39,10 @@ namespace gtsam
typedef FastVector<sharedFactor> Factors; typedef FastVector<sharedFactor> Factors;
typedef FastVector<boost::shared_ptr<Cluster> > Children; typedef FastVector<boost::shared_ptr<Cluster> > Children;
Cluster() {} Cluster() {
Cluster(Key key, const Factors& factors) : factors(factors) { }
Cluster(Key key, const Factors& factors) :
factors(factors) {
orderedFrontalKeys.push_back(key); orderedFrontalKeys.push_back(key);
} }
@ -51,10 +51,16 @@ namespace gtsam
Children children; ///< sub-trees Children children; ///< sub-trees
int problemSize_; int problemSize_;
int problemSize() const { return problemSize_; } int problemSize() const {
return problemSize_;
}
/** print this node */ /// print this node
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// Merge all children for which bit is set into this node
void mergeChildren(const std::vector<bool>& merge);
}; };
typedef boost::shared_ptr<Cluster> sharedCluster; ///< Shared pointer to Cluster typedef boost::shared_ptr<Cluster> sharedCluster; ///< Shared pointer to Cluster
@ -64,7 +70,7 @@ namespace gtsam
/** concept check */ /** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
protected: protected:
FastVector<sharedNode> roots_; FastVector<sharedNode> roots_;
FastVector<sharedFactor> remainingFactors_; FastVector<sharedFactor> remainingFactors_;
@ -73,11 +79,11 @@ namespace gtsam
/** 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. */ * copied, factors are not cloned. */
ClusterTree(const This& other) { *this = other; } ClusterTree(const This& other) {*this = other;}
/// @} /// @}
public: public:
/// @name Testable /// @name Testable
/// @{ /// @{
@ -103,14 +109,14 @@ namespace gtsam
/// @{ /// @{
/** Return the set of roots (one for a tree, multiple for a forest) */ /** Return the set of roots (one for a tree, multiple for a forest) */
const FastVector<sharedNode>& roots() const { return roots_; } const FastVector<sharedNode>& roots() const {return roots_;}
/** Return the remaining factors that are not pulled into elimination */ /** Return the remaining factors that are not pulled into elimination */
const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; } const FastVector<sharedFactor>& remainingFactors() const {return remainingFactors_;}
/// @} /// @}
protected: protected:
/// @name Details /// @name Details
/// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors
@ -122,9 +128,7 @@ namespace gtsam
/// @} /// @}
}; };
} }

95
gtsam/inference/JunctionTree-inst.h
View File

@ -27,7 +27,7 @@
namespace gtsam { namespace gtsam {
template <class BAYESTREE, class GRAPH, class ETREE_NODE> template<class BAYESTREE, class GRAPH, class ETREE_NODE>
struct ConstructorTraversalData { struct ConstructorTraversalData {
typedef typename JunctionTree<BAYESTREE, GRAPH>::Node Node; typedef typename JunctionTree<BAYESTREE, GRAPH>::Node Node;
typedef typename JunctionTree<BAYESTREE, GRAPH>::sharedNode sharedNode; typedef typename JunctionTree<BAYESTREE, GRAPH>::sharedNode sharedNode;
@ -37,8 +37,13 @@ struct ConstructorTraversalData {
FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals; FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors; FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
ConstructorTraversalData(ConstructorTraversalData* _parentData) // Small inner class to store symbolic factors
: parentData(_parentData) {} class SymbolicFactors: public FactorGraph<Factor> {
};
ConstructorTraversalData(ConstructorTraversalData* _parentData) :
parentData(_parentData) {
}
// Pre-order visitor function // Pre-order visitor function
static ConstructorTraversalData ConstructorTraversalVisitorPre( static ConstructorTraversalData ConstructorTraversalVisitorPre(
@ -64,13 +69,12 @@ struct ConstructorTraversalData {
// our number of symbolic elimination parents is exactly 1 less than // our number of symbolic elimination parents is exactly 1 less than
// our child's symbolic elimination parents - this condition indicates that // our child's symbolic elimination parents - this condition indicates that
// eliminating the current node did not introduce any parents beyond those // eliminating the current node did not introduce any parents beyond those
// already in the child. // already in the child->
// Do symbolic elimination for this node // Do symbolic elimination for this node
class : public FactorGraph<Factor> {} SymbolicFactors symbolicFactors;
symbolicFactors; symbolicFactors.reserve(
symbolicFactors.reserve(ETreeNode->factors.size() + ETreeNode->factors.size() + myData.childSymbolicFactors.size());
myData.childSymbolicFactors.size());
// Add ETree node factors // Add ETree node factors
symbolicFactors += ETreeNode->factors; symbolicFactors += ETreeNode->factors;
// Add symbolic factors passed up from children // Add symbolic factors passed up from children
@ -78,60 +82,47 @@ struct ConstructorTraversalData {
Ordering keyAsOrdering; Ordering keyAsOrdering;
keyAsOrdering.push_back(ETreeNode->key); keyAsOrdering.push_back(ETreeNode->key);
std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr> SymbolicConditional::shared_ptr myConditional;
symbolicElimResult = SymbolicFactor::shared_ptr mySeparatorFactor;
internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(
symbolicFactors, keyAsOrdering);
// Store symbolic elimination results in the parent // Store symbolic elimination results in the parent
myData.parentData->childSymbolicConditionals.push_back( myData.parentData->childSymbolicConditionals.push_back(myConditional);
symbolicElimResult.first); myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
myData.parentData->childSymbolicFactors.push_back(
symbolicElimResult.second);
sharedNode node = myData.myJTNode; sharedNode node = myData.myJTNode;
const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
myData.childSymbolicConditionals;
node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size());
// Merge our children if they are in our clique - if our conditional has // Merge our children if they are in our clique - if our conditional has
// exactly one fewer parent than our child's conditional. // exactly one fewer parent than our child's conditional.
size_t myNrFrontals = 1; const size_t myNrParents = myConditional->nrParents();
const size_t myNrParents = symbolicElimResult.first->nrParents(); const size_t nrChildren = node->children.size();
size_t nrMergedChildren = 0; assert(childConditionals.size() == nrChildren);
assert(node->children.size() == myData.childSymbolicConditionals.size());
// Loop over children // decide which children to merge, as index into children
int combinedProblemSize = std::vector<bool> merge(nrChildren, false);
(int)(symbolicElimResult.first->size() * symbolicFactors.size()); size_t myNrFrontals = 1, i = 0;
gttic(merge_children); BOOST_FOREACH(const sharedNode& child, node->children) {
for (size_t i = 0; i < myData.childSymbolicConditionals.size(); ++i) {
// Check if we should merge the i^th child // Check if we should merge the i^th child
if (myNrParents + myNrFrontals == if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
myData.childSymbolicConditionals[i]->nrParents()) {
// Get a reference to the i, adjusting the index to account for children
// previously merged and removed from the i list.
const Node& child = *node->children[i - nrMergedChildren];
// Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(),
child.orderedFrontalKeys.rbegin(),
child.orderedFrontalKeys.rend());
// Merge keys, factors, and children.
node->factors.insert(node->factors.end(), child.factors.begin(), child.factors.end());
node->children.insert(node->children.end(), child.children.begin(), child.children.end());
// Increment problem size
combinedProblemSize = std::max(combinedProblemSize, child.problemSize_);
// Increment number of frontal variables // Increment number of frontal variables
myNrFrontals += child.orderedFrontalKeys.size(); myNrFrontals += child->orderedFrontalKeys.size();
// Remove i from list. merge[i] = true;
node->children.erase(node->children.begin() + (i - nrMergedChildren));
// Increment number of merged children
++nrMergedChildren;
} }
++i;
} }
std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end());
gttoc(merge_children); // now really merge
node->problemSize_ = combinedProblemSize; node->mergeChildren(merge);
} }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template <class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
template <class ETREE_BAYESNET, class ETREE_GRAPH> template<class ETREE_BAYESNET, class ETREE_GRAPH>
JunctionTree<BAYESTREE, GRAPH>::JunctionTree( JunctionTree<BAYESTREE, GRAPH>::JunctionTree(
const EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>& eliminationTree) { const EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>& eliminationTree) {
gttic(JunctionTree_FromEliminationTree); gttic(JunctionTree_FromEliminationTree);
@ -147,15 +138,17 @@ JunctionTree<BAYESTREE, GRAPH>::JunctionTree(
typedef typename EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>::Node ETreeNode; typedef typename EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>::Node ETreeNode;
typedef ConstructorTraversalData<BAYESTREE, GRAPH, ETreeNode> Data; typedef ConstructorTraversalData<BAYESTREE, GRAPH, ETreeNode> Data;
Data rootData(0); Data rootData(0);
rootData.myJTNode = rootData.myJTNode = boost::make_shared<typename Base::Node>(); // Make a dummy node to gather
boost::make_shared<typename Base::Node>(); // Make a dummy node to gather
// the junction tree roots // the junction tree roots
treeTraversal::DepthFirstForest(eliminationTree, rootData, treeTraversal::DepthFirstForest(eliminationTree, rootData,
Data::ConstructorTraversalVisitorPre, Data::ConstructorTraversalVisitorPre,
Data::ConstructorTraversalVisitorPostAlg2); Data::ConstructorTraversalVisitorPostAlg2);
// Assign roots from the dummy node // Assign roots from the dummy node
Base::roots_ = rootData.myJTNode->children; typedef typename JunctionTree<BAYESTREE, GRAPH>::Node Node;
const typename Node::Children& children = rootData.myJTNode->children;
Base::roots_.reserve(children.size());
Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end());
// Transfer remaining factors from elimination tree // Transfer remaining factors from elimination tree
Base::remainingFactors_ = eliminationTree.remainingFactors(); Base::remainingFactors_ = eliminationTree.remainingFactors();