MRPT  1.9.9
SemanticClustering.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a
13  * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
14  */
15 
16 #ifndef __SemanticClustering_H
17 #define __SemanticClustering_H
18 
19 #include <mrpt/config.h>
20 #if MRPT_HAS_PCL
21 
23 
24 #include <mrpt/pbmap/PbMap.h>
25 #include <mrpt/pbmap/Plane.h>
26 #include <vector>
27 #include <set>
28 #include <map>
29 
30 static std::vector<unsigned> DEFAULT_VECTOR_U;
31 
32 namespace mrpt::pbmap
33 {
34 /*! This class arranges the planes of a PbMap into groups according to a
35  * co-visibility measure
36  * The groups are divided using graph minimum normaized-cut (minNcut). The
37  * resulting groups can
38  * be used to represent semantic groups corresponding human identifiable
39  * structures such as rooms,
40  * or environments as an office. These groups of closely related planes can be
41  * used also for
42  * relocalization.
43  *
44  * \ingroup mrpt_pbmap_grp
45  */
47 {
48  private:
50 
52 
53  std::vector<unsigned> neighborGroups;
54 
55  std::map<unsigned, std::vector<unsigned>> groups; //[group][planeID]
56 
57  std::map<unsigned, std::vector<unsigned>>
58  vicinity; //[group][neighborGroup]
59 
61 
62  std::vector<unsigned> planesVicinity_order;
63 
64  /*!
65  * Build the proximity matrix
66  */
68  {
69  size_t neigSize = 0;
70  planesVicinity_order.clear();
71 
72  // Set the Vicinity (planes of the current group and its neighbors)
75  // cout << "neighborGroups: ";
76  for (unsigned i = 0; i < neighborGroups.size(); i++)
77  {
78  // cout << neighborGroups[i] << " ";
79  neigSize += groups[neighborGroups[i]].size();
80  planesVicinity_order.insert(
81  planesVicinity_order.end(), groups[neighborGroups[i]].begin(),
82  groups[neighborGroups[i]].end());
83  }
84  // cout << endl;
85 
86  // Fill the matrix
87  assert(neigSize <= mPbMap.vPlanes.size());
88  connectivity_matrix.resize(neigSize, neigSize);
89  connectivity_matrix.zeros();
90  for (unsigned i = 0; i < planesVicinity_order.size(); i++)
91  {
92  unsigned plane_i = planesVicinity_order[i];
93  for (unsigned j = i + 1; j < planesVicinity_order.size(); j++)
94  {
95  unsigned plane_j = planesVicinity_order[j];
96  if (mPbMap.vPlanes[plane_i].nearbyPlanes.count(plane_j))
97  connectivity_matrix(i, j) = connectivity_matrix(j, i) = 1;
98  }
99  }
100  // cout << "Planes in the vicinity: ";
101  // for(unsigned i=0; i < planesVicinity_order.size(); i++)
102  // cout << planesVicinity_order[i] << " ";
103  // cout << endl;
104 
105  // for(unsigned i=0; i < planesVicinity_order.size(); i++)
106  //{
107  // cout << "\nNeighbors of " << planesVicinity_order[i] << " obs " <<
108  // mPbMap.vPlanes[planesVicinity_order[i]].numObservations << ":\n";
109  // for(map<unsigned,unsigned>::iterator
110  // it=mPbMap.vPlanes[planesVicinity_order[i]].neighborPlanes.begin();
111  // it !=
112  // mPbMap.vPlanes[planesVicinity_order[i]].neighborPlanes.end();
113  // it++)
114  // cout << it->first << " " << it->second << endl;
115  //}
116  // cout << endl;
117 
118  cout << "connectivity_matrix matrix\n" << connectivity_matrix << endl;
119  };
120 
121  /*!
122  * Build the co-visibility matrix
123  */
125  {
126  size_t neigSize = 0;
127  planesVicinity_order.clear();
128 
129  // Set the Vicinity (planes of the current group and its neighbors)
132  cout << "neighborGroups: ";
133  for (unsigned i = 0; i < neighborGroups.size(); i++)
134  {
135  cout << neighborGroups[i] << " ";
136  neigSize += groups[neighborGroups[i]].size();
137  planesVicinity_order.insert(
138  planesVicinity_order.end(), groups[neighborGroups[i]].begin(),
139  groups[neighborGroups[i]].end());
140  }
141  cout << endl;
142 
143  // Fill the matrix
144  assert(neigSize <= mPbMap.vPlanes.size());
145  connectivity_matrix.resize(neigSize, neigSize);
146  connectivity_matrix.zeros();
147  for (unsigned i = 0; i < planesVicinity_order.size(); i++)
148  {
149  unsigned plane_i = planesVicinity_order[i];
150  for (unsigned j = i + 1; j < planesVicinity_order.size(); j++)
151  {
152  unsigned plane_j = planesVicinity_order[j];
153  if (mPbMap.vPlanes[plane_i].neighborPlanes.count(plane_j))
154  {
155  // Calculate the co-visibility index
156  float sso =
157  (float)mPbMap.vPlanes[plane_i].neighborPlanes[plane_j] /
158  std::min(
159  mPbMap.vPlanes[plane_i].numObservations,
160  mPbMap.vPlanes[plane_j].numObservations);
161  connectivity_matrix(i, j) = connectivity_matrix(j, i) = sso;
162  }
163  }
164  }
165  cout << "Planes in the vicinity: ";
166  for (unsigned i = 0; i < planesVicinity_order.size(); i++)
167  cout << planesVicinity_order[i] << " ";
168  cout << endl;
169 
170  // for(unsigned i=0; i < planesVicinity_order.size(); i++)
171  //{
172  // cout << "\nNeighbors of " << planesVicinity_order[i] << " obs " <<
173  // mPbMap.vPlanes[planesVicinity_order[i]].numObservations << ":\n";
174  // for(map<unsigned,unsigned>::iterator
175  // it=mPbMap.vPlanes[planesVicinity_order[i]].neighborPlanes.begin();
176  // it !=
177  // mPbMap.vPlanes[planesVicinity_order[i]].neighborPlanes.end();
178  // it++)
179  // cout << it->first << " " << it->second << endl;
180  //}
181  // cout << endl;
182 
183  cout << "connectivity_matrix matrix\n" << connectivity_matrix << endl;
184  };
185 
186  /*!
187  * Arrange the semantic groups
188  */
189  void arrangeNewGroups(std::vector<std::vector<uint32_t>>& parts)
190  {
191  using namespace std;
192  int group_diff = parts.size() - neighborGroups.size();
193  std::map<unsigned, std::vector<unsigned>> newGroups;
194 
195  if (group_diff > 0) // Create new groups
196  {
197  for (unsigned i = 0; i < neighborGroups.size(); i++)
198  newGroups[neighborGroups[i]] = DEFAULT_VECTOR_U;
199  for (int i = 0; i < group_diff; i++)
200  newGroups[groups.size() + i] = DEFAULT_VECTOR_U;
201  // groups[groups.size()] = DEFAULT_VECTOR_U;
202  }
203  else if (group_diff < 0) // Discard old groups
204  {
205  for (unsigned i = 0; i < parts.size(); i++)
206  newGroups[neighborGroups[i]] = DEFAULT_VECTOR_U;
207  }
208  else // Re-arrange previous groups
209  {
210  for (unsigned i = 0; i < neighborGroups.size(); i++)
211  newGroups[neighborGroups[i]] = DEFAULT_VECTOR_U;
212  }
213  // cout << "Size new groups " << newGroups.size();
214  std::vector<unsigned> group_limits(1, 0);
215  // cout << "group_limits_: ";
216  // for(unsigned i=0; i < group_limits.size(); i++)
217  // cout << group_limits[i] << " ";
218  // cout << endl;
219  for (unsigned i = 0; i < neighborGroups.size(); i++)
220  group_limits.push_back(
221  group_limits.back() + groups[neighborGroups[i]].size());
222  // cout << "group_limits: ";
223  // for(unsigned i=0; i < group_limits.size(); i++)
224  // cout << group_limits[i] << " ";
225  // cout << endl;
226 
227  // Re-assign plane's semanticGroup and groups
228  for (unsigned i = 0; i < parts.size(); i++)
229  {
230  for (unsigned j = 0; j < parts[i].size(); j++)
231  {
232  if (i < neighborGroups.size())
233  {
234  if (parts[i][j] < group_limits[i] ||
235  parts[i][j] >=
236  group_limits[i +
237  1]) // The plane has changed the group
238  {
239  for (unsigned k = 0;
240  k < neighborGroups.size() && i != k; k++)
241  if (parts[i][j] >= group_limits[k] ||
242  parts[i][j] < group_limits[k + 1]) // The plane
243  // has
244  // changed
245  // the group
246  {
247  assert(
249  [parts[i][j] - group_limits[k]] ==
250  planesVicinity_order[parts[i][j]]);
251  // cout << parts[i][j] << "
252  // swithces to group " <<
253  // neighborGroups[k] << endl;
254  newGroups[neighborGroups[k]].push_back(
255  mPbMap
256  .vPlanes[groups[neighborGroups[k]]
257  [parts[i][j] -
258  group_limits[k]]]
259  .id);
260  mPbMap
262  [parts[i][j] -
263  group_limits[k]]]
264  .semanticGroup = neighborGroups[k];
265  }
266  }
267  else // The plane remains in its group
268  {
269  // cout << parts[i][j] << " remains in group
270  // " << neighborGroups[i] << endl;
271  newGroups[neighborGroups[i]].push_back(
272  mPbMap
273  .vPlanes[groups[neighborGroups[i]]
274  [parts[i][j] - group_limits[i]]]
275  .id);
276  // mPbMap.vPlanes[groups[neighborGroups[k]][parts[i][j]-group_limits[k]]].semanticGroup
277  // = neighborGroups[k];
278  }
279  }
280  else
281  {
282  newGroups[groups.size() + i - neighborGroups.size()]
283  .push_back(planesVicinity_order[parts[i][j]]);
284  mPbMap.vPlanes[planesVicinity_order[parts[i][j]]]
285  .semanticGroup =
286  groups.size() + i - neighborGroups.size();
287  // cout << parts[i][j] << " swithces to NEW group "
288  // << groups.size()+i-neighborGroups.size() <<
289  // endl;
290  }
291  }
292  }
293 
294  for (map<unsigned, vector<unsigned>>::iterator it = newGroups.begin();
295  it != newGroups.end(); it++)
296  groups[it->first] = it->second;
297 
298  if (group_diff < 0)
299  {
300  int sizeVicinity = neighborGroups.size();
301  for (int i = -1; i >= group_diff; i--)
302  {
303  if (neighborGroups[sizeVicinity + i] == groups.size() - 1)
304  groups.erase(neighborGroups[sizeVicinity + i]);
305  else
306  {
307  for (unsigned j = 0; j < mPbMap.vPlanes.size(); j++)
308  if (mPbMap.vPlanes[j].semanticGroup >
309  neighborGroups[sizeVicinity + i])
310  mPbMap.vPlanes[j].semanticGroup--;
311  for (unsigned j = neighborGroups[sizeVicinity + i];
312  j < groups.size() - 1; j++)
313  groups[j] = groups[j + 1];
314  groups.erase(groups.size() - 1);
315  }
316  }
317  }
318 
319  // cout << "Updated arrangement of groups\n";
320  // for(map<unsigned,vector<unsigned> >::iterator it=groups.begin(); it
321  // != groups.end(); it++)
322  //{
323  // cout << "group " << it->first << ": ";
324  // for(unsigned i=0; i < it->second.size(); i++)
325  // cout << it->second[i] << " ";
326  // cout << endl;
327  //}
328 
329  // Re-define currentSemanticGroup and current vicinity
330  // std::vector<unsigned> newNeighborGroups;
331 
332  for (std::map<unsigned, std::vector<unsigned>>::iterator it1 =
333  newGroups.begin();
334  it1 != newGroups.end(); it1++)
335  vicinity[it1->first] = DEFAULT_VECTOR_U;
336 
337  for (std::map<unsigned, std::vector<unsigned>>::iterator it1 =
338  newGroups.begin();
339  it1 != newGroups.end(); it1++)
340  {
341  std::map<unsigned, std::vector<unsigned>>::iterator it2 = it1;
342  for (it2++; it2 != newGroups.end(); it2++)
343  for (unsigned i = 0; i < it1->second.size(); i++)
344  {
345  bool linked = false;
346  for (unsigned j = 0; j < it2->second.size(); j++)
347  {
348  if (mPbMap.vPlanes[it1->second[i]].neighborPlanes.count(
349  mPbMap.vPlanes[it2->second[j]].id))
350  {
351  vicinity[it1->first].push_back(it2->first);
352  vicinity[it2->first].push_back(it1->first);
353  linked = true;
354  break;
355  }
356  }
357  if (linked) break;
358  }
359  }
360 
361  // TODO: Re-define second order vicinity
362  }
363 
364  public:
365  friend class PbMapMaker;
366 
367  // Constructor
369  {
371  };
372 
373  /*!
374  * Evaluate the partition of the current semantic groups into new ones with
375  * minNcut
376  */
377  int evalPartition(std::set<unsigned>& observedPlanes)
378  {
379  using namespace std;
380  // mrpt::system::CTicTac time;
381  // time.Tic();
382  //
383 
384  // Select current group
385  unsigned current_group_votes = 0;
386  map<unsigned, unsigned> observed_group;
387  for (set<unsigned>::iterator it = observedPlanes.begin();
388  it != observedPlanes.end(); it++)
389  if (observed_group.count(mPbMap.vPlanes[*it].semanticGroup))
390  observed_group[mPbMap.vPlanes[*it].semanticGroup]++;
391  else
392  observed_group[mPbMap.vPlanes[*it].semanticGroup] = 1;
393  for (map<unsigned, unsigned>::iterator it = observed_group.begin();
394  it != observed_group.end(); it++)
395  if (it->second > current_group_votes)
396  {
397  currentSemanticGroup = it->first;
398  current_group_votes = it->second;
399  }
400  // cout << "currentSemanticGroup " << currentSemanticGroup << endl;
401 
402  // buildCoVisibilityMatrix();
404  std::vector<std::vector<uint32_t>>
405  parts; // Vector of vectors to keep the
406  // KFs index of the different
407  // partitions (submaps)
410  connectivity_matrix, parts, 0.8, false, true, true, 1);
411 
412  // cout << "Time RecursiveSpectralPartition " << time.Tac()*1000 <<
413  // "ms" << endl;
414 
415  if (neighborGroups.size() == 1 && parts.size() == 1) return 1;
416 
417  // Check if this partition produces any change over the previous group
418  // structure
419  bool different_partition = false;
420  if (neighborGroups.size() != parts.size())
421  different_partition = true;
422  else
423  {
424  unsigned prev_size = 0;
425  for (unsigned i = 0; i < neighborGroups.size(); i++)
426  {
427  if (groups[neighborGroups[i]].size() != parts[i].size())
428  {
429  different_partition = true;
430  break;
431  }
432 
433  for (unsigned j = 0; j < parts[i].size(); j++)
434  {
435  if (parts[i][j] != j + prev_size)
436  {
437  different_partition = true;
438  break;
439  }
440  }
441  prev_size += parts[i].size();
442  }
443  }
444 
445  if (!different_partition) return -1;
446 
447  arrangeNewGroups(parts);
448 
449  // Update currentSemanticGroup
450  current_group_votes = 0;
451  observed_group.clear();
452  for (set<unsigned>::iterator it = observedPlanes.begin();
453  it != observedPlanes.end(); it++)
454  if (observed_group.count(mPbMap.vPlanes[*it].semanticGroup))
455  observed_group[mPbMap.vPlanes[*it].semanticGroup]++;
456  else
457  observed_group[mPbMap.vPlanes[*it].semanticGroup] = 1;
458  for (map<unsigned, unsigned>::iterator it = observed_group.begin();
459  it != observed_group.end(); it++)
460  if (it->second > current_group_votes)
461  {
462  currentSemanticGroup = it->first;
463  current_group_votes = it->second;
464  }
465  // cout << "Updated currentSemanticGroup " << currentSemanticGroup <<
466  // endl;
467  //
468  // cout << "Planes' semantics2: ";
469  // for(unsigned i=0; i < mPbMap.vPlanes.size(); i++)
470  // cout << mPbMap.vPlanes[i].semanticGroup << " ";
471  // cout << endl;
472  return parts.size();
473  };
474 };
475 }
476 #endif
477 #endif
478 
479 
Scalar * iterator
Definition: eigen_plugins.h:26
#define min(a, b)
std::map< unsigned, std::vector< unsigned > > vicinity
STL namespace.
mrpt::math::CMatrix connectivity_matrix
std::vector< unsigned > planesVicinity_order
void arrangeNewGroups(std::vector< std::vector< uint32_t >> &parts)
std::map< unsigned, std::vector< unsigned > > groups
std::vector< unsigned > neighborGroups
static std::vector< unsigned > DEFAULT_VECTOR_U
int evalPartition(std::set< unsigned > &observedPlanes)
GLsizeiptr size
Definition: glext.h:3923
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:55
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:22
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:45
static void RecursiveSpectralPartition(GRAPH_MATRIX &in_A, std::vector< std::vector< uint32_t >> &out_parts, num_t threshold_Ncut=1, bool forceSimetry=true, bool useSpectralBisection=true, bool recursive=true, unsigned minSizeClusters=1, const bool verbose=false)
Performs the spectral recursive partition into K-parts for a given graph.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020