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

allow for GaussianMixtureFactors with 0 continuous keys

release/4.3a0
Varun Agrawal 2024-08-25 00:49:28 -04:00
parent 1f373fd136
commit 7358effe03
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -242,6 +242,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors,
for (auto &f : factors) {
if (auto df = dynamic_pointer_cast<DiscreteFactor>(f)) {
dfg.push_back(df);
} else if (auto gmf = dynamic_pointer_cast<GaussianMixtureFactor>(f)) {
// Case where we have a GaussianMixtureFactor with no continuous keys.
// In this case, compute discrete probabilities.
auto probability =
[&](const GaussianFactor::shared_ptr &factor) -> double {
if (!factor) return 0.0;
return exp(-factor->error(VectorValues()));
};
dfg.emplace_shared<DecisionTreeFactor>(
gmf->discreteKeys(),
DecisionTree<Key, double>(gmf->factors(), probability));
} else if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) {
// Ignore orphaned clique.
// TODO(dellaert): is this correct? If so explain here.