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

remove fixed values when keys are reintroduced in newFactors

release/4.3a0
Varun Agrawal 2025-03-17 15:39:56 -04:00
parent f401a9056e
commit 88fb8f7c65
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -73,9 +73,10 @@ HybridBayesNet HybridBayesNet::prune(
if (conditional->isDiscrete()) continue; if (conditional->isDiscrete()) continue;
// No-op if not a HybridGaussianConditional. // No-op if not a HybridGaussianConditional.
if (marginalThreshold) if (marginalThreshold) {
conditional = std::static_pointer_cast<HybridConditional>( conditional = std::static_pointer_cast<HybridConditional>(
conditional->restrict(fixed)); conditional->restrict(fixed));
}
// Now decide on type what to do: // Now decide on type what to do:
if (auto hgc = conditional->asHybrid()) { if (auto hgc = conditional->asHybrid()) {

7
gtsam/hybrid/HybridSmoother.cpp
View File

@ -118,6 +118,13 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
} }
#endif #endif
// Remove fixed values for discrete keys which are introduced in newFactors
for (Key key : newFactors.discreteKeySet()) {
if (fixedValues_.find(key) != fixedValues_.end()) {
fixedValues_.erase(key);
}
}
#ifdef DEBUG_SMOOTHER #ifdef DEBUG_SMOOTHER
// Print discrete keys in the bayesNetFragment: // Print discrete keys in the bayesNetFragment:
std::cout << "Discrete keys in bayesNetFragment: "; std::cout << "Discrete keys in bayesNetFragment: ";