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;
   }