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

Converted elimination and printing of elimination trees to use generic DFS code

release/4.3a0
Richard Roberts 2013-06-06 15:36:20 +00:00
parent df728a969c
commit 5b1ac91c85
2 changed files with 40 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,7 +56,7 @@ namespace gtsam {
void no_op(const NODE& node, const DATA& data) {}
}
/** Traverse a forest depth-first.
/** 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
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
@ -70,9 +70,8 @@ namespace gtsam {
* 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
* root node. */
template<class FOREST, typename VISITOR_PRE, typename VISITOR_POST, typename DATA>
void DepthFirstForest(FOREST& forest, DATA& rootData,
VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost = no_op<typename FOREST::Node, DATA>)
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost)
{
// Depth first traversal stack
typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
@ -83,7 +82,7 @@ namespace gtsam {
// Add roots to stack (use reverse iterators so children are processed in the order they
// appear)
(void) std::for_each(forest.roots().rbegin(), forest.roots().rend(),
Expander(visitor, &rootData, stack));
Expander(visitorPre, &rootData, stack));
// Traverse
while(!stack.empty())
@ -105,6 +104,25 @@ namespace gtsam {
}
}
}
/** 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
* and return a collection of (shared) pointers to \c FOREST::Node.
* @param visitorPre \c visitorPre(node, parentData) will be called at every node, before
* visiting its children, and will be passed, by reference, the \c DATA object returned
* by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object
* to pass to the children. The returned \c DATA object will be copy-constructed only
* upon returning to store internally, thus may be modified by visiting the children.
* 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
* root node. */
template<class FOREST, typename DATA, typename VISITOR_PRE>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre)
{
DepthFirstForest<FOREST, DATA, VISITOR_PRE, void(&)(const typename FOREST::Node&, const DATA&)>(
forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
}
}
}

88
gtsam/inference/inference-inst.h
View File

@ -27,21 +27,9 @@
namespace gtsam {
namespace inference {
/* ************************************************************************* */
namespace {
template<class ELIMINATIONTREE>
struct EliminationNode {
bool expanded;
const typename ELIMINATIONTREE::Node* const treeNode;
std::vector<typename ELIMINATIONTREE::sharedFactor> childrenFactors;
EliminationNode<ELIMINATIONTREE>* const parent;
EliminationNode(const typename ELIMINATIONTREE::Node* _treeNode, EliminationNode<ELIMINATIONTREE>* _parent) :
expanded(false), treeNode(_treeNode), parent(_parent) {
childrenFactors.reserve(treeNode->children.size()); }
};
/* ************************************************************************* */
template<TREE>
template<class TREE>
struct EliminationData {
EliminationData* const parentData;
std::vector<typename TREE::sharedFactor> childFactors;
@ -50,22 +38,22 @@ namespace gtsam {
};
/* ************************************************************************* */
template<TREE>
template<class TREE>
EliminationData<TREE> eliminationPreOrderVisitor(
const typename TREE::sharedNode& node, EliminationData<TREE>* parentData)
const typename TREE::sharedNode& node, EliminationData<TREE>& parentData)
{
// This function is called before visiting the children. Here, we create this node's data,
// which includes a pointer to the parent data and space for the factors of the children.
return EliminationData<TREE>(parentData, node->children.size());
return EliminationData<TREE>(&parentData, node->children.size());
}
/* ************************************************************************* */
template<TREE, RESULT>
void eliminationPostOrderVisitor(const TREE::Node* const node, EliminationData<TREE>& myData,
template<class TREE, class RESULT>
void eliminationPostOrderVisitor(const typename TREE::Node& const node, EliminationData<TREE>& myData,
RESULT& result, const typename TREE::Eliminate& eliminationFunction)
{
// Call eliminate on the node and add the result to the parent's gathered factors
myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors));
myData.parentData->childFactors.push_back(node.eliminate(result, eliminationFunction, myData.childFactors));
}
}
@ -81,60 +69,18 @@ namespace gtsam {
typedef typename TREE::sharedNode sharedNode;
typedef typename TREE::sharedFactor sharedFactor;
// Allocate remaining factors
std::vector<sharedFactor> remainingFactors;
remainingFactors.reserve(tree.roots().size());
treeTraversal::DepthFirstForest(tree, remainingFactors, )
// Stack for eliminating nodes. We use this stack instead of recursive function calls to
// avoid call stack overflow due to very long trees that arise from chain-like graphs.
// TODO: Check whether this is faster as a vector (then use indices instead of parent pointers).
typedef EliminationNode<TREE> EliminationNode;
std::stack<EliminationNode, FastList<EliminationNode> > eliminationStack;
// Allocate remaining factors
std::vector<sharedFactor> remainingFactors;
remainingFactors.reserve(tree.roots().size());
// Add roots to the stack (use reverse foreach so conditionals to appear in elimination order -
// doesn't matter for computation but can make printouts easier to interpret by hand).
BOOST_REVERSE_FOREACH(const sharedNode& root, tree.roots()) {
eliminationStack.push(
EliminationNode(root.get(), 0)); }
// Until the stack is empty
while(!eliminationStack.empty()) {
// Process the next node. If it has children, add its children to the stack and mark it
// expanded - we'll come back and eliminate it later after the children have been processed.
EliminationNode& node = eliminationStack.top();
if(node.expanded)
{
// Do elimination step
sharedFactor remainingFactor = node.treeNode->eliminate(result, function, node.childrenFactors);
// TODO: Don't add null factor?
if(node.parent)
node.parent->childrenFactors.push_back(remainingFactor);
else
remainingFactors.push_back(remainingFactor);
// Remove from stack
eliminationStack.pop();
} else
{
// Expand children and mark as expanded (use reverse foreach so conditionals to appear in
// elimination order - doesn't matter for computation but can make printouts easier to
// interpret by hand).
node.expanded = true;
BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->children) {
eliminationStack.push(
EliminationNode(child.get(), &node)); }
}
}
// Do elimination using a depth-first traversal. During the pre-order visit (see
// eliminationPreOrderVisitor), we store a pointer to the parent data (where we'll put the
// remaining factor) and reserve a vector of factors to store the children elimination
// results. During the post-order visit (see eliminationPostOrderVisitor), we call dense
// elimination (using the gathered child factors) and store the result in the parent's
// gathered factors.
EliminationData<TREE> rootData(0, tree.roots().size());
treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor<TREE>,
boost::bind(eliminationPostOrderVisitor<TREE,RESULT>, _1, _2, result, function));
// Return remaining factors
return remainingFactors;
return rootData.childFactors;
}
}