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

In the middle of converting to generic DFS code but committing to save history

release/4.3a0
Richard Roberts 2013-06-06 15:36:18 +00:00
parent 7dfd999673
commit df728a969c
17 changed files with 798 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
gtsam/base/treeTraversal-inst.h Normal file
View File

@ -0,0 +1,110 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file treeTraversal-inst.h
* @author Richard Roberts
* @date April 9, 2013
*/
#pragma once
#include <gtsam/base/FastList.h>
namespace gtsam {
/** Internal functions used for traversing trees */
namespace treeTraversal {
namespace {
// Internal node used in DFS preorder stack
template<typename NODE, typename DATA>
struct TraversalNode {
bool expanded;
NODE* const treeNode;
DATA data;
TraversalNode(NODE* _treeNode, const DATA& _data) :
expanded(false), treeNode(_treeNode), data(_data) {}
};
// Functional object to visit a node and then add it to the traversal stack
template<typename NODE, typename VISITOR, typename DATA, typename STACK>
struct PreOrderExpand {
VISITOR& visitor_;
DATA* parentData_;
STACK& stack_;
PreOrderExpand(VISITOR& visitor, DATA* parentData, STACK& stack) :
visitor_(visitor), parentData_(parentData), stack_(stack) {}
template<typename P>
void operator()(const P& nodePointer) {
// Get raw pointer (in case nodePointer is a shared_ptr)
NODE* rawNodePointer = &(*nodePointer);
// Add node
stack_.push(TraversalNode<NODE,DATA>(rawNodePointer, visitor_(nodePointer, *parentData_)));
}
};
/// Do nothing - default argument for post-visitor for tree traversal
template<typename NODE, typename DATA>
void no_op(const NODE& node, const DATA& data) {}
}
/** Traverse a forest depth-first.
* @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 visitorPost \c visitorPost(node, data) will be called at every node, after visiting
* its children, and will be passed, by reference, the \c DATA object returned by the
* 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>)
{
// Depth first traversal stack
typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
typedef std::stack<TraversalNode, FastList<TraversalNode> > Stack;
typedef PreOrderExpand<typename FOREST::Node, VISITOR_PRE, DATA, Stack> Expander;
Stack stack;
// 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));
// Traverse
while(!stack.empty())
{
// Get next node
TraversalNode& node = stack.top();
if(node.expanded) {
// If already expanded, then the data stored in the node is no longer needed, so visit
// then delete it.
(void) visitorPost(*node.treeNode, node.data);
stack.pop();
} else {
// If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear)
(void) std::for_each(node.treeNode->children.rbegin(), node.treeNode->children.rend(),
Expander(visitorPre, &node.data, stack));
node.expanded = true;
}
}
}
}
}

11
gtsam/dllexport.h
View File

@ -16,6 +16,16 @@
* @date Mar 9, 2013
*/
// Macros for exporting DLL symbols on Windows
// Usage example:
// In header file:
// class GTSAM_EXPORT MyClass { ... };
//
// Results in the following declarations:
// When included while compiling the GTSAM library itself:
// class __declspec(dllexport) MyClass { ... };
// When included while compiling other code against GTSAM:
// class __declspec(dllimport) MyClass { ... };
#ifdef _WIN32
# ifdef GTSAM_EXPORTS
# define GTSAM_EXPORT __declspec(dllexport)
@ -33,4 +43,3 @@
# define GTSAM_EXPORT
# define GTSAM_EXTERN_EXPORT extern
#endif

41
gtsam/inference/ConditionalUnordered-inst.h Normal file
View File

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Conditional.h
* @brief Base class for conditional densities
* @author Frank Dellaert
*/
// \callgraph
#pragma once
#include <boost/foreach.hpp>
#include <iostream>
#include <gtsam/inference/ConditionalUnordered.h>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR>
void ConditionalUnordered<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
BOOST_FOREACH(Key parent, parents())
std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}
}

15
gtsam/inference/ConditionalUnordered.h
View File

@ -19,8 +19,6 @@
#pragma once
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <iostream>
#include <gtsam/inference/Key.h>
@ -152,17 +150,4 @@ namespace gtsam {
};
/* ************************************************************************* */
template<class FACTOR, class DERIVEDFACTOR>
void ConditionalUnordered<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
BOOST_FOREACH(Key parent, parents())
std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}
} // gtsam

156
gtsam/inference/EliminationTreeUnordered-inl.h → gtsam/inference/EliminationTreeUnordered-inst.h
View File

@ -18,14 +18,46 @@
#pragma once
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/bind.hpp>
#include <stack>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/inference-inst.h>
namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
typename EliminationTreeUnordered<BAYESNET,GRAPH>::sharedFactor
EliminationTreeUnordered<BAYESNET,GRAPH>::Node::eliminate(
const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
{
assert(childrenResults.size() == children.size());
// Gather factors
std::vector<sharedFactor> gatheredFactors;
gatheredFactors.reserve(factors.size() + children.size());
gatheredFactors.insert(gatheredFactors.end(), factors.begin(), factors.end());
gatheredFactors.insert(gatheredFactors.end(), childrenResults.begin(), childrenResults.end());
// Do dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(gatheredFactors, keyAsVector);
// Add conditional to BayesNet
output->push_back(eliminationResult.first);
// Return result
return eliminationResult.second;
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(const FactorGraphType& graph,
@ -51,7 +83,7 @@ namespace gtsam {
for (size_t j = 0; j < n; j++)
{
// Retrieve the factors involving this variable and create the current node
const VariableIndex::Factors& factors = structure[order[j]];
const VariableIndexUnordered::Factors& factors = structure[order[j]];
nodes[j] = boost::make_shared<Node>();
nodes[j]->key = order[j];
@ -74,7 +106,7 @@ namespace gtsam {
if (r != j) {
// Now that we found the root, hook up parent and child pointers in the nodes.
parents[r] = j;
nodes[j]->subTrees.push_back(nodes[r]);
nodes[j]->children.push_back(nodes[r]);
}
} else {
// Add the current factor to the current node since we are at the first variable in this
@ -136,7 +168,7 @@ namespace gtsam {
while(!stack.empty()) {
sharedNode node = stack.top();
stack.pop();
BOOST_FOREACH(sharedNode& child, node->subTrees) {
BOOST_FOREACH(sharedNode& child, node->children) {
// Important: since we are working with a *reference* to a shared pointer in this
// BOOST_FOREACH loop, the next statement modifies the pointer in the current node's child
// list - it replaces it with a pointer to a copy of the child.
@ -152,112 +184,46 @@ namespace gtsam {
return *this;
}
/* ************************************************************************* */
namespace {
template<class ELIMINATIONTREE>
struct EliminationNode {
bool expanded;
const typename ELIMINATIONTREE::Node* const treeNode;
std::vector<typename ELIMINATIONTREE::sharedFactor> factors;
EliminationNode<ELIMINATIONTREE>* const parent;
template<typename ITERATOR> EliminationNode(const typename ELIMINATIONTREE::Node* _treeNode, size_t nFactorsToReserve,
ITERATOR firstFactor, ITERATOR lastFactor, EliminationNode<ELIMINATIONTREE>* _parent) :
expanded(false), treeNode(_treeNode), parent(_parent) {
factors.reserve(nFactorsToReserve);
factors.insert(factors.end(), firstFactor, lastFactor);
}
};
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
EliminationTreeUnordered<BAYESNET,GRAPH>::eliminate(Eliminate function) const
{
// 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<This> EliminationNode;
std::stack<EliminationNode, FastList<EliminationNode> > eliminationStack;
// Allocate result
boost::shared_ptr<BayesNetType> result = boost::make_shared<BayesNetType>();
// Create empty Bayes net and factor graph to hold result
boost::shared_ptr<BayesNetType> bayesNet = boost::make_shared<BayesNetType>();
// Initialize remaining factors with the factors remaining from creation of the
// EliminationTree - these are the factors that were not included in the partial elimination
// at all.
boost::shared_ptr<FactorGraphType> remainingFactors =
boost::make_shared<FactorGraphType>(remainingFactors_.begin(), remainingFactors_.end());
// Run tree elimination algorithm
std::vector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
// 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, roots_) {
eliminationStack.push(
EliminationNode(root.get(), root->factors.size() + root->subTrees.size(),
root->factors.begin(), root->factors.end(), 0)); }
// Add remaining factors that were not involved with eliminated variables
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
// 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 a dense elimination step
std::vector<Key> keyAsVector(1); keyAsVector[0] = node.treeNode->key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(node.factors, keyAsVector);
// Return result
return std::make_pair(result, allRemainingFactors);
}
// Add conditional to BayesNet and remaining factor to parent
bayesNet->push_back(eliminationResult.first);
// TODO: Don't add null factor?
if(node.parent)
node.parent->factors.push_back(eliminationResult.second);
/* ************************************************************************* */
namespace {
template<class ETREE>
std::string printVisitor(const typename ETREE::sharedNode& node, const std::string& myString, const KeyFormatter& formatter) {
std::cout << myString << "-(" << formatter(node->key) << ")\n";
BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) {
if(factor)
factor->print(myString + "| ");
else
remainingFactors->push_back(eliminationResult.second);
// 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->subTrees) {
eliminationStack.push(
EliminationNode(child.get(), child->factors.size() + child->subTrees.size(),
child->factors.begin(), child->factors.end(), &node)); }
std::cout << myString << "| null factor\n";
}
return myString + "| ";
}
// Return results
return std::make_pair(bayesNet, remainingFactors);
}
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
void EliminationTreeUnordered<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
{
// Depth-first-traversal stack
std::stack<std::pair<sharedNode, std::string> > stack;
// Add roots
BOOST_FOREACH(const sharedNode& node, roots_) { stack.push(std::make_pair(node, " ")); }
// Traverse
while(!stack.empty()) {
std::pair<sharedNode,string> node_string = stack.top();
stack.pop();
std::cout << node_string.second << " (" << formatter(node_string.first->key) << ")\n";
BOOST_FOREACH(const sharedFactor& factor, node_string.first->factors) {
if(factor)
factor->print(node_string.second + " ");
else
std::cout << node_string.second << " null factor\n";
}
BOOST_FOREACH(const sharedNode& child, node_string.first->subTrees) {
stack.push(std::make_pair(child, node_string.second + " "));
}
}
treeTraversal::DepthFirstForest(*this, name, boost::bind(&printVisitor<This>, _1, _2, formatter));
}
/* ************************************************************************* */
@ -270,13 +236,13 @@ namespace gtsam {
// Add roots in sorted order
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(make_pair(root->key, root)); }
BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
}
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(make_pair(root->key, root)); }
BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
}
@ -310,13 +276,13 @@ namespace gtsam {
// Add children in sorted order
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& node, node1->subTrees) { keys.insert(make_pair(node->key, node)); }
BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
}
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& node, node2->subTrees) { keys.insert(make_pair(node->key, node)); }
BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); }
typedef FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
}

27
gtsam/inference/EliminationTreeUnordered.cpp
View File

@ -1,27 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file EliminationTree.cpp
* @author Frank Dellaert
* @author Richard Roberts
* @date Oct 13, 2010
*/
#include <gtsam/inference/EliminationTreeUnordered.h>
namespace gtsam {
namespace internal {
}
}

26
gtsam/inference/EliminationTreeUnordered.h
View File

@ -26,7 +26,6 @@
#include <gtsam/inference/Key.h>
class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
namespace { template<class ELIMINATIONTREE> struct EliminationNode; }
namespace gtsam {
@ -56,7 +55,7 @@ namespace gtsam {
typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef EliminationTreeUnordered<BAYESNET,GRAPH> This; ///< This class
typedef EliminationTreeUnordered<BAYESNET, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR
@ -65,20 +64,22 @@ namespace gtsam {
typedef boost::function<std::pair<sharedConditional,sharedFactor>(std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
private:
class Node {
public:
struct Node {
typedef FastList<sharedFactor> Factors;
typedef FastList<boost::shared_ptr<Node> > SubTrees;
typedef FastList<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root
Factors factors; ///< factors associated with root
SubTrees subTrees; ///< sub-trees
Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
private:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
@ -140,6 +141,10 @@ namespace gtsam {
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
const FastList<sharedNode>& roots() const { return roots_; }
protected:
/// Protected default constructor
@ -148,11 +153,6 @@ namespace gtsam {
private:
/// Allow access to constructor and add methods for testing purposes
friend class ::EliminationTreeUnorderedTester;
friend struct EliminationNode<This>;
};
}
#include <gtsam/inference/EliminationTreeUnordered-inl.h>

1
gtsam/inference/FactorGraphUnordered-inl.h → gtsam/inference/FactorGraphUnordered-inst.h
View File

@ -23,6 +23,7 @@
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraphUnordered.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>

2
gtsam/inference/FactorGraphUnordered.h
View File

@ -228,5 +228,3 @@ namespace gtsam {
}; // FactorGraph
} // namespace gtsam
#include <gtsam/inference/FactorGraphUnordered-inl.h>

203
gtsam/inference/JunctionTreeUnordered-inst.h Normal file
View File

@ -0,0 +1,203 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file JunctionTree-inl.h
* @date Feb 4, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief The junction tree, template bodies
*/
#pragma once
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
void JunctionTree<FG,BTCLIQUE>::construct(const FG& fg, const VariableIndex& variableIndex) {
gttic(JT_symbolic_ET);
const typename EliminationTree<IndexFactor>::shared_ptr symETree =
EliminationTree<IndexFactor>::Create(fg, variableIndex);
assert(symETree.get());
gttoc(JT_symbolic_ET);
gttic(JT_symbolic_eliminate);
SymbolicBayesNet::shared_ptr sbn = symETree->eliminate(&EliminateSymbolic);
assert(sbn.get());
gttoc(JT_symbolic_eliminate);
gttic(symbolic_BayesTree);
SymbolicBayesTree sbt(*sbn);
gttoc(symbolic_BayesTree);
// distribute factors
gttic(distributeFactors);
this->root_ = distributeFactors(fg, sbt.root());
gttoc(distributeFactors);
}
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg) {
gttic(VariableIndex);
VariableIndex varIndex(fg);
gttoc(VariableIndex);
construct(fg, varIndex);
}
/* ************************************************************************* */
template <class FG, class BTCLIQUE>
JunctionTree<FG,BTCLIQUE>::JunctionTree(const FG& fg, const VariableIndex& variableIndex) {
construct(fg, variableIndex);
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(
const FG& fg, const SymbolicBayesTree::sharedClique& bayesClique) {
// Build "target" index. This is an index for each variable of the factors
// that involve this variable as their *lowest-ordered* variable. For each
// factor, it is the lowest-ordered variable of that factor that pulls the
// factor into elimination, after which all of the information in the
// factor is contained in the eliminated factors that are passed up the
// tree as elimination continues.
// Two stages - first build an array of the lowest-ordered variable in each
// factor and find the last variable to be eliminated.
std::vector<Index> lowestOrdered(fg.size(), std::numeric_limits<Index>::max());
Index maxVar = 0;
for(size_t i=0; i<fg.size(); ++i)
if(fg[i]) {
typename FG::FactorType::const_iterator min = std::min_element(fg[i]->begin(), fg[i]->end());
if(min != fg[i]->end()) {
lowestOrdered[i] = *min;
maxVar = std::max(maxVar, *min);
}
}
// Now add each factor to the list corresponding to its lowest-ordered
// variable.
std::vector<FastList<size_t> > targets(maxVar+1);
for(size_t i=0; i<lowestOrdered.size(); ++i)
if(lowestOrdered[i] != std::numeric_limits<Index>::max())
targets[lowestOrdered[i]].push_back(i);
// Now call the recursive distributeFactors
return distributeFactors(fg, targets, bayesClique);
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
typename JunctionTree<FG,BTCLIQUE>::sharedClique JunctionTree<FG,BTCLIQUE>::distributeFactors(const FG& fg,
const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& bayesClique) {
if(bayesClique) {
// create a new clique in the junction tree
sharedClique clique(new Clique((*bayesClique)->beginFrontals(), (*bayesClique)->endFrontals(),
(*bayesClique)->beginParents(), (*bayesClique)->endParents()));
// count the factors for this cluster to pre-allocate space
{
size_t nFactors = 0;
BOOST_FOREACH(const Index frontal, clique->frontal) {
// There may be less variables in "targets" than there really are if
// some of the highest-numbered variables do not pull in any factors.
if(frontal < targets.size())
nFactors += targets[frontal].size(); }
clique->reserve(nFactors);
}
// add the factors to this cluster
BOOST_FOREACH(const Index frontal, clique->frontal) {
if(frontal < targets.size()) {
BOOST_FOREACH(const size_t factorI, targets[frontal]) {
clique->push_back(fg[factorI]); } } }
// recursively call the children
BOOST_FOREACH(const typename SymbolicBayesTree::sharedClique bayesChild, bayesClique->children()) {
sharedClique child = distributeFactors(fg, targets, bayesChild);
clique->addChild(child);
child->parent() = clique;
}
return clique;
} else
return sharedClique();
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
std::pair<typename JunctionTree<FG,BTCLIQUE>::BTClique::shared_ptr,
typename FG::sharedFactor> JunctionTree<FG,BTCLIQUE>::eliminateOneClique(
typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& current) const {
FG fg; // factor graph will be assembled from local factors and marginalized children
fg.reserve(current->size() + current->children().size());
fg.push_back(*current); // add the local factors
// receive the factors from the child and its clique point
std::list<typename BTClique::shared_ptr> children;
BOOST_FOREACH(const boost::shared_ptr<const Clique>& child, current->children()) {
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> tree_factor(
eliminateOneClique(function, child));
children.push_back(tree_factor.first);
fg.push_back(tree_factor.second);
}
// eliminate the combined factors
// warning: fg is being eliminated in-place and will contain marginal afterwards
// Now that we know which factors and variables, and where variables
// come from and go to, create and eliminate the new joint factor.
gttic(CombineAndEliminate);
typename FG::EliminationResult eliminated(function(fg,
current->frontal.size()));
gttoc(CombineAndEliminate);
assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin()));
gttic(Update_tree);
// create a new clique corresponding the combined factors
typename BTClique::shared_ptr new_clique(BTClique::Create(eliminated));
new_clique->children_ = children;
BOOST_FOREACH(typename BTClique::shared_ptr& childRoot, children) {
childRoot->parent_ = new_clique;
}
gttoc(Update_tree);
return std::make_pair(new_clique, eliminated.second);
}
/* ************************************************************************* */
template<class FG, class BTCLIQUE>
typename BTCLIQUE::shared_ptr JunctionTree<FG,BTCLIQUE>::eliminate(
typename FG::Eliminate function) const {
if (this->root()) {
gttic(JT_eliminate);
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor> ret =
this->eliminateOneClique(function, this->root());
if (ret.second->size() != 0) throw std::runtime_error(
"JuntionTree::eliminate: elimination failed because of factors left over!");
gttoc(JT_eliminate);
return ret.first;
} else
return typename BTClique::shared_ptr();
}
} //namespace gtsam

172
gtsam/inference/JunctionTreeUnordered.h Normal file
View File

@ -0,0 +1,172 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file JunctionTree.h
* @date Feb 4, 2010
* @author Kai Ni
* @author Frank Dellaert
* @brief: The junction tree
*/
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
template<class BAYESTREE, class GRAPH>
class JunctionTreeUnordered {
public:
typedef GRAPH FactorGraphType; ///< The factor graph type
typedef typename GRAPH::FactorType FactorType; ///< The type of factors
typedef JunctionTreeUnordered<BAYESTREE, GRAPH> This; ///< This class
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
typedef typename boost::shared_ptr<FactorType> sharedFactor; ///< Shared pointer to a factor
typedef BAYESTREE BayesTreeType; ///< The BayesNet corresponding to FACTOR
typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals
typedef typename boost::shared_ptr<ConditionalType> sharedConditional; ///< Shared pointer to a conditional
typedef boost::function<std::pair<sharedConditional,sharedFactor>(std::vector<sharedFactor>, std::vector<Key>)>
Eliminate; ///< Typedef for an eliminate subroutine
struct Node {
typedef FastList<sharedFactor> Factors;
typedef FastList<boost::shared_ptr<Node> > Children;
Key key; ///< key associated with root
Factors factors; ///< factors associated with root
Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
private:
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
FastList<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
public:
};
/**
* 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
* clique of variables that are eliminated all at once. In detail, every node k represents
* a clique (maximal fully connected subset) of an associated chordal graph, such as a
* chordal Bayes net resulting from elimination.
*
* The difference with the BayesTree is that a JunctionTree stores factors, whereas a
* BayesTree stores conditionals, that are the product of eliminating the factors in the
* corresponding JunctionTree cliques.
*
* The tree structure and elimination method are exactly analagous to the EliminationTree,
* except that in the JunctionTree, at each node multiple variables are eliminated at a time.
*
*
* \addtogroup Multifrontal
* \nosubgrouping
*/
template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
class JunctionTree: public ClusterTree<FG> {
public:
/// In a junction tree each cluster is associated with a clique
typedef typename ClusterTree<FG>::Cluster Clique;
typedef typename Clique::shared_ptr sharedClique; ///< Shared pointer to a clique
/// The BayesTree type produced by elimination
typedef BTCLIQUE BTClique;
/// Shared pointer to this class
typedef boost::shared_ptr<JunctionTree<FG> > shared_ptr;
/// We will frequently refer to a symbolic Bayes tree, used to find the clique structure
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
private:
/// @name Advanced Interface
/// @{
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg,
const SymbolicBayesTree::sharedClique& clique);
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& clique);
/// recursive elimination function
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique) const;
/// internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex);
/// @}
public:
/// @name Standard Constructors
/// @{
/** Default constructor */
JunctionTree() {}
/** Named constructor to build the junction tree of a factor graph. Note
* that this has to compute the column structure as a VariableIndex, so if you
* already have this precomputed, use the JunctionTree(const FG&, const VariableIndex&)
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
JunctionTree(const FG& factorGraph);
/** Construct from a factor graph and pre-computed variable index.
* @param fg The factor graph for which to build the junction tree
* @param structure The set of factors involving each variable. If this is not
* precomputed, you can call the JunctionTree(const FG&)
* constructor instead.
*/
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors in the subgraphs to produce a BayesTree.
* @param function The function used to eliminate, see the namespace functions
* in GaussianFactorGraph.h
* @return The BayesTree resulting from elimination
*/
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
/// @}
}; // JunctionTree
} // namespace gtsam
#include <gtsam/inference/JunctionTree-inl.h>

141
gtsam/inference/inference-inst.h Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file inference-inst.h
* @brief Contains *generic* inference algorithms that convert between templated
* graphical models, i.e., factor graphs, Bayes nets, and Bayes trees
* @author Frank Dellaert
* @author Richard Roberts
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <utility>
#include <gtsam/base/treeTraversal-inst.h>
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>
struct EliminationData {
EliminationData* const parentData;
std::vector<typename TREE::sharedFactor> childFactors;
EliminationData(EliminationData* _parentData, size_t nChildren) :
parentData(_parentData) { childFactors.reserve(nChildren); }
};
/* ************************************************************************* */
template<TREE>
EliminationData<TREE> eliminationPreOrderVisitor(
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());
}
/* ************************************************************************* */
template<TREE, RESULT>
void eliminationPostOrderVisitor(const 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));
}
}
/* ************************************************************************* */
/** Eliminate an elimination tree or a Bayes tree (used internally). Requires
* TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor,
* TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children. */
template<class TREE, class RESULT>
std::vector<typename TREE::sharedFactor>
EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function)
{
// Typedefs
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)); }
}
}
// Return remaining factors
return remainingFactors;
}
}
}

1
gtsam/symbolic/SymbolicConditionalUnordered.cpp
View File

@ -15,6 +15,7 @@
* @date Oct 17, 2010
*/
#include <gtsam/inference/ConditionalUnordered-inst.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
namespace gtsam {

28
gtsam/symbolic/SymbolicEliminationTreeUnordered.cpp Normal file
View File

@ -0,0 +1,28 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SymbolicEliminationTreeUnordered.cpp
* @date Mar 29, 2013
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/inference/EliminationTreeUnordered-inst.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
namespace gtsam {
SymbolicEliminationTreeUnordered::SymbolicEliminationTreeUnordered() {
}
}

10
gtsam/symbolic/SymbolicEliminationTreeUnordered.h
View File

@ -22,7 +22,7 @@
namespace gtsam {
class SymbolicEliminationTreeUnordered :
class GTSAM_EXPORT SymbolicEliminationTreeUnordered :
public EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> {
public:
typedef EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> Base; ///< Base class
@ -60,10 +60,16 @@ namespace gtsam {
private:
/// Private default constructor
SymbolicEliminationTreeUnordered() {}
SymbolicEliminationTreeUnordered();
friend class ::EliminationTreeUnorderedTester;
};
//#ifdef GTSAM_INSTANTIATE_SymbolicEliminationTreeUnordered
// template GTSAM_EXPORT class EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered>;
//#else
// extern template GTSAM_EXPORT class EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered>;
//#endif
}

1
gtsam/symbolic/SymbolicFactorGraphUnordered.cpp
View File

@ -17,6 +17,7 @@
#include <boost/make_shared.hpp>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
namespace gtsam {

13
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -18,7 +18,10 @@
#include <CppUnitLite/TestHarness.h>
#include <vector>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <boost/make_shared.hpp>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
@ -39,12 +42,12 @@ public:
SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTreeUnordered::Node);
node1->key = 1;
node1->factors.push_back(fg[2]);
node1->subTrees.push_back(leaf0);
node1->children.push_back(leaf0);
SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTreeUnordered::Node);
node2->key = 2;
node2->factors.push_back(fg[3]);
node2->subTrees.push_back(node1);
node2->children.push_back(node1);
SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTreeUnordered::Node);
leaf3->key = 3;
@ -52,8 +55,8 @@ public:
SymbolicEliminationTreeUnordered::sharedNode root(new SymbolicEliminationTreeUnordered::Node);
root->key = 4;
root->subTrees.push_back(leaf3);
root->subTrees.push_back(node2);
root->children.push_back(leaf3);
root->children.push_back(node2);
SymbolicEliminationTreeUnordered tree;
tree.roots_.push_back(root);
@ -131,7 +134,7 @@ TEST(EliminationTree, disconnected_graph) {
vector<size_t> order;
order += 0,1,2,3,4;
SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first;
EXPECT(assert_equal(expected,actual));
}