1/*
2 * CSP.cpp
3 * @brief Constraint Satisfaction Problem class
4 * @date Feb 6, 2012
5 * @author Frank Dellaert
6 */
7
8#include <gtsam/base/Testable.h>
9#include <gtsam/discrete/DiscreteBayesNet.h>
10#include <gtsam_unstable/discrete/CSP.h>
11#include <gtsam_unstable/discrete/Domain.h>
12
13using namespace std;
14
15namespace gtsam {
16
17bool CSP::runArcConsistency(const VariableIndex& index,
18 Domains* domains) const {
19 bool changed = false;
20
21 // iterate over all variables in the index
22 for (auto entry : index) {
23 // Get the variable's key and associated factors:
24 const Key key = entry.first;
25 const FactorIndices& factors = entry.second;
26
27 // If this domain is already a singleton, we do nothing.
28 if (domains->at(k: key).isSingleton()) continue;
29
30 // Otherwise, loop over all factors/constraints for variable with given key.
31 for (size_t f : factors) {
32 // If this factor is a constraint, call its ensureArcConsistency method:
33 auto constraint = std::dynamic_pointer_cast<Constraint>(r: (*this)[f]);
34 if (constraint) {
35 changed = constraint->ensureArcConsistency(j: key, domains) || changed;
36 }
37 }
38 }
39 return changed;
40}
41
42// TODO(dellaert): This is AC1, which is inefficient as any change will cause
43// the algorithm to revisit *all* variables again. Implement AC3.
44Domains CSP::runArcConsistency(size_t cardinality, size_t maxIterations) const {
45 // Create VariableIndex
46 VariableIndex index(*this);
47
48 // Initialize domains
49 Domains domains;
50 for (auto entry : index) {
51 const Key key = entry.first;
52 domains.emplace(args: key, args: DiscreteKey(key, cardinality));
53 }
54
55 // Iterate until convergence or not a single domain changed.
56 for (size_t it = 0; it < maxIterations; it++) {
57 bool changed = runArcConsistency(index, domains: &domains);
58 if (!changed) break;
59 }
60 return domains;
61}
62
63CSP CSP::partiallyApply(const Domains& domains) const {
64 // Create new problem with all singleton variables removed
65 // We do this by adding simplifying all factors using partial application.
66 // TODO: create a new ordering as we go, to ensure a connected graph
67 // KeyOrdering ordering;
68 // vector<Index> dkeys;
69 CSP new_csp;
70
71 // Add tightened domains as new factors:
72 for (auto key_domain : domains) {
73 new_csp.emplace_shared<Domain>(args&: key_domain.second);
74 }
75
76 // Reduce all existing factors:
77 for (const DiscreteFactor::shared_ptr& f : factors_) {
78 auto constraint = std::dynamic_pointer_cast<Constraint>(r: f);
79 if (!constraint)
80 throw runtime_error("CSP:runArcConsistency: non-constraint factor");
81 Constraint::shared_ptr reduced = constraint->partiallyApply(domains);
82 if (reduced->size() > 1) {
83 new_csp.push_back(factor: reduced);
84 }
85 }
86 return new_csp;
87}
88} // namespace gtsam
89