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

Refactor scaledProduct

release/4.3a0
Frank Dellaert 2025-01-30 15:55:33 -05:00
parent c7864d32b5
commit 16b2710c3f
1 changed files with 6 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -65,15 +65,10 @@ namespace gtsam {
/* ************************************************************************ */ /* ************************************************************************ */
DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const { DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const {
DiscreteFactor::shared_ptr result; DiscreteFactor::shared_ptr result = nullptr;
for (auto it = this->begin(); it != this->end(); ++it) { for (const auto& factor : *this) {
if (*it) { if (factor) {
if (result) { result = result ? result->multiply(factor) : factor;
result = result->multiply(*it);
} else {
// Assign to the first non-null factor
result = *it;
}
} }
} }
return result; return result;
@ -120,15 +115,7 @@ namespace gtsam {
/* ************************************************************************ */ /* ************************************************************************ */
DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const { DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const {
// PRODUCT: multiply all factors return product()->scale();
gttic(product);
DiscreteFactor::shared_ptr product = this->product();
gttoc(product);
// Normalize the product factor to prevent underflow.
product = product->scale();
return product;
} }
/* ************************************************************************ */ /* ************************************************************************ */
@ -216,7 +203,7 @@ namespace gtsam {
const Ordering& frontalKeys) { const Ordering& frontalKeys) {
gttic(product); gttic(product);
// `product` is scaled later to prevent underflow. // `product` is scaled later to prevent underflow.
DiscreteFactor::shared_ptr product = factors.product(); DiscreteFactor::shared_ptr product = factors.scaledProduct();
gttoc(product); gttoc(product);
// sum out frontals, this is the factor on the separator // sum out frontals, this is the factor on the separator
@ -224,16 +211,6 @@ namespace gtsam {
DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); DiscreteFactor::shared_ptr sum = product->sum(frontalKeys);
gttoc(sum); gttoc(sum);
// Normalize/scale to prevent underflow.
// We divide both `product` and `sum` by `max(sum)`
// since it is faster to compute and when the conditional
// is formed by `product/sum`, the scaling term cancels out.
// 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(),