From 92496dc88658cc83292fb995341da1c641336399 Mon Sep 17 00:00:00 2001 From: Frank Dellaert <dellaert@gmail.com> Date: Fri, 24 Jan 2025 14:15:21 -0500 Subject: [PATCH 1/2] Address comments in #1991 --- gtsam/inference/BayesTree-inst.h | 3 ++- gtsam/inference/BayesTreeCliqueBase-inst.h | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index c65e2ddc26..f97972e5ae 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -338,6 +338,7 @@ namespace gtsam { /* ************************************************************************* */ // Find the lowest common ancestor of two cliques + // TODO(Varun): consider implementing this as a Range Minimum Query template <class CLIQUE> static std::shared_ptr<CLIQUE> findLowestCommonAncestor( const std::shared_ptr<CLIQUE>& C1, const std::shared_ptr<CLIQUE>& C2) { @@ -360,7 +361,7 @@ namespace gtsam { /* ************************************************************************* */ // Given the clique P(F:S) and the ancestor clique B - // Return the Bayes tree P(S\B | S \cap B) + // Return the Bayes tree P(S\B | S \cap B), where \cap is intersection template <class CLIQUE> static auto factorInto( const std::shared_ptr<CLIQUE>& p_F_S, const std::shared_ptr<CLIQUE>& B, diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 9e687be6b6..40fa5b9a4d 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -107,7 +107,7 @@ namespace gtsam { // The shortcut density is a conditional P(S|B) of the separator of this // clique on the root or common ancestor B. We can compute it recursively from // the parent shortcut P(Sp|B) as \int P(Fp|Sp) P(Sp|B), where Fp are the - // frontal nodes in p + // frontal nodes in pthe parent p, and Sp the separator of the parent. /* ************************************************************************* */ template <class DERIVED, class FACTORGRAPH> @@ -141,7 +141,8 @@ namespace gtsam { /* *********************************************************************** */ // Separator marginal, uses separator marginal of parent recursively // Calculates P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) - // if P(Sp) is not cached, it will call separatorMarginal on the parent + // if P(Sp) is not cached, it will call separatorMarginal on the parent. + // Here again, Fp and Sp are the frontal nodes and separator in the parent p. /* *********************************************************************** */ template <class DERIVED, class FACTORGRAPH> typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType From 40f8a12ffaaf35cbf9e7b21d8f14af7c6b9c694b Mon Sep 17 00:00:00 2001 From: Frank Dellaert <dellaert@gmail.com> Date: Fri, 24 Jan 2025 14:16:05 -0500 Subject: [PATCH 2/2] Comment and refactor cartesian product --- gtsam/discrete/Assignment.h | 38 +++++++++++++++++++++---------------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index 56a0ed3276..8d29c008d9 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -85,25 +85,31 @@ class Assignment : public std::map<L, size_t> { * variables with each having cardinalities 4, we get 4096 possible * configurations!! */ - template <typename Derived = Assignment<L>> - static std::vector<Derived> CartesianProduct( + template <typename AssignmentType = Assignment<L>> + static std::vector<AssignmentType> CartesianProduct( const std::vector<std::pair<L, size_t>>& keys) { - std::vector<Derived> allPossValues; - Derived values; - typedef std::pair<L, size_t> DiscreteKey; - for (const DiscreteKey& key : keys) - values[key.first] = 0; // Initialize from 0 - while (1) { - allPossValues.push_back(values); + std::vector<AssignmentType> allPossValues; + AssignmentType assignment; + for (const auto [idx, _] : keys) assignment[idx] = 0; // Initialize from 0 + + const size_t nrKeys = keys.size(); + while (true) { + allPossValues.push_back(assignment); + + // Increment the assignment. This generalizes incrementing a binary number size_t j = 0; - for (j = 0; j < keys.size(); j++) { - L idx = keys[j].first; - values[idx]++; - if (values[idx] < keys[j].second) break; - // Wrap condition - values[idx] = 0; + for (j = 0; j < nrKeys; j++) { + auto [idx, cardinality] = keys[j]; + // Most of the time, we just increment the value for the first key, j=0: + assignment[idx]++; + // But if this key is done, we increment next key. + const bool carry = (assignment[idx] == cardinality); + if (!carry) break; + assignment[idx] = 0; // wrap on carry, and continue to next variable } - if (j == keys.size()) break; + + // If we propagated carry past the last key, exit: + if (j == nrKeys) break; } return allPossValues; }