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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019