CSP.cpp
Go to the documentation of this file.
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>
12 
13 using namespace std;
14 
15 namespace gtsam {
16 
17 bool 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(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>((*this)[f]);
34  if (constraint) {
35  changed = constraint->ensureArcConsistency(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.
44 Domains 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(key, 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);
58  if (!changed) break;
59  }
60  return domains;
61 }
62 
63 CSP 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>(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>(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(reduced);
84  }
85  }
86  return new_csp;
87 }
88 } // namespace gtsam
DiscreteBayesNet.h
Testable.h
Concept check for values that can be used in unit tests.
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::Domain
Definition: Domain.h:20
gtsam::Domains
std::map< Key, Domain > Domains
Definition: Constraint.h:29
gtsam::VariableIndex
Definition: VariableIndex.h:41
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
Domain.h
gtsam
traits
Definition: SFMdata.h:40
gtsam::DiscreteFactor::shared_ptr
std::shared_ptr< DiscreteFactor > shared_ptr
shared_ptr to this class
Definition: DiscreteFactor.h:44
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::CSP
Definition: CSP.h:21
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
std
Definition: BFloat16.h:88
CSP.h
gtsam::Constraint::shared_ptr
std::shared_ptr< Constraint > shared_ptr
Definition: Constraint.h:37
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:18