diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index 56a0ed327..8d29c008d 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -85,25 +85,31 @@ class Assignment : public std::map { * variables with each having cardinalities 4, we get 4096 possible * configurations!! */ - template > - static std::vector CartesianProduct( + template > + static std::vector CartesianProduct( const std::vector>& keys) { - std::vector allPossValues; - Derived values; - typedef std::pair DiscreteKey; - for (const DiscreteKey& key : keys) - values[key.first] = 0; // Initialize from 0 - while (1) { - allPossValues.push_back(values); + std::vector 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; }