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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018