Refactored elimination traversal a tiny bit
dellaert
parent
a35adc127c
commit
ef829c333e
|
|
@ -17,23 +17,29 @@
|
|||
#include <boost/bind.hpp>
|
||||
|
||||
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 {
|
||||
// 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;
|
||||
size_t myIndexInParent;
|
||||
FastVector<typename CLUSTERTREE::sharedFactor> childFactors;
|
||||
boost::shared_ptr<typename CLUSTERTREE::BayesTreeType::Node> bayesTreeNode;
|
||||
FastVector<sharedFactor> childFactors;
|
||||
boost::shared_ptr<BTNode> bayesTreeNode;
|
||||
EliminationData(EliminationData* _parentData, size_t nChildren) :
|
||||
parentData(_parentData), bayesTreeNode(
|
||||
boost::make_shared<typename CLUSTERTREE::BayesTreeType::Node>()) {
|
||||
parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>()) {
|
||||
if (parentData) {
|
||||
myIndexInParent = parentData->childFactors.size();
|
||||
parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor());
|
||||
parentData->childFactors.push_back(sharedFactor());
|
||||
} else {
|
||||
myIndexInParent = 0;
|
||||
}
|
||||
|
|
@ -44,76 +50,67 @@ struct EliminationData {
|
|||
parentData->bayesTreeNode->children.push_back(bayesTreeNode);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Elimination pre-order visitor - just creates the EliminationData structure for the visited
|
||||
// node.
|
||||
template<class CLUSTERTREE>
|
||||
EliminationData<CLUSTERTREE> eliminationPreOrderVisitor(
|
||||
const typename CLUSTERTREE::sharedNode& node,
|
||||
EliminationData<CLUSTERTREE>& parentData) {
|
||||
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) {
|
||||
// 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;
|
||||
}
|
||||
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
|
||||
FactorGraphType gatheredFactors;
|
||||
gatheredFactors.reserve(node->factors.size() + node->children.size());
|
||||
gatheredFactors += node->factors;
|
||||
gatheredFactors += myData.childFactors;
|
||||
|
||||
// Check for Bayes tree orphan subtrees, and add them to our children
|
||||
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;
|
||||
}
|
||||
// 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);
|
||||
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
|
||||
// Gather factors
|
||||
FactorGraphType gatheredFactors;
|
||||
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
|
||||
myData.bayesTreeNode->setEliminationResult(eliminationResult);
|
||||
// Check for Bayes tree orphan subtrees, and add them to our children
|
||||
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
|
||||
// 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));
|
||||
// Do dense elimination step
|
||||
std::pair<boost::shared_ptr<ConditionalType>,
|
||||
boost::shared_ptr<FactorType> > eliminationResult =
|
||||
eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
|
||||
|
||||
// Store remaining factor in parent's gathered factors
|
||||
if (!eliminationResult.second->empty())
|
||||
myData.parentData->childFactors[myData.myIndexInParent] =
|
||||
eliminationResult.second;
|
||||
}
|
||||
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
|
||||
myData.bayesTreeNode->setEliminationResult(eliminationResult);
|
||||
|
||||
// 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>
|
||||
|
|
@ -150,7 +147,7 @@ void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
|
|||
orderedFrontalKeys.reserve(nrKeys);
|
||||
factors.reserve(nrFactors);
|
||||
typename Node::Children newChildren;
|
||||
// newChildren.reserve(nrNewChildren);
|
||||
newChildren.reserve(nrNewChildren);
|
||||
i = 0;
|
||||
BOOST_FOREACH(const sharedNode& child, children) {
|
||||
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
|
||||
// uneliminated factors passed up from the roots.
|
||||
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
|
||||
EliminationData<This> rootsContainer(0, roots_.size());
|
||||
EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
|
||||
typedef EliminationData<This> Data;
|
||||
Data rootsContainer(0, roots_.size());
|
||||
typename Data::EliminationPostOrderVisitor visitorPost(function,
|
||||
result->nodes_);
|
||||
{
|
||||
TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
|
||||
treeTraversal::DepthFirstForestParallel(*this, rootsContainer,
|
||||
eliminationPreOrderVisitor<This>, visitorPost, 10);
|
||||
Data::EliminationPreOrderVisitor, visitorPost, 10);
|
||||
}
|
||||
|
||||
// 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());
|
||||
|
||||
// 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>();
|
||||
allRemainingFactors->reserve(
|
||||
remaining->reserve(
|
||||
remainingFactors_.size() + rootsContainer.childFactors.size());
|
||||
allRemainingFactors->push_back(remainingFactors_.begin(),
|
||||
remainingFactors_.end());
|
||||
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors)
|
||||
remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
|
||||
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) {
|
||||
if (factor)
|
||||
allRemainingFactors->push_back(factor);
|
||||
|
||||
remaining->push_back(factor);
|
||||
}
|
||||
// Return result
|
||||
return std::make_pair(result, allRemainingFactors);
|
||||
return std::make_pair(result, remaining);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue