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

fix creation of updatedBayesTree

release/4.3a0
Varun Agrawal 2022-11-09 20:04:51 -05:00
parent 7ae4e57d66
commit d54cf484de
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -768,10 +768,13 @@ HybridGaussianFactorGraph::eliminateHybridMultifrontal(
auto discrete_clique = (*updatedBayesTree)[discrete_ordering.at(0)];
// Set the root of the bayes tree as the discrete clique
std::set<HybridBayesTreeClique::shared_ptr> clique_set;
for (auto node : bayesTree->nodes()) {
auto clique = node.second;
clique_set.insert(node.second);
}
// Set the root of the bayes tree as the discrete clique
for (auto clique : clique_set) {
if (clique->conditional()->parents() ==
discrete_clique->conditional()->frontals()) {
updatedBayesTree->addClique(clique, discrete_clique);