Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
31static std::vector<unsigned> DEFAULT_VECTOR_U;
32
33namespace mrpt {
34namespace 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
111cout << "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 }
154cout << "Planes in the vicinity: ";
155for(unsigned i=0; i < planesVicinity_order.size(); i++)
156 cout << planesVicinity_order[i] << " ";
157cout << 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
168cout << "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
static std::vector< unsigned > DEFAULT_VECTOR_U
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.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:31
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:46
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:55
std::vector< unsigned > planesVicinity_order
std::map< unsigned, std::vector< unsigned > > groups
void arrangeNewGroups(std::vector< mrpt::vector_uint > &parts)
std::vector< unsigned > neighborGroups
int evalPartition(std::set< unsigned > &observedPlanes)
std::map< unsigned, std::vector< unsigned > > vicinity
mrpt::math::CMatrix connectivity_matrix
Scalar * iterator
Definition: eigen_plugins.h:23
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:59 UTC 2022