GenericGraph.cpp
Go to the documentation of this file.
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 
16 using namespace std;
17 
18 namespace gtsam { namespace partition {
19 
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(itFactor); nrFactors--; continue;
48  }
49 
50  size_t label1 = dsf.find(key1.index);
51  size_t label2 = dsf.find(key2.index);
52  if (label1 == label2) { toErase.push_back(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(itFactor); nrFactors--;
58  dsf.merge(label1, label2);
59  succeed = true;
60  break;
61  }
62 
63  // single landmark island only need one measurement
64  if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
65  (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
66  toErase.push_back(itFactor); nrFactors--;
67  dsf.merge(label1, label2);
68  succeed = true;
69  break;
70  }
71 
72  // stack the current factor with the cached constraint
73  IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
74  Connections::iterator itCached = connections.find(labels);
75  if (itCached == connections.end()) {
76  connections.insert(make_pair(labels, 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(itFactor); nrFactors--;
86  continue;
87  } else {
88  toErase.push_back(itFactor); nrFactors--;
89  toErase.push_back(itCached->second); nrFactors--;
90  dsf.merge(label1, label2);
91  connections.erase(itCached);
92  succeed = true;
93  break;
94  }
95  }
96  }
97 
98  // erase unused factors
99  for(const FactorList::iterator& it: toErase)
100  factors.erase(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(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(itFactor); nrFactors--; continue;
150  }
151 
152  if (graph.size() == 178765) cout << "kai22" << endl;
153  size_t label1 = dsf.find(key1.index);
154  size_t label2 = dsf.find(key2.index);
155  if (label1 == label2) { toErase.push_back(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(itFactor); nrFactors--;
163  dsf.merge(label1, 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(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(node.index) != singularCameras.end(); break;
188  case NODE_LANDMARK_3D:
189  return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
190  default:
191  throw runtime_error("unrecognized key type!");
192  }
193  }
194 
195  /* ************************************************************************* */
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(nrConstraints.begin(), nrConstraints.end(), 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, factor_->key1) &&
209  !isSingular(singularCameras, singularLandmarks, 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(i) == singularCameras.end()) {
228  singularCameras.insert(i);
229  foundSingularCamera = true;
230  }
231  if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
232  singularLandmarks.find(i) == singularLandmarks.end()) {
233  singularLandmarks.insert(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(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(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(label, island) = li;
308  vector<size_t> filteredIsland; // remove singular cameras from array
309  filteredIsland.reserve(island.size());
310  for(const size_t key: island) {
311  if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular
312  (isLandmark[key] && singularLandmarks.find(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(key);
315  }
316  }
317  islands.push_back(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(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(landmarks.begin(), 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(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
396  hasOdometry = cameraToCamera[camera1] == camera2;
397  if (nrCommonLandmarks > 0 || hasOdometry) {
398  nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
399  reducedGraph.push_back(std::make_shared<GenericFactor3D>(camera1, camera2,
400  factorIndex++, NODE_POSE_3D, NODE_POSE_3D, 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(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(nrConstraints[i], 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(nrConstraints[j], 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(minFoundConstraintsPerCamera));
469  if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
470  throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + std::to_string(minFoundConstraintsPerLandmark));
471  }
472 
473 }} // namespace
DSFVector createDSF(const GenericGraph3D &graph, const vector< size_t > &keys, const WorkSpace &workspace)
const gtsam::Symbol key('X', 0)
void checkSingularity(const GenericGraph3D &graph, const std::vector< size_t > &frontals, WorkSpace &workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark)
#define min(a, b)
Definition: datatypes.h:19
std::vector< std::string > labels
std::shared_ptr< GenericFactor2D > sharedGenericFactor2D
Definition: GenericGraph.h:48
std::shared_ptr< GenericFactor3D > sharedGenericFactor3D
Definition: GenericGraph.h:96
const GaussianFactorGraph factors
Definition: BFloat16.h:88
size_t find(size_t key) const
Find the label of the set in which {key} lives.
Definition: DSFVector.cpp:44
if((m *x).isApprox(y))
int getNrCommonLandmarks(const vector< size_t > &landmarks1, const vector< size_t > &landmarks2)
NonlinearFactorGraph graph
std::shared_ptr< std::vector< size_t > > dsf
A faster implementation for DSF, which uses vector rather than btree.
static const size_t nrKeys
std::vector< sharedGenericFactor2D > GenericGraph2D
Definition: GenericGraph.h:49
std::vector< sharedGenericFactor3D > GenericGraph3D
Definition: GenericGraph.h:97
const Symbol key1('v', 1)
bool isSingular(const set< size_t > &singularCameras, const set< size_t > &singularLandmarks, const GenericNode3D &node)
std::map< size_t, std::vector< size_t > > arrays() const
Return all sets, i.e. a partition of all elements.
Definition: DSFVector.cpp:110
static const Camera2 camera2(pose1, K2)
void reduceGenericGraph(const GenericGraph3D &graph, const std::vector< size_t > &cameraKeys, const std::vector< size_t > &landmarkKeys, const std::vector< int > &dictionary, GenericGraph3D &reducedGraph)
bool isSingleton(const size_t &label) const
Find whether there is one and only one occurrence for the given {label}.
Definition: DSFVector.cpp:79
traits
Definition: chartTesting.h:28
void print(const GenericGraph3D &graph, const std::string name)
void prepareDictionary(const std::vector< size_t > &keys)
void merge(const size_t &i1, const size_t &i2)
Merge the sets containing i1 and i2. Does nothing if i1 and i2 are already in the same set...
Definition: DSFVector.cpp:54
Annotation for function names.
Definition: attr.h:48
list< vector< size_t > > findIslands(const GenericGraph3D &graph, const vector< size_t > &keys, WorkSpace &workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark)
const KeyVector keys
PinholeCamera< Cal3_S2 > camera1(pose1, *sharedCal)
std::ptrdiff_t j
void findSingularCamerasLandmarks(const GenericGraph3D &graph, const WorkSpace &workspace, const vector< bool > &isCamera, const vector< bool > &isLandmark, set< size_t > &singularCameras, set< size_t > &singularLandmarks, vector< int > &nrConstraints, bool &foundSingularCamera, bool &foundSingularLandmark, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
const Symbol key2('v', 2)


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