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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019