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
template<class FACTOR> class FactorGraph;
template<class BAYESTREE, class GRAPH> class JunctionTree;
/* ************************************************************************* */
/** clique statistics */
@ -242,6 +243,9 @@ namespace gtsam {
/** Fill the nodes index for a 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:
/** Serialization function */
friend class boost::serialization::access;

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

@ -166,7 +166,10 @@ namespace gtsam {
struct EliminationPostOrderVisitor
{
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)
{
// Typedefs
@ -199,6 +202,12 @@ namespace gtsam {
// 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)).second;
// Store remaining factor in parent's gathered factors
if(!eliminationResult.second->empty())
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
@ -279,16 +288,15 @@ namespace gtsam {
// 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
// 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);
EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
//tbb::task_scheduler_init init(1);
treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer,
eliminationPreOrderVisitor<This>, visitorPost/*, 10*/);
// Create BayesTree from roots stored in the dummy BayesTree node.
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
BOOST_FOREACH(const typename BayesTreeType::sharedNode& root, rootsContainer.bayesTreeNode->children)
result->insertRoot(root);
result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end());
// Add remaining factors that were not involved with eliminated variables
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();