1/*
2 * testGenericGraph.cpp
3 *
4 * Created on: Nov 23, 2010
5 * Author: nikai
6 * Description: unit tests for generic graph
7 */
8
9#include <gtsam_unstable/partition/GenericGraph.h>
10
11#include <CppUnitLite/TestHarness.h>
12
13
14#include <map>
15
16using namespace std;
17using namespace gtsam;
18using namespace gtsam::partition;
19
20/* ************************************************************************* */
21/**
22 * l7 l9
23 * / | \ / |
24 * x1 -x2-x3 - l8 - x4- x5-x6
25 */
26TEST ( GenerciGraph, findIslands )
27{
28 GenericGraph2D graph;
29 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
30 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
31 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
32 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 8, args: NODE_LANDMARK_2D));
33 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 8, args: NODE_LANDMARK_2D));
34 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 9, args: NODE_LANDMARK_2D));
35 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 5, args: NODE_POSE_2D, args: 9, args: NODE_LANDMARK_2D));
36 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 6, args: NODE_POSE_2D, args: 9, args: NODE_LANDMARK_2D));
37
38 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 2, args: NODE_POSE_2D));
39 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 3, args: NODE_POSE_2D));
40 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 5, args: NODE_POSE_2D));
41 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 5, args: NODE_POSE_2D, args: 6, args: NODE_POSE_2D));
42 std::vector<size_t> keys{1, 2, 3, 4, 5, 6, 7, 8, 9};
43
44 WorkSpace workspace(10); // from 0 to 9
45 list<vector<size_t> > islands = findIslands(graph, keys, workspace, minNrConstraintsPerCamera: 7, minNrConstraintsPerLandmark: 2);
46 LONGS_EQUAL(2, islands.size());
47 vector<size_t> island1{1, 2, 3, 7, 8};
48 vector<size_t> island2{4, 5, 6, 9};
49 CHECK(island1 == islands.front());
50 CHECK(island2 == islands.back());
51}
52
53/* ************************************************************************* */
54/**
55 * l7 l8
56 * / / | X | \
57 * x1 -x2-x3 x4- x5-x6
58 */
59TEST( GenerciGraph, findIslands2 )
60{
61 GenericGraph2D graph;
62 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
63 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
64 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
65 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 8, args: NODE_LANDMARK_2D));
66 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
67 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 8, args: NODE_LANDMARK_2D));
68 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 5, args: NODE_POSE_2D, args: 8, args: NODE_LANDMARK_2D));
69 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 6, args: NODE_POSE_2D, args: 8, args: NODE_LANDMARK_2D));
70
71 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 2, args: NODE_POSE_2D));
72 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 3, args: NODE_POSE_2D));
73 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 5, args: NODE_POSE_2D));
74 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 5, args: NODE_POSE_2D, args: 6, args: NODE_POSE_2D));
75 std::vector<size_t> keys{1, 2, 3, 4, 5, 6, 7, 8};
76
77 WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
78 list<vector<size_t> > islands = findIslands(graph, keys, workspace, minNrConstraintsPerCamera: 7, minNrConstraintsPerLandmark: 2);
79 LONGS_EQUAL(1, islands.size());
80 vector<size_t> island1{1, 2, 3, 4, 5, 6, 7, 8};
81 CHECK(island1 == islands.front());
82}
83
84/* ************************************************************************* */
85/**
86 * x1 - l5
87 * x2 - x3 - x4 - l6
88 */
89TEST ( GenerciGraph, findIslands3 )
90{
91 GenericGraph2D graph;
92 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 5, args: NODE_LANDMARK_2D));
93 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 6, args: NODE_LANDMARK_2D));
94
95 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 3, args: NODE_POSE_2D));
96 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 4, args: NODE_POSE_2D));
97 std::vector<size_t> keys{1, 2, 3, 4, 5, 6};
98
99 WorkSpace workspace(7); // from 0 to 9
100 list<vector<size_t> > islands = findIslands(graph, keys, workspace, minNrConstraintsPerCamera: 7, minNrConstraintsPerLandmark: 2);
101 LONGS_EQUAL(2, islands.size());
102 vector<size_t> island1{1, 5};
103 vector<size_t> island2{2, 3, 4, 6};
104 CHECK(island1 == islands.front());
105 CHECK(island2 == islands.back());
106}
107
108/* ************************************************************************* */
109/**
110 * x3 - l4 - x7
111 */
112TEST ( GenerciGraph, findIslands4 )
113{
114 GenericGraph2D graph;
115 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 4, args: NODE_LANDMARK_2D));
116 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 7, args: NODE_POSE_2D, args: 7, args: NODE_LANDMARK_2D));
117 std::vector<size_t> keys{3, 4, 7};
118
119 WorkSpace workspace(8); // from 0 to 7
120 list<vector<size_t> > islands = findIslands(graph, keys, workspace, minNrConstraintsPerCamera: 7, minNrConstraintsPerLandmark: 2);
121 LONGS_EQUAL(2, islands.size());
122 vector<size_t> island1{3, 4};
123 vector<size_t> island2{7};
124 CHECK(island1 == islands.front());
125 CHECK(island2 == islands.back());
126}
127
128/* ************************************************************************* */
129/**
130 * x1 - l5 - x2
131 * | / \ |
132 * x3 x4
133 */
134TEST ( GenerciGraph, findIslands5 )
135{
136 GenericGraph2D graph;
137 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 5, args: NODE_LANDMARK_2D));
138 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 5, args: NODE_LANDMARK_2D));
139 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 3, args: NODE_POSE_2D, args: 5, args: NODE_LANDMARK_2D));
140 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 4, args: NODE_POSE_2D, args: 5, args: NODE_LANDMARK_2D));
141
142 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 1, args: NODE_POSE_2D, args: 3, args: NODE_POSE_2D));
143 graph.push_back(x: std::make_shared<GenericFactor2D>(args: 2, args: NODE_POSE_2D, args: 4, args: NODE_POSE_2D));
144
145 std::vector<size_t> keys{1, 2, 3, 4, 5};
146
147 WorkSpace workspace(6); // from 0 to 5
148 list<vector<size_t> > islands = findIslands(graph, keys, workspace, minNrConstraintsPerCamera: 7, minNrConstraintsPerLandmark: 2);
149 LONGS_EQUAL(2, islands.size());
150 vector<size_t> island1{1, 3, 5};
151 vector<size_t> island2{2, 4};
152 CHECK(island1 == islands.front());
153 CHECK(island2 == islands.back());
154}
155
156/* ************************************************************************* */
157/**
158 * l3 l4 l5 l6
159 * \ | / \ /
160 * x1 x2
161 */
162TEST ( GenerciGraph, reduceGenericGraph )
163{
164 GenericGraph3D graph;
165 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 1, args: 3));
166 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 1, args: 4));
167 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 1, args: 5));
168 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 2, args: 5));
169 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 2, args: 6));
170
171 std::vector<size_t> cameraKeys, landmarkKeys;
172 cameraKeys.push_back(x: 1);
173 cameraKeys.push_back(x: 2);
174 landmarkKeys.push_back(x: 3);
175 landmarkKeys.push_back(x: 4);
176 landmarkKeys.push_back(x: 5);
177 landmarkKeys.push_back(x: 6);
178
179 std::vector<int> dictionary;
180 dictionary.resize(new_size: 7, x: -1); // from 0 to 6
181 dictionary[1] = 0;
182 dictionary[2] = 1;
183
184 GenericGraph3D reduced;
185 std::map<size_t, vector<size_t> > cameraToLandmarks;
186 reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph&: reduced);
187 LONGS_EQUAL(1, reduced.size());
188 LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
189}
190
191/* ************************************************************************* */
192/**
193 * l3 l4 l5 l6
194 * \ | / \ /
195 * x1 x2 - x7
196 */
197TEST ( GenericGraph, reduceGenericGraph2 )
198{
199 GenericGraph3D graph;
200 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 1, args: 3, args: 0, args: NODE_POSE_3D, args: NODE_LANDMARK_3D));
201 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 1, args: 4, args: 1, args: NODE_POSE_3D, args: NODE_LANDMARK_3D));
202 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 1, args: 5, args: 2, args: NODE_POSE_3D, args: NODE_LANDMARK_3D));
203 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 2, args: 5, args: 3, args: NODE_POSE_3D, args: NODE_LANDMARK_3D));
204 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 2, args: 6, args: 4, args: NODE_POSE_3D, args: NODE_LANDMARK_3D));
205 graph.push_back(x: std::make_shared<GenericFactor3D>(args: 2, args: 7, args: 5, args: NODE_POSE_3D, args: NODE_POSE_3D));
206
207 std::vector<size_t> cameraKeys, landmarkKeys;
208 cameraKeys.push_back(x: 1);
209 cameraKeys.push_back(x: 2);
210 cameraKeys.push_back(x: 7);
211 landmarkKeys.push_back(x: 3);
212 landmarkKeys.push_back(x: 4);
213 landmarkKeys.push_back(x: 5);
214 landmarkKeys.push_back(x: 6);
215
216 std::vector<int> dictionary;
217 dictionary.resize(new_size: 8, x: -1); // from 0 to 7
218 dictionary[1] = 0;
219 dictionary[2] = 1;
220 dictionary[7] = 6;
221
222 GenericGraph3D reduced;
223 std::map<size_t, vector<size_t> > cameraToLandmarks;
224 reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph&: reduced);
225 LONGS_EQUAL(2, reduced.size());
226 LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
227 LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
228}
229
230/* ************************************************************************* */
231TEST ( GenerciGraph, hasCommonCamera )
232{
233 std::set<size_t> cameras1{1, 2, 3, 4, 5};
234 std::set<size_t> cameras2{8, 7, 6, 5};
235 bool actual = hasCommonCamera(cameras1, cameras2);
236 CHECK(actual);
237}
238
239/* ************************************************************************* */
240TEST ( GenerciGraph, hasCommonCamera2 )
241{
242 std::set<size_t> cameras1{1, 3, 5, 7};
243 std::set<size_t> cameras2{2, 4, 6, 8, 10};
244 bool actual = hasCommonCamera(cameras1, cameras2);
245 CHECK(!actual);
246}
247
248/* ************************************************************************* */
249int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr);}
250/* ************************************************************************* */
251