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

Fixed bottleneck in reelimination with orphans

release/4.3a0
Richard Roberts 2013-08-12 03:57:17 +00:00
parent 4c59e13f94
commit f73db8847b
2 changed files with 20 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/BayesTree.h
View File

@ -30,6 +30,7 @@ namespace gtsam {
// Forward declarations // Forward declarations
template<class FACTOR> class FactorGraph; template<class FACTOR> class FactorGraph;
template<class BAYESTREE, class GRAPH> class JunctionTree;
/* ************************************************************************* */ /* ************************************************************************* */
/** clique statistics */ /** clique statistics */
@ -242,6 +243,9 @@ namespace gtsam {
/** Fill the nodes index for a subtree */ /** Fill the nodes index for a subtree */
void fillNodesIndex(const sharedClique& subtree); void fillNodesIndex(const sharedClique& subtree);
// Friend JunctionTree because it directly fills roots and nodes index.
template<class BAYESRTEE, class GRAPH> friend class JunctionTree;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

24
gtsam/inference/JunctionTree-inst.h
View File

@ -166,7 +166,10 @@ namespace gtsam {
struct EliminationPostOrderVisitor struct EliminationPostOrderVisitor
{ {
const typename JUNCTIONTREE::Eliminate& eliminationFunction; const typename JUNCTIONTREE::Eliminate& eliminationFunction;
EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction) : eliminationFunction(eliminationFunction) {} typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex;
EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction,
typename JUNCTIONTREE::BayesTreeType::Nodes& nodesIndex) :
eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {}
void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData) void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData)
{ {
// Typedefs // Typedefs
@ -194,10 +197,16 @@ namespace gtsam {
// 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, Ordering(node->keys)); eliminationFunction(gatheredFactors, Ordering(node->keys));
// 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
myData.bayesTreeNode->setEliminationResult(eliminationResult); 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)).second;
// Store remaining factor in parent's gathered factors // Store remaining factor in parent's gathered factors
if(!eliminationResult.second->empty()) if(!eliminationResult.second->empty())
@ -279,16 +288,15 @@ namespace gtsam {
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
// 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>();
EliminationData<This> rootsContainer(0, roots_.size()); EliminationData<This> rootsContainer(0, roots_.size());
EliminationPostOrderVisitor<This> visitorPost(function); EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
//tbb::task_scheduler_init init(1); //tbb::task_scheduler_init init(1);
treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer, treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer,
eliminationPreOrderVisitor<This>, visitorPost/*, 10*/); eliminationPreOrderVisitor<This>, visitorPost/*, 10*/);
// Create BayesTree from roots stored in the dummy BayesTree node. // Create BayesTree from roots stored in the dummy BayesTree node.
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>(); result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end());
BOOST_FOREACH(const typename BayesTreeType::sharedNode& root, rootsContainer.bayesTreeNode->children)
result->insertRoot(root);
// 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<FactorGraphType>(); boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();