testGenericGraph.cpp
Go to the documentation of this file.
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 #include <boost/assign/std/list.hpp> // for operator +=
14 #include <boost/assign/std/set.hpp> // for operator +=
15 #include <boost/assign/std/vector.hpp> // for operator +=
16 using namespace boost::assign;
17 #include <boost/make_shared.hpp>
18 
19 #include <map>
20 
21 using namespace std;
22 using namespace gtsam;
23 using namespace gtsam::partition;
24 
25 /* ************************************************************************* */
31 TEST ( GenerciGraph, findIslands )
32 {
33  GenericGraph2D graph;
34  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
35  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
36  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
37  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
38  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
39  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
40  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
41  graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
42 
43  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
44  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
45  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
46  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
47  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
48 
49  WorkSpace workspace(10); // from 0 to 9
50  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
51  LONGS_EQUAL(2, islands.size());
52  vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
53  vector<size_t> island2; island2 += 4, 5, 6, 9;
54  CHECK(island1 == islands.front());
55  CHECK(island2 == islands.back());
56 }
57 
58 /* ************************************************************************* */
64 TEST( GenerciGraph, findIslands2 )
65 {
66  GenericGraph2D graph;
67  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
68  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
69  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
70  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
71  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
72  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
73  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
74  graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
75 
76  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
77  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
78  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
79  graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
80  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
81 
82  WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
83  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
84  LONGS_EQUAL(1, islands.size());
85  vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
86  CHECK(island1 == islands.front());
87 }
88 
89 /* ************************************************************************* */
94 TEST ( GenerciGraph, findIslands3 )
95 {
96  GenericGraph2D graph;
97  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
98  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
99 
100  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
101  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
102  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
103 
104  WorkSpace workspace(7); // from 0 to 9
105  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
106  LONGS_EQUAL(2, islands.size());
107  vector<size_t> island1; island1 += 1, 5;
108  vector<size_t> island2; island2 += 2, 3, 4, 6;
109  CHECK(island1 == islands.front());
110  CHECK(island2 == islands.back());
111 }
112 
113 /* ************************************************************************* */
117 TEST ( GenerciGraph, findIslands4 )
118 {
119  GenericGraph2D graph;
120  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
121  graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
122  std::vector<size_t> keys; keys += 3, 4, 7;
123 
124  WorkSpace workspace(8); // from 0 to 7
125  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
126  LONGS_EQUAL(2, islands.size());
127  vector<size_t> island1; island1 += 3, 4;
128  vector<size_t> island2; island2 += 7;
129  CHECK(island1 == islands.front());
130  CHECK(island2 == islands.back());
131 }
132 
133 /* ************************************************************************* */
139 TEST ( GenerciGraph, findIslands5 )
140 {
141  GenericGraph2D graph;
142  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
143  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
144  graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
145  graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
146 
147  graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
148  graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
149 
150  std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
151 
152  WorkSpace workspace(6); // from 0 to 5
153  list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
154  LONGS_EQUAL(2, islands.size());
155  vector<size_t> island1; island1 += 1, 3, 5;
156  vector<size_t> island2; island2 += 2, 4;
157  CHECK(island1 == islands.front());
158  CHECK(island2 == islands.back());
159 }
160 
161 /* ************************************************************************* */
167 TEST ( GenerciGraph, reduceGenericGraph )
168 {
169  GenericGraph3D graph;
170  graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
171  graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
172  graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
173  graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
174  graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
175 
176  std::vector<size_t> cameraKeys, landmarkKeys;
177  cameraKeys.push_back(1);
178  cameraKeys.push_back(2);
179  landmarkKeys.push_back(3);
180  landmarkKeys.push_back(4);
181  landmarkKeys.push_back(5);
182  landmarkKeys.push_back(6);
183 
184  std::vector<int> dictionary;
185  dictionary.resize(7, -1); // from 0 to 6
186  dictionary[1] = 0;
187  dictionary[2] = 1;
188 
189  GenericGraph3D reduced;
190  std::map<size_t, vector<size_t> > cameraToLandmarks;
191  reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
192  LONGS_EQUAL(1, reduced.size());
193  LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
194 }
195 
196 /* ************************************************************************* */
202 TEST ( GenericGraph, reduceGenericGraph2 )
203 {
204  GenericGraph3D graph;
205  graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
206  graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
207  graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
208  graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
209  graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
210  graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
211 
212  std::vector<size_t> cameraKeys, landmarkKeys;
213  cameraKeys.push_back(1);
214  cameraKeys.push_back(2);
215  cameraKeys.push_back(7);
216  landmarkKeys.push_back(3);
217  landmarkKeys.push_back(4);
218  landmarkKeys.push_back(5);
219  landmarkKeys.push_back(6);
220 
221  std::vector<int> dictionary;
222  dictionary.resize(8, -1); // from 0 to 7
223  dictionary[1] = 0;
224  dictionary[2] = 1;
225  dictionary[7] = 6;
226 
227  GenericGraph3D reduced;
228  std::map<size_t, vector<size_t> > cameraToLandmarks;
229  reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
230  LONGS_EQUAL(2, reduced.size());
231  LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
232  LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
233 }
234 
235 /* ************************************************************************* */
236 TEST ( GenerciGraph, hasCommonCamera )
237 {
238  std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
239  std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
240  bool actual = hasCommonCamera(cameras1, cameras2);
241  CHECK(actual);
242 }
243 
244 /* ************************************************************************* */
245 TEST ( GenerciGraph, hasCommonCamera2 )
246 {
247  std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
248  std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
249  bool actual = hasCommonCamera(cameras1, cameras2);
250  CHECK(!actual);
251 }
252 
253 /* ************************************************************************* */
254 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
255 /* ************************************************************************* */
gtsam::partition::findIslands
list< vector< size_t > > findIslands(const GenericGraph2D &graph, const vector< size_t > &keys, WorkSpace &workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
Definition: GenericGraph.cpp:23
CHECK
#define CHECK(condition)
Definition: Test.h:109
gtsam::partition::hasCommonCamera
bool hasCommonCamera(const std::set< size_t > &cameras1, const std::set< size_t > &cameras2)
Definition: GenericGraph.h:135
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
std
Definition: Half.h:150
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:166
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
key1
const Symbol key1('v', 1)
gtsam::partition::GenericGraph2D
std::vector< sharedGenericFactor2D > GenericGraph2D
Definition: GenericGraph.h:49
gtsam::partition::GenericGraph3D
std::vector< sharedGenericFactor3D > GenericGraph3D
Definition: GenericGraph.h:97
gtsam::partition::reduceGenericGraph
void reduceGenericGraph(const GenericGraph3D &graph, const std::vector< size_t > &cameraKeys, const std::vector< size_t > &landmarkKeys, const std::vector< int > &dictionary, GenericGraph3D &reducedGraph)
Definition: GenericGraph.cpp:353
TEST
TEST(GenerciGraph, findIslands)
Definition: testGenericGraph.cpp:31
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam
traits
Definition: chartTesting.h:28
main
int main()
Definition: testGenericGraph.cpp:254
key2
const Symbol key2('v', 2)
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:45
gtsam::Symbol::index
std::uint64_t index() const
Definition: inference/Symbol.h:77

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:47:03