1/*
2 * GenericGraph2D.cpp
3 *
4 * Created on: Nov 23, 2010
5 * Author: nikai
6 * Description: generic graph types used in partitioning
7 */
8#include <iostream>
9#include <cassert>
10#include <algorithm>
11
12#include <gtsam/base/DSFVector.h>
13
14#include "GenericGraph.h"
15
16using namespace std;
17
18namespace gtsam { namespace partition {
19
20 /**
21 * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
22 */
23 list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
24 const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
25 {
26 typedef pair<int, int> IntPair;
27 typedef list<sharedGenericFactor2D> FactorList;
28 typedef map<IntPair, FactorList::iterator> Connections;
29
30 // create disjoin set forest
31 DSFVector dsf(workspace.dsf, keys);
32
33 FactorList factors(graph.begin(), graph.end());
34 size_t nrFactors = factors.size();
35 FactorList::iterator itEnd;
36 workspace.prepareDictionary(keys);
37 while (nrFactors) {
38 Connections connections;
39 bool succeed = false;
40 itEnd = factors.end();
41 list<FactorList::iterator> toErase;
42 for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
43
44 // remove invalid factors
45 GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
46 if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
47 toErase.push_back(x: itFactor); nrFactors--; continue;
48 }
49
50 size_t label1 = dsf.find(key: key1.index);
51 size_t label2 = dsf.find(key: key2.index);
52 if (label1 == label2) { toErase.push_back(x: itFactor); nrFactors--; continue; }
53
54 // merge two trees if the connection is strong enough, otherwise cache it
55 // an odometry factor always merges two islands
56 if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) {
57 toErase.push_back(x: itFactor); nrFactors--;
58 dsf.merge(i1: label1, i2: label2);
59 succeed = true;
60 break;
61 }
62
63 // single landmark island only need one measurement
64 if ((dsf.isSingleton(label: label1)==1 && key1.type == NODE_LANDMARK_2D) ||
65 (dsf.isSingleton(label: label2)==1 && key2.type == NODE_LANDMARK_2D)) {
66 toErase.push_back(x: itFactor); nrFactors--;
67 dsf.merge(i1: label1, i2: label2);
68 succeed = true;
69 break;
70 }
71
72 // stack the current factor with the cached constraint
73 IntPair labels = (label1 < label2) ? make_pair(x&: label1, y&: label2) : make_pair(x&: label2, y&: label1);
74 Connections::iterator itCached = connections.find(x: labels);
75 if (itCached == connections.end()) {
76 connections.insert(x: make_pair(x&: labels, y&: itFactor));
77 continue;
78 } else {
79 GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
80 // if observe the same landmark, we can not merge, abandon the current factor
81 if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
82 (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
83 (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
84 (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
85 toErase.push_back(x: itFactor); nrFactors--;
86 continue;
87 } else {
88 toErase.push_back(x: itFactor); nrFactors--;
89 toErase.push_back(x: itCached->second); nrFactors--;
90 dsf.merge(i1: label1, i2: label2);
91 connections.erase(position: itCached);
92 succeed = true;
93 break;
94 }
95 }
96 }
97
98 // erase unused factors
99 for(const FactorList::iterator& it: toErase)
100 factors.erase(position: it);
101
102 if (!succeed) break;
103 }
104
105 list<vector<size_t> > islands;
106 map<size_t, vector<size_t> > arrays = dsf.arrays();
107 for(const auto& kv : arrays)
108 islands.push_back(x: kv.second);
109 return islands;
110 }
111
112
113 /* ************************************************************************* */
114 void print(const GenericGraph2D& graph, const std::string name) {
115 cout << name << endl;
116 for(const sharedGenericFactor2D& factor_: graph)
117 cout << factor_->key1.index << " " << factor_->key2.index << endl;
118 }
119
120 /* ************************************************************************* */
121 void print(const GenericGraph3D& graph, const std::string name) {
122 cout << name << endl;
123 for(const sharedGenericFactor3D& factor_: graph)
124 cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
125 factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
126 }
127
128 /* ************************************************************************* */
129 // create disjoin set forest
130 DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
131 DSFVector dsf(workspace.dsf, keys);
132 typedef list<sharedGenericFactor3D> FactorList;
133
134 FactorList factors(graph.begin(), graph.end());
135 size_t nrFactors = factors.size();
136 FactorList::iterator itEnd;
137 while (nrFactors) {
138
139 bool succeed = false;
140 itEnd = factors.end();
141 list<FactorList::iterator> toErase;
142 for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
143
144 // remove invalid factors
145 if (graph.size() == 178765) cout << "kai21" << endl;
146 GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
147 if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
148 if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
149 toErase.push_back(x: itFactor); nrFactors--; continue;
150 }
151
152 if (graph.size() == 178765) cout << "kai22" << endl;
153 size_t label1 = dsf.find(key: key1.index);
154 size_t label2 = dsf.find(key: key2.index);
155 if (label1 == label2) { toErase.push_back(x: itFactor); nrFactors--; continue; }
156
157 if (graph.size() == 178765) cout << "kai23" << endl;
158 // merge two trees if the connection is strong enough, otherwise cache it
159 // an odometry factor always merges two islands
160 if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) ||
161 (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) {
162 toErase.push_back(x: itFactor); nrFactors--;
163 dsf.merge(i1: label1, i2: label2);
164 succeed = true;
165 break;
166 }
167
168 if (graph.size() == 178765) cout << "kai24" << endl;
169
170
171 }
172
173 // erase unused factors
174 for(const FactorList::iterator& it: toErase)
175 factors.erase(position: it);
176
177 if (!succeed) break;
178 }
179 return dsf;
180 }
181
182 /* ************************************************************************* */
183 // first check the type of the key (pose or landmark), and then check whether it is singular
184 inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
185 switch(node.type) {
186 case NODE_POSE_3D:
187 return singularCameras.find(x: node.index) != singularCameras.end(); break;
188 case NODE_LANDMARK_3D:
189 return singularLandmarks.find(x: node.index) != singularLandmarks.end(); break;
190 default:
191 throw runtime_error("unrecognized key type!");
192 }
193 }
194
195 /* ************************************************************************* */
196 void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
197 const vector<bool>& isCamera, const vector<bool>& isLandmark,
198 set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
199 bool& foundSingularCamera, bool& foundSingularLandmark,
200 const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
201
202 // compute the constraint number per camera
203 std::fill(first: nrConstraints.begin(), last: nrConstraints.end(), value: 0);
204 for(const sharedGenericFactor3D& factor_: graph) {
205 const int& key1 = factor_->key1.index;
206 const int& key2 = factor_->key2.index;
207 if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
208 !isSingular(singularCameras, singularLandmarks, node: factor_->key1) &&
209 !isSingular(singularCameras, singularLandmarks, node: factor_->key2)) {
210 nrConstraints[key1]++;
211 nrConstraints[key2]++;
212
213 // a single pose constraint is sufficient for stereo, so we add 2 to the counter
214 // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
215 if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
216 nrConstraints[key1]+=2;
217 nrConstraints[key2]+=2;
218 }
219 }
220 }
221
222 // find singular cameras and landmarks
223 foundSingularCamera = false;
224 foundSingularLandmark = false;
225 for (size_t i=0; i<nrConstraints.size(); i++) {
226 if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
227 singularCameras.find(x: i) == singularCameras.end()) {
228 singularCameras.insert(x: i);
229 foundSingularCamera = true;
230 }
231 if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
232 singularLandmarks.find(x: i) == singularLandmarks.end()) {
233 singularLandmarks.insert(x: i);
234 foundSingularLandmark = true;
235 }
236 }
237 }
238
239 /* ************************************************************************* */
240 list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
241 const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
242
243 // create disjoint set forest
244 workspace.prepareDictionary(keys);
245 DSFVector dsf = createDSF(graph, keys, workspace);
246
247 const bool verbose = false;
248 bool foundSingularCamera = true;
249 bool foundSingularLandmark = true;
250
251 list<vector<size_t> > islands;
252 set<size_t> singularCameras, singularLandmarks;
253 vector<bool> isCamera(workspace.dictionary.size(), false);
254 vector<bool> isLandmark(workspace.dictionary.size(), false);
255
256 // check the constraint number of every variable
257 // find the camera and landmark keys
258 for(const sharedGenericFactor3D& factor_: graph) {
259 //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
260 if (workspace.dictionary[factor_->key1.index] != -1) {
261 if (factor_->key1.type == NODE_POSE_3D)
262 isCamera[factor_->key1.index] = true;
263 else
264 isLandmark[factor_->key1.index] = true;
265 }
266 if (workspace.dictionary[factor_->key2.index] != -1) {
267 if (factor_->key2.type == NODE_POSE_3D)
268 isCamera[factor_->key2.index] = true;
269 else
270 isLandmark[factor_->key2.index] = true;
271 }
272 }
273
274 vector<int> nrConstraints(workspace.dictionary.size(), 0);
275 // iterate until all singular variables have been removed. Removing a singular variable
276 // can cause another to become singular, so this will probably run several times
277 while (foundSingularCamera || foundSingularLandmark) {
278 findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input
279 singularCameras, singularLandmarks, nrConstraints, // output
280 foundSingularCamera, foundSingularLandmark, // output
281 minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input
282 }
283
284 // add singular variables directly as islands
285 if (!singularCameras.empty()) {
286 if (verbose) cout << "singular cameras:";
287 for(const size_t i: singularCameras) {
288 islands.push_back(x: vector<size_t>(1, i)); // <---------------------------
289 if (verbose) cout << i << " ";
290 }
291 if (verbose) cout << endl;
292 }
293 if (!singularLandmarks.empty()) {
294 if (verbose) cout << "singular landmarks:";
295 for(const size_t i: singularLandmarks) {
296 islands.push_back(x: vector<size_t>(1, i)); // <---------------------------
297 if (verbose) cout << i << " ";
298 }
299 if (verbose) cout << endl;
300 }
301
302
303 // regenerating islands
304 map<size_t, vector<size_t> > labelIslands = dsf.arrays();
305 size_t label; vector<size_t> island;
306 for(const auto& li: labelIslands) {
307 tie(args&: label, args&: island) = li;
308 vector<size_t> filteredIsland; // remove singular cameras from array
309 filteredIsland.reserve(n: island.size());
310 for(const size_t key: island) {
311 if ((isCamera[key] && singularCameras.find(x: key) == singularCameras.end()) || // not singular
312 (isLandmark[key] && singularLandmarks.find(x: key) == singularLandmarks.end()) || // not singular
313 (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined
314 filteredIsland.push_back(x: key);
315 }
316 }
317 islands.push_back(x: filteredIsland);
318 }
319
320 // sanity check
321 size_t nrKeys = 0;
322 for(const vector<size_t>& island: islands)
323 nrKeys += island.size();
324 if (nrKeys != keys.size()) {
325 cout << nrKeys << " vs " << keys.size() << endl;
326 throw runtime_error("findIslands: the number of keys is inconsistent!");
327 }
328
329
330 if (verbose) cout << "found " << islands.size() << " islands!" << endl;
331 return islands;
332 }
333
334 /* ************************************************************************* */
335 // return the number of intersection between two **sorted** landmark vectors
336 inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
337 size_t i1 = 0, i2 = 0;
338 int nrCommonLandmarks = 0;
339 while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
340 if (landmarks1[i1] < landmarks2[i2])
341 i1 ++;
342 else if (landmarks1[i1] > landmarks2[i2])
343 i2 ++;
344 else {
345 i1++; i2++;
346 nrCommonLandmarks ++;
347 }
348 }
349 return nrCommonLandmarks;
350 }
351
352 /* ************************************************************************* */
353 void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
354 const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
355
356 typedef size_t LandmarkKey;
357 // get a mapping from each landmark to its connected cameras
358 vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
359 // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
360 vector<int> cameraToCamera(dictionary.size(), -1);
361 size_t key_i, key_j;
362 for(const sharedGenericFactor3D& factor_: graph) {
363 if (factor_->key1.type == NODE_POSE_3D) {
364 if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
365 cameraToLandmarks[factor_->key1.index].push_back(x: factor_->key2.index);
366 }
367 else { // odometry factor
368 if (factor_->key1.index < factor_->key2.index) {
369 key_i = factor_->key1.index;
370 key_j = factor_->key2.index;
371 } else {
372 key_i = factor_->key2.index;
373 key_j = factor_->key1.index;
374 }
375 cameraToCamera[key_i] = key_j;
376 }
377 }
378 }
379
380 // sort the landmark keys for the late getNrCommonLandmarks call
381 for(vector<LandmarkKey> &landmarks: cameraToLandmarks){
382 if (!landmarks.empty())
383 std::sort(first: landmarks.begin(), last: landmarks.end());
384 }
385
386 // generate the reduced graph
387 reducedGraph.clear();
388 int factorIndex = 0;
389 int camera1, camera2, nrTotalConstraints;
390 bool hasOdometry;
391 for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
392 for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
393 camera1 = cameraKeys[i1];
394 camera2 = cameraKeys[i2];
395 int nrCommonLandmarks = getNrCommonLandmarks(landmarks1: cameraToLandmarks[camera1], landmarks2: cameraToLandmarks[camera2]);
396 hasOdometry = cameraToCamera[camera1] == camera2;
397 if (nrCommonLandmarks > 0 || hasOdometry) {
398 nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
399 reducedGraph.push_back(x: std::make_shared<GenericFactor3D>(args&: camera1, args&: camera2,
400 args: factorIndex++, args: NODE_POSE_3D, args: NODE_POSE_3D, args&: nrTotalConstraints));
401 }
402 }
403 }
404 }
405
406 /* ************************************************************************* */
407 void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
408 WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
409 workspace.prepareDictionary(keys: frontals);
410 vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
411
412 // summarize the constraint number
413 const vector<int>& dictionary = workspace.dictionary;
414 vector<bool> isValidCamera(workspace.dictionary.size(), false);
415 vector<bool> isValidLandmark(workspace.dictionary.size(), false);
416 for(const sharedGenericFactor3D& factor_: graph) {
417 assert(factor_->key1.type == NODE_POSE_3D);
418 //assert(factor_->key2.type == NODE_LANDMARK_3D);
419 const size_t& key1 = factor_->key1.index;
420 const size_t& key2 = factor_->key2.index;
421 if (dictionary[key1] == -1 || dictionary[key2] == -1)
422 continue;
423
424 isValidCamera[key1] = true;
425 if(factor_->key2.type == NODE_LANDMARK_3D)
426 isValidLandmark[key2] = true;
427 else
428 isValidCamera[key2] = true;
429
430 nrConstraints[key1]++;
431 nrConstraints[key2]++;
432
433 // a single pose constraint is sufficient for stereo, so we add 2 to the counter
434 // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
435 if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
436 nrConstraints[key1]+=2;
437 nrConstraints[key2]+=2;
438 }
439 }
440
441 // find the minimum constraint for cameras and landmarks
442 size_t minFoundConstraintsPerCamera = 10000;
443 size_t minFoundConstraintsPerLandmark = 10000;
444
445 for (size_t i=0; i<isValidCamera.size(); i++) {
446 if (isValidCamera[i]) {
447 minFoundConstraintsPerCamera = std::min(a: nrConstraints[i], b: minFoundConstraintsPerCamera);
448 if (nrConstraints[i] < minNrConstraintsPerCamera)
449 cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
450 }
451
452 }
453 for (size_t j=0; j<isValidLandmark.size(); j++) {
454 if (isValidLandmark[j]) {
455 minFoundConstraintsPerLandmark = std::min(a: nrConstraints[j], b: minFoundConstraintsPerLandmark);
456 if (nrConstraints[j] < minNrConstraintsPerLandmark)
457 cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
458 }
459 }
460
461 // debug info
462 for(const size_t key: frontals) {
463 if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
464 cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
465 }
466
467 if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
468 throw runtime_error("checkSingularity:minConstraintsPerCamera < " + std::to_string(val: minFoundConstraintsPerCamera));
469 if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
470 throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + std::to_string(val: minFoundConstraintsPerLandmark));
471 }
472
473}} // namespace
474