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

HybridGaussianFactorGraph::errorTree is better encompassing

release/4.3a0
Varun Agrawal 2024-09-20 16:20:49 -04:00
parent d145872916
commit 939fdcc720
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -542,10 +542,15 @@ AlgebraicDecisionTree<Key> HybridGaussianFactorGraph::errorTree(
// Iterate over each factor. // Iterate over each factor.
for (auto &factor : factors_) { for (auto &factor : factors_) {
if (auto f = std::dynamic_pointer_cast<HybridFactor>(factor)) { if (auto f = std::dynamic_pointer_cast<HybridFactor>(factor)) {
// Check for HybridFactor, and call errorTree
error_tree = error_tree + f->errorTree(continuousValues); error_tree = error_tree + f->errorTree(continuousValues);
} else if (auto f = std::dynamic_pointer_cast<GaussianFactor>(factor)) { } else if (auto f = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
error_tree = // Skip discrete factors
error_tree + AlgebraicDecisionTree<Key>(f->error(continuousValues)); continue;
} else {
// Everything else is a continuous only factor
HybridValues hv(continuousValues, DiscreteValues());
error_tree = error_tree + AlgebraicDecisionTree<Key>(factor->error(hv));
} }
} }
return error_tree; return error_tree;