testCSP.cpp
Go to the documentation of this file.
1 /*
2  * testCSP.cpp
3  * @brief develop code for CSP solver
4  * @date Feb 5, 2012
5  * @author Frank Dellaert
6  */
7 
8 #include <gtsam_unstable/discrete/CSP.h>
9 #include <gtsam_unstable/discrete/Domain.h>
10 #include <boost/assign/std/map.hpp>
11 using boost::assign::insert;
12 #include <CppUnitLite/TestHarness.h>
13 #include <iostream>
14 #include <fstream>
15 
16 using namespace std;
17 using namespace gtsam;
18 
19 /* ************************************************************************* */
20 TEST_UNSAFE( BinaryAllDif, allInOne)
21 {
22  // Create keys and ordering
23  size_t nrColors = 2;
24 // DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", nrColors);
25  DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);
26 
27  // Check construction and conversion
28  BinaryAllDiff c1(ID, UT);
29  DecisionTreeFactor f1(ID & UT, "0 1 1 0");
30  EXPECT(assert_equal(f1,c1.toDecisionTreeFactor()));
31 
32  // Check construction and conversion
33  BinaryAllDiff c2(UT, AZ);
34  DecisionTreeFactor f2(UT & AZ, "0 1 1 0");
35  EXPECT(assert_equal(f2,c2.toDecisionTreeFactor()));
36 
37  DecisionTreeFactor f3 = f1*f2;
38  EXPECT(assert_equal(f3,c1*f2));
39  EXPECT(assert_equal(f3,c2*f1));
40 }
41 
42 /* ************************************************************************* */
43 TEST_UNSAFE( CSP, allInOne)
44 {
45  // Create keys and ordering
46  size_t nrColors = 2;
47  DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);
48 
49  // Create the CSP
50  CSP csp;
51  csp.addAllDiff(ID,UT);
52  csp.addAllDiff(UT,AZ);
53 
54  // Check an invalid combination, with ID==UT==AZ all same color
55  DiscreteFactor::Values invalid;
56  invalid[ID.first] = 0;
57  invalid[UT.first] = 0;
58  invalid[AZ.first] = 0;
59  EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);
60 
61  // Check a valid combination
62  DiscreteFactor::Values valid;
63  valid[ID.first] = 0;
64  valid[UT.first] = 1;
65  valid[AZ.first] = 0;
66  EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);
67 
68  // Just for fun, create the product and check it
69  DecisionTreeFactor product = csp.product();
70  // product.dot("product");
71  DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
72  EXPECT(assert_equal(expectedProduct,product));
73 
74  // Solve
75  CSP::sharedValues mpe = csp.optimalAssignment();
76  CSP::Values expected;
77  insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
78  EXPECT(assert_equal(expected,*mpe));
79  EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
80 }
81 
82 /* ************************************************************************* */
83 TEST_UNSAFE( CSP, WesternUS)
84 {
85  // Create keys
86  size_t nrColors = 4;
87  DiscreteKey
88  // Create ordering according to example in ND-CSP.lyx
89  WA(0, nrColors), OR(3, nrColors), CA(1, nrColors),NV(2, nrColors),
90  ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors),
91  MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors);
92 
93  // Create the CSP
94  CSP csp;
95  csp.addAllDiff(WA,ID);
96  csp.addAllDiff(WA,OR);
97  csp.addAllDiff(OR,ID);
98  csp.addAllDiff(OR,CA);
99  csp.addAllDiff(OR,NV);
100  csp.addAllDiff(CA,NV);
101  csp.addAllDiff(CA,AZ);
102  csp.addAllDiff(ID,MT);
103  csp.addAllDiff(ID,WY);
104  csp.addAllDiff(ID,UT);
105  csp.addAllDiff(ID,NV);
106  csp.addAllDiff(NV,UT);
107  csp.addAllDiff(NV,AZ);
108  csp.addAllDiff(UT,WY);
109  csp.addAllDiff(UT,CO);
110  csp.addAllDiff(UT,NM);
111  csp.addAllDiff(UT,AZ);
112  csp.addAllDiff(AZ,CO);
113  csp.addAllDiff(AZ,NM);
114  csp.addAllDiff(MT,WY);
115  csp.addAllDiff(WY,CO);
116  csp.addAllDiff(CO,NM);
117 
118  // Solve
119  Ordering ordering;
120  ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7),Key(8),Key(9),Key(10);
121  CSP::sharedValues mpe = csp.optimalAssignment(ordering);
122  // GTSAM_PRINT(*mpe);
123  CSP::Values expected;
124  insert(expected)
125  (WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0)
126  (MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2)
127  (ID.first,2)(UT.first,1)(AZ.first,0);
128 
129  // TODO: Fix me! mpe result seems to be right. (See the printing)
130  // It has the same prob as the expected solution.
131  // Is mpe another solution, or the expected solution is unique???
132  EXPECT(assert_equal(expected,*mpe));
133  EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
134 
135  // Write out the dual graph for hmetis
136 #ifdef DUAL
137  VariableIndexOrdered index(csp);
138  index.print("index");
139  ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/US-West-dual.txt");
140  index.outputMetisFormat(os);
141 #endif
142 }
143 
144 /* ************************************************************************* */
145 TEST_UNSAFE( CSP, AllDiff)
146 {
147  // Create keys and ordering
148  size_t nrColors = 3;
149  DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);
150 
151  // Create the CSP
152  CSP csp;
153  vector<DiscreteKey> dkeys;
154  dkeys += ID,UT,AZ;
155  csp.addAllDiff(dkeys);
156  csp.addSingleValue(AZ,2);
157 // GTSAM_PRINT(csp);
158 
159  // Check construction and conversion
160  SingleValue s(AZ,2);
161  DecisionTreeFactor f1(AZ,"0 0 1");
162  EXPECT(assert_equal(f1,s.toDecisionTreeFactor()));
163 
164  // Check construction and conversion
165  AllDiff alldiff(dkeys);
166  DecisionTreeFactor actual = alldiff.toDecisionTreeFactor();
167 // GTSAM_PRINT(actual);
168 // actual.dot("actual");
169  DecisionTreeFactor f2(ID & AZ & UT,
170  "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0");
171  EXPECT(assert_equal(f2,actual));
172 
173  // Check an invalid combination, with ID==UT==AZ all same color
174  DiscreteFactor::Values invalid;
175  invalid[ID.first] = 0;
176  invalid[UT.first] = 1;
177  invalid[AZ.first] = 0;
178  EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);
179 
180  // Check a valid combination
181  DiscreteFactor::Values valid;
182  valid[ID.first] = 0;
183  valid[UT.first] = 1;
184  valid[AZ.first] = 2;
185  EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);
186 
187  // Solve
188  CSP::sharedValues mpe = csp.optimalAssignment();
189  CSP::Values expected;
190  insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
191  EXPECT(assert_equal(expected,*mpe));
192  EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
193 
194  // Arc-consistency
195  vector<Domain> domains;
196  domains += Domain(ID), Domain(AZ), Domain(UT);
197  SingleValue singleValue(AZ,2);
198  EXPECT(singleValue.ensureArcConsistency(1,domains));
199  EXPECT(alldiff.ensureArcConsistency(0,domains));
200  EXPECT(!alldiff.ensureArcConsistency(1,domains));
201  EXPECT(alldiff.ensureArcConsistency(2,domains));
202  LONGS_EQUAL(2,domains[0].nrValues());
203  LONGS_EQUAL(1,domains[1].nrValues());
204  LONGS_EQUAL(2,domains[2].nrValues());
205 
206  // Parial application, version 1
207  DiscreteFactor::Values known;
208  known[AZ.first] = 2;
209  DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known);
210  DecisionTreeFactor f3(ID & UT, "0 1 1 1 0 1 1 1 0");
211  EXPECT(assert_equal(f3,reduced1->toDecisionTreeFactor()));
212  DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known);
213  DecisionTreeFactor f4(AZ, "0 0 1");
214  EXPECT(assert_equal(f4,reduced2->toDecisionTreeFactor()));
215 
216  // Parial application, version 2
217  DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains);
218  EXPECT(assert_equal(f3,reduced3->toDecisionTreeFactor()));
219  DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains);
220  EXPECT(assert_equal(f4,reduced4->toDecisionTreeFactor()));
221 
222  // full arc-consistency test
223  csp.runArcConsistency(nrColors);
224 }
225 
226 /* ************************************************************************* */
227 int main() {
228  TestResult tr;
229  return TestRegistry::runAllTests(tr);
230 }
231 /* ************************************************************************* */
232 
c2
static const Key c2
Definition: testSmartProjectionFactor.cpp:34
insert
A insert(1, 2)=0
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
ordering
static enum @843 ordering
gtsam::AllDiff::toDecisionTreeFactor
DecisionTreeFactor toDecisionTreeFactor() const override
Convert into a decisiontree, can be very expensive !
Definition: AllDiff.cpp:43
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
std
Definition: Half.h:150
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
gtsam::SingleValue::partiallyApply
Constraint::shared_ptr partiallyApply(const Values &values) const override
Partially apply known values.
Definition: SingleValue.cpp:62
TEST_UNSAFE
TEST_UNSAFE(BinaryAllDif, allInOne)
Definition: testCSP.cpp:20
OR
#define OR(a, b)
Definition: metis/libmetis/macros.h:22
gtsam::AllDiff::ensureArcConsistency
bool ensureArcConsistency(size_t j, std::vector< Domain > &domains) const override
Definition: AllDiff.cpp:62
gtsam::DecisionTreeFactor::toDecisionTreeFactor
DecisionTreeFactor toDecisionTreeFactor() const override
Convert into a decisiontree.
Definition: DecisionTreeFactor.h:98
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:34
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
gtsam::CSP::runArcConsistency
void runArcConsistency(size_t cardinality, size_t nrIterations=10, bool print=false) const
Definition: CSP.cpp:30
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
s
RealScalar s
Definition: level1_cplx_impl.h:104
gtsam::DiscreteFactor::shared_ptr
boost::shared_ptr< DiscreteFactor > shared_ptr
shared_ptr to this class
Definition: DiscreteFactor.h:40
gtsam::BinaryAllDiff::toDecisionTreeFactor
DecisionTreeFactor toDecisionTreeFactor() const override
Convert into a decisiontree.
Definition: BinaryAllDiff.h:58
main
int main()
Definition: testCSP.cpp:227
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
os
ofstream os("timeSchurFactors.csv")
gtsam::CSP::addAllDiff
void addAllDiff(const DiscreteKey &key1, const DiscreteKey &key2)
Add a binary AllDiff constraint.
Definition: CSP.h:42
unary::f1
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Definition: testExpression.cpp:82
f4
double f4(double x, double y, double z)
Definition: testNumericalDerivative.cpp:105
gtsam::AllDiff::partiallyApply
Constraint::shared_ptr partiallyApply(const Values &) const override
Partially apply known values.
Definition: AllDiff.cpp:88
gtsam::SingleValue::toDecisionTreeFactor
DecisionTreeFactor toDecisionTreeFactor() const override
Convert into a decisiontree.
Definition: SingleValue.cpp:31
f3
double f3(double x1, double x2)
Definition: testNumericalDerivative.cpp:76
ID
#define ID
Definition: ccolamd.c:637
gtsam::CSP
Definition: CSP.h:21
gtsam::CSP::sharedValues
boost::shared_ptr< Values > sharedValues
Definition: CSP.h:27
gtsam::CSP::addSingleValue
void addSingleValue(const DiscreteKey &dkey, size_t value)
Add a unary constraint, allowing only a single value.
Definition: CSP.h:36
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::DiscreteFactorGraph::product
DecisionTreeFactor product() const
Definition: DiscreteFactorGraph.cpp:50
gtsam::SingleValue::ensureArcConsistency
bool ensureArcConsistency(size_t j, std::vector< Domain > &domains) const override
Definition: SingleValue.cpp:48
c1
static const Key c1
Definition: testSmartProjectionFactor.cpp:34
gtsam::CSP::optimalAssignment
sharedValues optimalAssignment() const
Find the best total assignment - can be expensive.
Definition: CSP.cpp:17
product
void product(const MatrixType &m)
Definition: product.h:20
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition: LossFunctions.cpp:26

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:21