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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST