diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 6f1f2d592..aec239d83 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -65,15 +65,10 @@ namespace gtsam { /* ************************************************************************ */ DiscreteFactor::shared_ptr DiscreteFactorGraph::product() const { - DiscreteFactor::shared_ptr result; - for (auto it = this->begin(); it != this->end(); ++it) { - if (*it) { - if (result) { - result = result->multiply(*it); - } else { - // Assign to the first non-null factor - result = *it; - } + DiscreteFactor::shared_ptr result = nullptr; + for (const auto& factor : *this) { + if (factor) { + result = result ? result->multiply(factor) : factor; } } return result; @@ -120,15 +115,7 @@ namespace gtsam { /* ************************************************************************ */ DiscreteFactor::shared_ptr DiscreteFactorGraph::scaledProduct() const { - // PRODUCT: multiply all factors - gttic(product); - DiscreteFactor::shared_ptr product = this->product(); - gttoc(product); - - // Normalize the product factor to prevent underflow. - product = product->scale(); - - return product; + return product()->scale(); } /* ************************************************************************ */ @@ -216,7 +203,7 @@ namespace gtsam { const Ordering& frontalKeys) { gttic(product); // `product` is scaled later to prevent underflow. - DiscreteFactor::shared_ptr product = factors.product(); + DiscreteFactor::shared_ptr product = factors.scaledProduct(); gttoc(product); // sum out frontals, this is the factor on the separator @@ -224,16 +211,6 @@ namespace gtsam { DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); 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 orderedKeys; orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),