From e49b40b4c45373a25f36052ba6026d2ceeebf77f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jan 2025 19:03:00 -0500 Subject: [PATCH 1/4] remove TableFactor check for another day --- gtsam/discrete/TableFactor.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index a833e1c5e..bf9662e34 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -252,15 +252,6 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - // If no keys, then return empty DecisionTreeFactor - if (dkeys.size() == 0) { - AlgebraicDecisionTree tree; - if (sparse_table_.size() != 0) { - tree = AlgebraicDecisionTree(sparse_table_.coeff(0)); - } - return DecisionTreeFactor(dkeys, tree); - } - std::vector table; for (auto i = 0; i < sparse_table_.size(); i++) { table.push_back(sparse_table_.coeff(i)); From bb4ee207b837aa5a613461b6c20a67e2d01ef29d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jan 2025 19:13:13 -0500 Subject: [PATCH 2/4] custom path for empty separator --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 49 +++++++++++----------- 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 387a5849f..7cc890dc0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -341,41 +341,40 @@ discreteElimination(const HybridGaussianFactorGraph &factors, #if GTSAM_HYBRID_TIMING gttic_(EliminateDiscrete); #endif - /**** NOTE: This does sum-product. ****/ - // Get product factor - TableFactor product = ProductAndNormalize(dfg); + // Check if separator is empty + Ordering allKeys(dfg.keyVector()); + Ordering separator; + std::set_difference(allKeys.begin(), allKeys.end(), frontalKeys.begin(), + frontalKeys.end(), + std::inserter(separator, separator.begin())); + + // If the separator is empty, we have a clique of all the discrete variables + // so we can use the TableFactor for efficiency. + if (separator.size() == 0) { + // Get product factor + TableFactor product = ProductAndNormalize(dfg); #if GTSAM_HYBRID_TIMING - gttic_(EliminateDiscreteSum); + gttic_(EliminateDiscreteFormDiscreteConditional); #endif - // All the discrete variables should form a single clique, - // so we can sum out on all the variables as frontals. - // This should give an empty separator. - TableFactor::shared_ptr sum = product.sum(frontalKeys); + auto conditional = std::make_shared( + frontalKeys.size(), product.toDecisionTreeFactor()); #if GTSAM_HYBRID_TIMING - gttoc_(EliminateDiscreteSum); + gttoc_(EliminateDiscreteFormDiscreteConditional); #endif - // Ordering keys for the conditional so that frontalKeys are really in front - Ordering orderedKeys; - orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end()); - orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end()); - + TableFactor::shared_ptr sum = product.sum(frontalKeys); #if GTSAM_HYBRID_TIMING - gttic_(EliminateDiscreteFormDiscreteConditional); -#endif - auto c = product / (*sum); - auto conditional = std::make_shared( - frontalKeys.size(), c.toDecisionTreeFactor(), orderedKeys); -#if GTSAM_HYBRID_TIMING - gttoc_(EliminateDiscreteFormDiscreteConditional); + gttoc_(EliminateDiscrete); #endif -#if GTSAM_HYBRID_TIMING - gttoc_(EliminateDiscrete); -#endif + return {std::make_shared(conditional), sum}; - return {std::make_shared(conditional), sum}; + } else { + // Perform sum-product. + auto result = EliminateDiscrete(dfg, frontalKeys); + return {std::make_shared(result.first), result.second}; + } } /* ************************************************************************ */ From 2894c957b1942d355b4c014b0a176ae6d592d078 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jan 2025 19:15:49 -0500 Subject: [PATCH 3/4] clarify TableProduct function --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7cc890dc0..25047bfad 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -256,13 +256,12 @@ static TableFactor::shared_ptr DiscreteFactorFromErrors( } /** - * @brief Multiply all the `factors` and normalize the - * product to prevent underflow. + * @brief Multiply all the `factors` using the machinery of the TableFactor. * * @param factors The factors to multiply as a DiscreteFactorGraph. * @return TableFactor */ -static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) { +static TableFactor TableProduct(const DiscreteFactorGraph &factors) { // PRODUCT: multiply all factors #if GTSAM_HYBRID_TIMING gttic_(DiscreteProduct); @@ -282,14 +281,13 @@ static TableFactor ProductAndNormalize(const DiscreteFactorGraph &factors) { gttoc_(DiscreteProduct); #endif - // Max over all the potentials by pretending all keys are frontal: - auto normalizer = product.max(product.size()); - #if GTSAM_HYBRID_TIMING gttic_(DiscreteNormalize); #endif + // Max over all the potentials by pretending all keys are frontal: + auto denominator = product.max(product.size()); // Normalize the product factor to prevent underflow. - product = product / (*normalizer); + product = product / (*denominator); #if GTSAM_HYBRID_TIMING gttoc_(DiscreteNormalize); #endif @@ -352,7 +350,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, // so we can use the TableFactor for efficiency. if (separator.size() == 0) { // Get product factor - TableFactor product = ProductAndNormalize(dfg); + TableFactor product = TableProduct(dfg); #if GTSAM_HYBRID_TIMING gttic_(EliminateDiscreteFormDiscreteConditional); From d22ba290547cb9a81c6e2656bd877bc4cf5a836d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Jan 2025 19:18:05 -0500 Subject: [PATCH 4/4] remove DiscreteConditional constructor since we no longer use it --- gtsam/discrete/DiscreteConditional.cpp | 9 --------- gtsam/discrete/DiscreteConditional.h | 11 ----------- 2 files changed, 20 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 8396b10e0..0eea8b4bd 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -47,15 +47,6 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals, const DecisionTreeFactor& f) : BaseFactor(f / (*f.sum(nrFrontals))), BaseConditional(nrFrontals) {} -/* ************************************************************************** */ -DiscreteConditional::DiscreteConditional(size_t nrFrontals, - const DecisionTreeFactor& f, - const Ordering& orderedKeys) - : BaseFactor(f), BaseConditional(nrFrontals) { - keys_.clear(); - keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end()); -} - /* ************************************************************************** */ DiscreteConditional::DiscreteConditional(size_t nrFrontals, const DiscreteKeys& keys, diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 549504985..3ec9ae590 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -56,17 +56,6 @@ class GTSAM_EXPORT DiscreteConditional /// Construct from factor, taking the first `nFrontals` keys as frontals. DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f); - /** - * @brief Construct from DecisionTreeFactor, - * taking the first `nrFrontals` from `orderedKeys`. - * - * @param nrFrontals The number of frontal variables. - * @param f The DecisionTreeFactor to construct from. - * @param orderedKeys Ordered list of keys involved in the conditional. - */ - DiscreteConditional(size_t nrFrontals, const DecisionTreeFactor& f, - const Ordering& orderedKeys); - /** * Construct from DiscreteKeys and AlgebraicDecisionTree, taking the first * `nFrontals` keys as frontals, in the order given.