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

some name cleaning in the HybridJunctionTree

release/4.3a0
Varun Agrawal 2022-09-02 15:11:49 -04:00
parent 773af1ed44
commit ba4720709b
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -31,9 +31,7 @@ template class EliminatableClusterTree<HybridBayesTree,
template class JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>;
struct HybridConstructorTraversalData {
typedef
typename JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>::Node
Node;
typedef HybridJunctionTree::Node Node;
typedef
typename JunctionTree<HybridBayesTree,
HybridGaussianFactorGraph>::sharedNode sharedNode;
@ -62,6 +60,7 @@ struct HybridConstructorTraversalData {
data.junctionTreeNode = boost::make_shared<Node>(node->key, node->factors);
parentData.junctionTreeNode->addChild(data.junctionTreeNode);
// Add all the discrete keys in the hybrid factors to the current data
for (HybridFactor::shared_ptr& f : node->factors) {
for (auto& k : f->discreteKeys()) {
data.discreteKeys.insert(k.first);
@ -72,8 +71,8 @@ struct HybridConstructorTraversalData {
}
// Post-order visitor function
static void ConstructorTraversalVisitorPostAlg2(
const boost::shared_ptr<HybridEliminationTree::Node>& ETreeNode,
static void ConstructorTraversalVisitorPost(
const boost::shared_ptr<HybridEliminationTree::Node>& node,
const HybridConstructorTraversalData& data) {
// In this post-order visitor, we combine the symbolic elimination results
// from the elimination tree children and symbolically eliminate the current
@ -86,15 +85,15 @@ struct HybridConstructorTraversalData {
// Do symbolic elimination for this node
SymbolicFactors symbolicFactors;
symbolicFactors.reserve(ETreeNode->factors.size() +
symbolicFactors.reserve(node->factors.size() +
data.childSymbolicFactors.size());
// Add ETree node factors
symbolicFactors += ETreeNode->factors;
symbolicFactors += node->factors;
// Add symbolic factors passed up from children
symbolicFactors += data.childSymbolicFactors;
Ordering keyAsOrdering;
keyAsOrdering.push_back(ETreeNode->key);
keyAsOrdering.push_back(node->key);
SymbolicConditional::shared_ptr conditional;
SymbolicFactor::shared_ptr separatorFactor;
boost::tie(conditional, separatorFactor) =
@ -105,19 +104,19 @@ struct HybridConstructorTraversalData {
data.parentData->childSymbolicFactors.push_back(separatorFactor);
data.parentData->discreteKeys.merge(data.discreteKeys);
sharedNode node = data.junctionTreeNode;
sharedNode jt_node = data.junctionTreeNode;
const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
data.childSymbolicConditionals;
node->problemSize_ = (int)(conditional->size() * symbolicFactors.size());
jt_node->problemSize_ = (int)(conditional->size() * symbolicFactors.size());
// Merge our children if they are in our clique - if our conditional has
// exactly one fewer parent than our child's conditional.
const size_t nrParents = conditional->nrParents();
const size_t nrChildren = node->nrChildren();
const size_t nrChildren = jt_node->nrChildren();
assert(childConditionals.size() == nrChildren);
// decide which children to merge, as index into children
std::vector<size_t> nrChildrenFrontals = node->nrFrontalsOfChildren();
std::vector<size_t> nrChildrenFrontals = jt_node->nrFrontalsOfChildren();
std::vector<bool> merge(nrChildren, false);
size_t nrFrontals = 1;
for (size_t i = 0; i < nrChildren; i++) {
@ -137,7 +136,7 @@ struct HybridConstructorTraversalData {
}
// now really merge
node->mergeChildren(merge);
jt_node->mergeChildren(merge);
}
};
@ -161,7 +160,7 @@ HybridJunctionTree::HybridJunctionTree(
// the junction tree roots
treeTraversal::DepthFirstForest(eliminationTree, rootData,
Data::ConstructorTraversalVisitorPre,
Data::ConstructorTraversalVisitorPostAlg2);
Data::ConstructorTraversalVisitorPost);
// Assign roots from the dummy node
this->addChildrenAsRoots(rootData.junctionTreeNode);