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

Fix bug

release/4.3a0
Frank Dellaert 2025-01-28 21:15:35 -05:00
parent d4d95e2342
commit 2702854c18
1 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -214,15 +214,22 @@ namespace gtsam {
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> // std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
EliminateDiscrete(const DiscreteFactorGraph& factors, EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys) { const Ordering& frontalKeys) {
DiscreteFactor::shared_ptr product = factors.scaledProduct(); gttic(product);
DiscreteFactor::shared_ptr product = factors.product();
gttoc(product);
// sum out frontals, this is the factor on the separator // sum out frontals, this is the factor on the separator
gttic(sum); gttic(sum);
DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); DiscreteFactor::shared_ptr sum = product->sum(frontalKeys);
// Normalize/scale to prevent underflow.
sum = sum->scale();
gttoc(sum); gttoc(sum);
// Normalize/scale to prevent underflow.
gttic(scale);
DiscreteFactor::shared_ptr denominator = sum->max(sum->size());
product = product->operator/(denominator);
sum = sum->operator/(denominator);
gttoc(scale);
// Ordering keys for the conditional so that frontalKeys are really in front // Ordering keys for the conditional so that frontalKeys are really in front
Ordering orderedKeys; Ordering orderedKeys;
orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),