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

156
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,76 +50,67 @@ 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. const typename CLUSTERTREE::sharedNode& node,
template<class CLUSTERTREE> EliminationData& parentData) {
EliminationData<CLUSTERTREE> eliminationPreOrderVisitor( assert(node);
const typename CLUSTERTREE::sharedNode& node, EliminationData myData(&parentData, node->children.size());
EliminationData<CLUSTERTREE>& parentData) { myData.bayesTreeNode->problemSize_ = node->problemSize();
EliminationData<CLUSTERTREE> myData(&parentData, node->children.size()); return myData;
myData.bayesTreeNode->problemSize_ = node->problemSize();
return myData;
}
/* ************************************************************************* */
// Elimination post-order visitor - combine the child factors with our own factors, add the
// resulting conditional to the BayesTree, and add the remaining factor to the parent.
template<class CLUSTERTREE>
struct EliminationPostOrderVisitor {
const typename CLUSTERTREE::Eliminate& eliminationFunction;
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex;
EliminationPostOrderVisitor(
const typename CLUSTERTREE::Eliminate& eliminationFunction,
typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {
} }
void operator()(const typename CLUSTERTREE::sharedNode& node,
EliminationData<CLUSTERTREE>& myData) {
// Typedefs
typedef typename CLUSTERTREE::sharedFactor sharedFactor;
typedef typename CLUSTERTREE::FactorType FactorType;
typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
typedef typename CLUSTERTREE::ConditionalType ConditionalType;
typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
// Gather factors // Elimination post-order visitor - combine the child factors with our own factors, add the
FactorGraphType gatheredFactors; // resulting conditional to the BayesTree, and add the remaining factor to the parent.
gatheredFactors.reserve(node->factors.size() + node->children.size()); struct EliminationPostOrderVisitor {
gatheredFactors += node->factors; const typename CLUSTERTREE::Eliminate& eliminationFunction;
gatheredFactors += myData.childFactors; typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex;
EliminationPostOrderVisitor(
// Check for Bayes tree orphan subtrees, and add them to our children const typename CLUSTERTREE::Eliminate& eliminationFunction,
BOOST_FOREACH(const sharedFactor& f, node->factors) { typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
if (const BayesTreeOrphanWrapper<BTNode>* asSubtree = eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {
dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) {
myData.bayesTreeNode->children.push_back(asSubtree->clique);
asSubtree->clique->parent_ = myData.bayesTreeNode;
}
} }
void operator()(const typename CLUSTERTREE::sharedNode& node,
EliminationData& myData) {
assert(node);
// Do dense elimination step // Gather factors
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = FactorGraphType gatheredFactors;
eliminationFunction(gatheredFactors, node->orderedFrontalKeys); gatheredFactors.reserve(node->factors.size() + node->children.size());
gatheredFactors += node->factors;
gatheredFactors += myData.childFactors;
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor // Check for Bayes tree orphan subtrees, and add them to our children
myData.bayesTreeNode->setEliminationResult(eliminationResult); BOOST_FOREACH(const sharedFactor& f, node->factors) {
if (const BayesTreeOrphanWrapper<BTNode>* asSubtree =
dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) {
myData.bayesTreeNode->children.push_back(asSubtree->clique);
asSubtree->clique->parent_ = myData.bayesTreeNode;
}
}
// Fill nodes index - we do this here instead of calling insertRoot at the end to avoid // Do dense elimination step
// putting orphan subtrees in the index - they'll already be in the index of the ISAM2 std::pair<boost::shared_ptr<ConditionalType>,
// object they're added to. boost::shared_ptr<FactorType> > eliminationResult =
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode));
// Store remaining factor in parent's gathered factors // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
if (!eliminationResult.second->empty()) myData.bayesTreeNode->setEliminationResult(eliminationResult);
myData.parentData->childFactors[myData.myIndexInParent] =
eliminationResult.second; // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
} // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
// object they're added to.
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode));
// Store remaining factor in parent's gathered factors
if (!eliminationResult.second->empty())
myData.parentData->childFactors[myData.myIndexInParent] =
eliminationResult.second;
}
};
}; };
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class BAYESTREE, class GRAPH> 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);
} }
} }