Comment and refactor cartesian product

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>>
+  template <typename AssignmentType = Assignment<L>>
-  static std::vector<Derived> CartesianProduct(
+  static std::vector<AssignmentType> CartesianProduct(
       const std::vector<std::pair<L, size_t>>& keys) {
-    std::vector<Derived> allPossValues;
+    std::vector<AssignmentType> allPossValues;
-    Derived values;
+    AssignmentType assignment;
-    typedef std::pair<L, size_t> DiscreteKey;
+    for (const auto [idx, _] : keys) assignment[idx] = 0;  // Initialize from 0
-    for (const DiscreteKey& key : keys)
+
-      values[key.first] = 0;  // Initialize from 0
+    const size_t nrKeys = keys.size();
-    while (1) {
+    while (true) {
-      allPossValues.push_back(values);
+      allPossValues.push_back(assignment);
+
+      // Increment the assignment. This generalizes incrementing a binary number
       size_t j = 0;
-      for (j = 0; j < keys.size(); j++) {
+      for (j = 0; j < nrKeys; j++) {
-        L idx = keys[j].first;
+        auto [idx, cardinality] = keys[j];
-        values[idx]++;
+        // Most of the time, we just increment the value for the first key, j=0:
-        if (values[idx] < keys[j].second) break;
+        assignment[idx]++;
-        // Wrap condition
+        // But if this key is done, we increment next key.
-        values[idx] = 0;
+        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;
   }