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 
14 #include <map>
15 
16 using namespace std;
17 using namespace gtsam;
18 using namespace gtsam::partition;
19 
20 /* ************************************************************************* */
26 TEST ( GenerciGraph, findIslands )
27 {
28  GenericGraph2D graph;
29  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
30  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
31  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
32  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
33  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
34  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
35  graph.push_back(std::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
36  graph.push_back(std::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
37 
38  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
39  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
40  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
41  graph.push_back(std::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, 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, 7, 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 /* ************************************************************************* */
59 TEST( GenerciGraph, findIslands2 )
60 {
61  GenericGraph2D graph;
62  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
63  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
64  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
65  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
66  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
67  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
68  graph.push_back(std::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
69  graph.push_back(std::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
70 
71  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
72  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
73  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
74  graph.push_back(std::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, 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, 7, 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 /* ************************************************************************* */
89 TEST ( GenerciGraph, findIslands3 )
90 {
91  GenericGraph2D graph;
92  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
93  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
94 
95  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
96  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, 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, 7, 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 /* ************************************************************************* */
112 TEST ( GenerciGraph, findIslands4 )
113 {
114  GenericGraph2D graph;
115  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
116  graph.push_back(std::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, 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, 7, 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 /* ************************************************************************* */
134 TEST ( GenerciGraph, findIslands5 )
135 {
136  GenericGraph2D graph;
137  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
138  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
139  graph.push_back(std::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
140  graph.push_back(std::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
141 
142  graph.push_back(std::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
143  graph.push_back(std::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, 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, 7, 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 /* ************************************************************************* */
162 TEST ( GenerciGraph, reduceGenericGraph )
163 {
164  GenericGraph3D graph;
165  graph.push_back(std::make_shared<GenericFactor3D>(1, 3));
166  graph.push_back(std::make_shared<GenericFactor3D>(1, 4));
167  graph.push_back(std::make_shared<GenericFactor3D>(1, 5));
168  graph.push_back(std::make_shared<GenericFactor3D>(2, 5));
169  graph.push_back(std::make_shared<GenericFactor3D>(2, 6));
170 
171  std::vector<size_t> cameraKeys, landmarkKeys;
172  cameraKeys.push_back(1);
173  cameraKeys.push_back(2);
174  landmarkKeys.push_back(3);
175  landmarkKeys.push_back(4);
176  landmarkKeys.push_back(5);
177  landmarkKeys.push_back(6);
178 
179  std::vector<int> dictionary;
180  dictionary.resize(7, -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, 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 /* ************************************************************************* */
197 TEST ( GenericGraph, reduceGenericGraph2 )
198 {
199  GenericGraph3D graph;
200  graph.push_back(std::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
201  graph.push_back(std::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
202  graph.push_back(std::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
203  graph.push_back(std::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
204  graph.push_back(std::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
205  graph.push_back(std::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
206 
207  std::vector<size_t> cameraKeys, landmarkKeys;
208  cameraKeys.push_back(1);
209  cameraKeys.push_back(2);
210  cameraKeys.push_back(7);
211  landmarkKeys.push_back(3);
212  landmarkKeys.push_back(4);
213  landmarkKeys.push_back(5);
214  landmarkKeys.push_back(6);
215 
216  std::vector<int> dictionary;
217  dictionary.resize(8, -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, 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 /* ************************************************************************* */
231 TEST ( 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 /* ************************************************************************* */
240 TEST ( 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 /* ************************************************************************* */
249 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
250 /* ************************************************************************* */
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:108
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
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
std
Definition: BFloat16.h:88
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::partition::GenericGraph2D
std::vector< sharedGenericFactor2D > GenericGraph2D
Definition: GenericGraph.h:49
gtsam::partition::GenericGraph3D
std::vector< sharedGenericFactor3D > GenericGraph3D
Definition: GenericGraph.h:97
key1
const Symbol key1('v', 1)
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:26
gtsam::Symbol::index
std::uint64_t index() const
Definition: inference/Symbol.h:80
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam
traits
Definition: chartTesting.h:28
main
int main()
Definition: testGenericGraph.cpp:249
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
key2
const Symbol key2('v', 2)

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:17