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

Refactored elimination traversal a tiny bit

release/4.3a0
dellaert 2015-06-21 14:19:01 -07:00
parent a35adc127c
commit ef829c333e
1 changed files with 77 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,23 +17,29 @@
#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 // 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 // resulting from elimination of the children. Also sets up BayesTree cliques with parent and
// child pointers. // child pointers.
template<class CLUSTERTREE> template<class CLUSTERTREE>
struct EliminationData { struct EliminationData {
// Typedefs
typedef typename CLUSTERTREE::sharedFactor sharedFactor;
typedef typename CLUSTERTREE::FactorType FactorType;
typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
typedef typename CLUSTERTREE::ConditionalType ConditionalType;
typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
EliminationData* const parentData; EliminationData* const parentData;
size_t myIndexInParent; size_t myIndexInParent;
FastVector<typename CLUSTERTREE::sharedFactor> childFactors; FastVector<sharedFactor> childFactors;
boost::shared_ptr<typename CLUSTERTREE::BayesTreeType::Node> bayesTreeNode; boost::shared_ptr<BTNode> bayesTreeNode;
EliminationData(EliminationData* _parentData, size_t nChildren) : EliminationData(EliminationData* _parentData, size_t nChildren) :
parentData(_parentData), bayesTreeNode( parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>()) {
boost::make_shared<typename CLUSTERTREE::BayesTreeType::Node>()) {
if (parentData) { if (parentData) {
myIndexInParent = parentData->childFactors.size(); myIndexInParent = parentData->childFactors.size();
parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); parentData->childFactors.push_back(sharedFactor());
} else { } else {
myIndexInParent = 0; myIndexInParent = 0;
} }
@ -44,25 +50,20 @@ struct EliminationData {
parentData->bayesTreeNode->children.push_back(bayesTreeNode); parentData->bayesTreeNode->children.push_back(bayesTreeNode);
} }
} }
};
/* ************************************************************************* */ // Elimination pre-order visitor - creates the EliminationData structure for the visited node.
// Elimination pre-order visitor - just creates the EliminationData structure for the visited static EliminationData EliminationPreOrderVisitor(
// node.
template<class CLUSTERTREE>
EliminationData<CLUSTERTREE> eliminationPreOrderVisitor(
const typename CLUSTERTREE::sharedNode& node, const typename CLUSTERTREE::sharedNode& node,
EliminationData<CLUSTERTREE>& parentData) { EliminationData& parentData) {
EliminationData<CLUSTERTREE> myData(&parentData, node->children.size()); assert(node);
EliminationData myData(&parentData, node->children.size());
myData.bayesTreeNode->problemSize_ = node->problemSize(); myData.bayesTreeNode->problemSize_ = node->problemSize();
return myData; return myData;
} }
/* ************************************************************************* */ // Elimination post-order visitor - combine the child factors with our own factors, add the
// 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.
// resulting conditional to the BayesTree, and add the remaining factor to the parent. struct EliminationPostOrderVisitor {
template<class CLUSTERTREE>
struct EliminationPostOrderVisitor {
const typename CLUSTERTREE::Eliminate& eliminationFunction; const typename CLUSTERTREE::Eliminate& eliminationFunction;
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex;
EliminationPostOrderVisitor( EliminationPostOrderVisitor(
@ -71,13 +72,8 @@ struct EliminationPostOrderVisitor {
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) { eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {
} }
void operator()(const typename CLUSTERTREE::sharedNode& node, void operator()(const typename CLUSTERTREE::sharedNode& node,
EliminationData<CLUSTERTREE>& myData) { EliminationData& myData) {
// Typedefs assert(node);
typedef typename CLUSTERTREE::sharedFactor sharedFactor;
typedef typename CLUSTERTREE::FactorType FactorType;
typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
typedef typename CLUSTERTREE::ConditionalType ConditionalType;
typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
// Gather factors // Gather factors
FactorGraphType gatheredFactors; FactorGraphType gatheredFactors;
@ -95,7 +91,8 @@ struct EliminationPostOrderVisitor {
} }
// 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
@ -112,8 +109,8 @@ struct EliminationPostOrderVisitor {
myData.parentData->childFactors[myData.myIndexInParent] = myData.parentData->childFactors[myData.myIndexInParent] =
eliminationResult.second; eliminationResult.second;
} }
};
}; };
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> template<class BAYESTREE, class GRAPH>
@ -150,7 +147,7 @@ void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
orderedFrontalKeys.reserve(nrKeys); orderedFrontalKeys.reserve(nrKeys);
factors.reserve(nrFactors); factors.reserve(nrFactors);
typename Node::Children newChildren; typename Node::Children newChildren;
// newChildren.reserve(nrNewChildren); newChildren.reserve(nrNewChildren);
i = 0; i = 0;
BOOST_FOREACH(const sharedNode& child, children) { BOOST_FOREACH(const sharedNode& child, children) {
if (merge[i]) { if (merge[i]) {
@ -204,12 +201,14 @@ std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > ClusterTree<
// 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.
@ -218,18 +217,17 @@ std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > ClusterTree<
rootsContainer.bayesTreeNode->children.end()); 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< boost::shared_ptr<FactorGraphType> remaining = boost::make_shared<
FactorGraphType>(); FactorGraphType>();
allRemainingFactors->reserve( remaining->reserve(
remainingFactors_.size() + rootsContainer.childFactors.size()); remainingFactors_.size() + rootsContainer.childFactors.size());
allRemainingFactors->push_back(remainingFactors_.begin(), remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
remainingFactors_.end()); BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) {
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors)
if (factor) if (factor)
allRemainingFactors->push_back(factor); remaining->push_back(factor);
}
// Return result // Return result
return std::make_pair(result, allRemainingFactors); return std::make_pair(result, remaining);
} }
} }