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

full discrete elimination

release/4.3a0
Varun Agrawal 2024-12-31 18:23:37 -05:00
parent bf4c0bd72d
commit 0e2e8bb8ce
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -356,13 +356,17 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
gttoc_(EliminateDiscreteSum); gttoc_(EliminateDiscreteSum);
#endif #endif
// Ordering keys for the conditional so that frontalKeys are really in front
Ordering orderedKeys;
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());
#if GTSAM_HYBRID_TIMING #if GTSAM_HYBRID_TIMING
gttic_(EliminateDiscreteFormDiscreteConditional); gttic_(EliminateDiscreteFormDiscreteConditional);
#endif #endif
// Finally, get the conditional // Finally, get the conditional
auto c = product / (*sum);
auto conditional = std::make_shared<DiscreteConditional>( auto conditional = std::make_shared<DiscreteConditional>(
frontalKeys.size(), c.toDecisionTreeFactor()); product.toDecisionTreeFactor(), sum->toDecisionTreeFactor(), orderedKeys);
#if GTSAM_HYBRID_TIMING #if GTSAM_HYBRID_TIMING
gttoc_(EliminateDiscreteFormDiscreteConditional); gttoc_(EliminateDiscreteFormDiscreteConditional);
#endif #endif