Main MRPT website > C++ reference for MRPT 1.5.6
SubgraphMatcher.cpp
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 #include "pbmap-precomp.h" // Precompiled headers
16 #include <mrpt/utils/utils_defs.h>
18 
19 //#define _VERBOSE 1
20 
21 extern double time1, time2;
22 
23 using namespace std;
24 using namespace mrpt::utils;
25 using namespace mrpt::pbmap;
26 
27 // Bhattacharyya histogram distance function
28 double BhattacharyyaDist_(std::vector<float> &hist1, std::vector<float> &hist2)
29 {
30  assert(hist1.size() == hist2.size());
31  double BhattachDist;
32  double BhattachDist_aux = 0.0;
33  for(unsigned i=0; i < hist1.size(); i++)
34  BhattachDist_aux += sqrt(hist1[i]*hist2[i]);
35 
36  BhattachDist = sqrt(1 - BhattachDist_aux);
37 
38  return BhattachDist;
39 }
40 
41 SubgraphMatcher::SubgraphMatcher()
42 {
43 // configLocaliser.load_params("/home/edu/Libraries/mrpt_edu/share/mrpt/config_files/pbmap/configPbMap.ini");
44 }
45 
46 /**!
47  * Check if the two input planes could be the same
48 */
49 bool SubgraphMatcher::evalUnaryConstraints(Plane &plane1, Plane &plane2, PbMap &trgPbMap, bool useStructure)
50 {
51 //Eigen::Vector3f color_dif = plane1.v3colorNrgb - plane2.v3colorNrgb;
52 //if(plane1.id==6 && plane2.id==8)
53 //cout << "color_dif \n" << color_dif << endl;
54 //double tCondition1, tCondition2, tCondition3;
55  #if _VERBOSE
56  cout << "unary between " << plane1.id << " " << plane2.id << "\n";
57  #endif
58 
59  //++nCheckConditions;
60 // ++totalUnary;
61 // // Semantic label condition
62 // if( !plane1.label.empty() && !plane2.label.empty() ){
63 //// ++semanticPair;
64 // if( plane1.label != plane2.label){
65 //// ++rejectSemantic;
66 // return false;
67 // }
68 // }
69 
70 // if( !plane1.label.empty() || !plane2.label.empty() ){
71 // if( plane1.label.empty() || plane2.label.empty() )
72 // return false;
73 //// ++semanticPair;
74 // if( plane1.label != plane2.label){
75 //// ++rejectSemantic;
76 // return false;
77 // }
78 // }
79 
80  // Main color
81 // if(plane1.bDominantColor)
82  {
83 //cout << "thresholds " << configLocaliser.color_threshold << " " << configLocaliser.intensity_threshold << endl;
84  //++nCheckConditions;
85 // if(configLocaliser.color_threshold > 0)
86 // tCondition1 = pcl::getTime();
87 
88 // if( fabs(plane1.v3colorNrgb[0] - plane2.v3colorNrgb[0]) > configLocaliser.color_threshold ||
89 // fabs(plane1.v3colorNrgb[1] - plane2.v3colorNrgb[1]) > configLocaliser.color_threshold ||
90 //// fabs(plane1.v3colorNrgb[2] - plane2.v3colorNrgb[2]) > configLocaliser.color_threshold ||//)
91 // fabs(plane1.dominantIntensity - plane2.dominantIntensity) > configLocaliser.intensity_threshold)
92 // ;
93 // tCondition2 = pcl::getTime();
94 //
95  if( BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) > configLocaliser.hue_threshold ){
96  #if _VERBOSE
97  cout << "Hist_H false " << BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) << " > " << configLocaliser.hue_threshold << "\n";
98  #endif
99 // ;
100 // tCondition3 = pcl::getTime();
101 
102 // time1 += tCondition2 - tCondition1;
103 // time2 += tCondition3 - tCondition2;
104  return false;}
105  }
106 
107  //++nCheckConditions;
108  if(plane1.bFromStructure && plane2.bFromStructure){//cout << "True: both structure\n";
109  return true;}
110 
111  double rel_areas = plane1.areaHull/ plane2.areaHull;
112 // double rel_areas = plane1.areaVoxels/ plane2.areaVoxels;
113 //if(plane1.id==6 && plane2.id==8)
114 //cout << "rel_areas " << rel_areas << endl;
115  double rel_elong = plane1.elongation / plane2.elongation;
116 // if(plane1.id==6 && plane2.id==8)
117 //cout << "rel_elong " << rel_elong << endl;
118 
119  // If the plane has been fully detected use a narrower threshold for the comparison
120  if(plane1.bFullExtent && plane2.bFullExtent)
121  {
122  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
123  #if _VERBOSE
124  cout << "False: rel_areas full " << rel_areas << endl;
125  #endif
126  return false;}
127  //++nCheckConditions;
128 
129  // We can use a narrower limit for elong
130  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
131  #if _VERBOSE
132  cout << "False: rel_elong full " << rel_elong << endl;
133  #endif
134  return false;}
135  //++nCheckConditions;
136  }
137  else
138  {
139  if(plane1.bFullExtent)
140  {
141  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_threshold ){
142  #if _VERBOSE
143  cout << "rel_areas RefFull " << rel_areas << endl;
144  #endif
145  return false;}
146  }
147  else if(plane2.bFullExtent)
148  {
149  if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
150  #if _VERBOSE
151  cout << "rel_areas CheckFull " << rel_areas << endl;
152  #endif
153  return false;}
154  }
155  else if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_threshold ){
156  #if _VERBOSE
157  cout << "rel_areas simple " << rel_areas << endl;
158  #endif
159  return false;}
160  //++nCheckConditions;
161 
162  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
163  #if _VERBOSE
164  cout << "rel_elong simple " << rel_elong << endl;
165  #endif
166  return false;}
167  //++nCheckConditions;
168  }
169 
170  // TODO. Use the inferred semantic information to control the search
171  #if _VERBOSE
172  cout << "UNARY TRUE" << endl;
173  #endif
174 
175  return true;
176 }
177 
178 bool SubgraphMatcher::evalUnaryConstraintsOdometry(Plane &plane1, Plane &plane2, PbMap &trgPbMap, bool useStructure)
179 {
180 //Eigen::Vector3f color_dif = plane1.v3colorNrgb - plane2.v3colorNrgb;
181 //if(plane1.id==6 && plane2.id==8)
182 //cout << "color_dif \n" << color_dif << endl;
183 //double tCondition1, tCondition2, tCondition3;
184  #if _VERBOSE
185  cout << "unaryOdometry between " << plane1.id << " " << plane2.id << "\n";
186  #endif
187 
188  //++nCheckConditions;
189 
190  // Constraints for odometry
191  if( fabs(plane1.d - plane2.d) > configLocaliser.dist_d){
192  #if _VERBOSE
193  cout << "depthUnaryConstraint false " << fabs(plane1.d - plane2.d) << " > " << configLocaliser.dist_d << "\n";
194  #endif
195  return false;
196  }
197 
198  if( plane1.v3normal .dot (plane2.v3normal) < configLocaliser.angle){
199  #if _VERBOSE
200  cout << "angleUnaryConstraint false " << plane1.v3normal .dot (plane2.v3normal) << " < " << configLocaliser.angle << "\n";
201  #endif
202  return false;
203  }
204 
205 // ++totalUnary;
206 // // Semantic label condition
207 // if( !plane1.label.empty() && !plane2.label.empty() ){
208 //// ++semanticPair;
209 // if( plane1.label != plane2.label){
210 //// ++rejectSemantic;
211 // return false;
212 // }
213 // }
214 
215 // if( !plane1.label.empty() || !plane2.label.empty() ){
216 // if( plane1.label.empty() || plane2.label.empty() )
217 // return false;
218 //// ++semanticPair;
219 // if( plane1.label != plane2.label){
220 //// ++rejectSemantic;
221 // return false;
222 // }
223 // }
224 
225  // Main color
226 // if(plane1.bDominantColor)
227  {
228 //cout << "thresholds " << configLocaliser.color_threshold << " " << configLocaliser.intensity_threshold << endl;
229  //++nCheckConditions;
230 // if(configLocaliser.color_threshold > 0)
231 // tCondition1 = pcl::getTime();
232 
233 // if( fabs(plane1.v3colorNrgb[0] - plane2.v3colorNrgb[0]) > configLocaliser.color_threshold ||
234 // fabs(plane1.v3colorNrgb[1] - plane2.v3colorNrgb[1]) > configLocaliser.color_threshold ||
235 //// fabs(plane1.v3colorNrgb[2] - plane2.v3colorNrgb[2]) > configLocaliser.color_threshold ||//)
236 // fabs(plane1.dominantIntensity - plane2.dominantIntensity) > configLocaliser.intensity_threshold)
237 // ;
238 // tCondition2 = pcl::getTime();
239 //
240  if( BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) > configLocaliser.hue_threshold ){
241  #if _VERBOSE
242  cout << "Hist_H false_ " << BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) << " > " << configLocaliser.hue_threshold << "\n";
243  #endif
244 // ;
245 // tCondition3 = pcl::getTime();
246 
247 // time1 += tCondition2 - tCondition1;
248 // time2 += tCondition3 - tCondition2;
249  return false;
250  }
251  }
252 
253  //++nCheckConditions;
254  if(plane1.bFromStructure && plane2.bFromStructure){//cout << "True: both structure\n";
255  return true;}
256 
257  double rel_areas = plane1.areaHull/ plane2.areaHull;
258 // double rel_areas = plane1.areaVoxels/ plane2.areaVoxels;
259 //if(plane1.id==6 && plane2.id==8)
260 //cout << "rel_areas " << rel_areas << endl;
261  double rel_elong = plane1.elongation / plane2.elongation;
262 // if(plane1.id==6 && plane2.id==8)
263 //cout << "rel_elong " << rel_elong << endl;
264  #if _VERBOSE
265  cout << "elong " << plane1.elongation << " " << plane2.elongation << endl;
266  #endif
267 
268  // If the plane has been fully detected use a narrower threshold for the comparison
269  if(plane1.bFullExtent && plane2.bFullExtent)
270  {
271  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
272  #if _VERBOSE
273  cout << "False: rel_areas full " << rel_areas << endl;
274  #endif
275  return false;}
276  //++nCheckConditions;
277 
278  // We can use a narrower limit for elong
279  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
280  #if _VERBOSE
281  cout << "False: rel_elong full " << rel_elong << endl;
282  #endif
283  return false;}
284  //++nCheckConditions;
285  }
286  else
287  {
288  if(plane1.bFullExtent)
289  {
290  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_threshold ){
291  #if _VERBOSE
292  cout << "rel_areas RefFull " << rel_areas << endl;
293  #endif
294  return false;}
295  }
296  else if(plane2.bFullExtent)
297  {
298  if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
299  #if _VERBOSE
300  cout << "rel_areas CheckFull " << rel_areas << endl;
301  #endif
302  return false;}
303  }
304  else if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_threshold ){
305  #if _VERBOSE
306  cout << "rel_areas simple " << rel_areas << endl;
307  #endif
308  return false;}
309  //++nCheckConditions;
310 
311  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
312  #if _VERBOSE
313  cout << "rel_elong simple " << rel_elong << endl;
314  #endif
315  return false;}
316  //++nCheckConditions;
317  }
318 
319  // TODO. Use the inferred semantic information to control the search
320  #if _VERBOSE
321  cout << "UNARY TRUE" << endl;
322  #endif
323 
324  return true;
325 }
326 
327 bool SubgraphMatcher::evalUnaryConstraints2D(Plane &plane1, Plane &plane2, PbMap &trgPbMap, bool useStructure)
328 {
329 //Eigen::Vector3f color_dif = plane1.v3colorNrgb - plane2.v3colorNrgb;
330 //if(plane1.id==6 && plane2.id==8)
331 //cout << "color_dif \n" << color_dif << endl;
332 //double tCondition1, tCondition2, tCondition3;
333  #if _VERBOSE
334  cout << "unary2D between " << plane1.id << " " << plane2.id << "\n";
335  #endif
336 
337  if( fabs(plane1.v3normal(0) - plane2.v3normal(0) ) > 0.08 ){
338  return false;
339  }
340 
341  if( plane1.v3normal(0) > 0.98 ){
342  if( fabs(plane1.d - plane2.d) < 0.15 ){
343 // ++rejectSemantic;
344  return false;
345  }
346  }
347 
348  //++nCheckConditions;
349 // ++totalUnary;
350 // // Semantic label condition
351 // if( !plane1.label.empty() && !plane2.label.empty() ){
352 //// ++semanticPair;
353 // if( plane1.label != plane2.label){
354 //// ++rejectSemantic;
355 // return false;
356 // }
357 // }
358 
359 // if( !plane1.label.empty() || !plane2.label.empty() ){
360 // if( plane1.label.empty() || plane2.label.empty() )
361 // return false;
362 //// ++semanticPair;
363 // if( plane1.label != plane2.label){
364 //// ++rejectSemantic;
365 // return false;
366 // }
367 // }
368 
369  // Main color
370 // if(plane1.bDominantColor)
371  {
372 //cout << "thresholds " << configLocaliser.color_threshold << " " << configLocaliser.intensity_threshold << endl;
373  //++nCheckConditions;
374 // if(configLocaliser.color_threshold > 0)
375 // tCondition1 = pcl::getTime();
376 
377 // if( fabs(plane1.v3colorNrgb[0] - plane2.v3colorNrgb[0]) > configLocaliser.color_threshold ||
378 // fabs(plane1.v3colorNrgb[1] - plane2.v3colorNrgb[1]) > configLocaliser.color_threshold ||
379 //// fabs(plane1.v3colorNrgb[2] - plane2.v3colorNrgb[2]) > configLocaliser.color_threshold ||//)
380 // fabs(plane1.dominantIntensity - plane2.dominantIntensity) > configLocaliser.intensity_threshold)
381 // ;
382 // tCondition2 = pcl::getTime();
383 //
384  if( BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) > configLocaliser.hue_threshold ){
385  #if _VERBOSE
386  cout << "Hist_H false " << BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) << " > " << configLocaliser.hue_threshold << "\n";
387  #endif
388 // ;
389 // tCondition3 = pcl::getTime();
390 
391 // time1 += tCondition2 - tCondition1;
392 // time2 += tCondition3 - tCondition2;
393  return false;}
394  }
395 
396  //++nCheckConditions;
397  if(plane1.bFromStructure && plane2.bFromStructure){//cout << "True: both structure\n";
398  return true;}
399 
400  double rel_areas = plane1.areaHull/ plane2.areaHull;
401 // double rel_areas = plane1.areaVoxels/ plane2.areaVoxels;
402 //if(plane1.id==6 && plane2.id==8)
403 //cout << "rel_areas " << rel_areas << endl;
404  double rel_elong = plane1.elongation / plane2.elongation;
405 // if(plane1.id==6 && plane2.id==8)
406 //cout << "rel_elong " << rel_elong << endl;
407 
408  // If the plane has been fully detected use a narrower threshold for the comparison
409  if(plane1.bFullExtent && plane2.bFullExtent)
410  {
411  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
412  #if _VERBOSE
413  cout << "False: rel_areas full " << rel_areas << endl;
414  #endif
415  return false;}
416  //++nCheckConditions;
417 
418  // We can use a narrower limit for elong
419  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
420  #if _VERBOSE
421  cout << "False: rel_elong full " << rel_elong << endl;
422  #endif
423  return false;}
424  //++nCheckConditions;
425  }
426  else
427  {
428  if(plane1.bFullExtent)
429  {
430  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_threshold ){
431  #if _VERBOSE
432  cout << "rel_areas RefFull " << rel_areas << endl;
433  #endif
434  return false;}
435  }
436  else if(plane2.bFullExtent)
437  {
438  if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
439  #if _VERBOSE
440  cout << "rel_areas CheckFull " << rel_areas << endl;
441  #endif
442  return false;}
443  }
444  else if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_threshold ){
445  #if _VERBOSE
446  cout << "rel_areas simple " << rel_areas << endl;
447  #endif
448  return false;}
449  //++nCheckConditions;
450 
451  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
452  #if _VERBOSE
453  cout << "rel_elong simple " << rel_elong << endl;
454  #endif
455  return false;}
456  //++nCheckConditions;
457  }
458 
459  // TODO. Use the inferred semantic information to control the search
460  #if _VERBOSE
461  cout << "UNARY TRUE" << endl;
462  #endif
463 
464  return true;
465 }
466 
467 bool SubgraphMatcher::evalUnaryConstraintsOdometry2D(Plane &plane1, Plane &plane2, PbMap &trgPbMap, bool useStructure)
468 {
469 //Eigen::Vector3f color_dif = plane1.v3colorNrgb - plane2.v3colorNrgb;
470 //if(plane1.id==6 && plane2.id==8)
471 //cout << "color_dif \n" << color_dif << endl;
472 //double tCondition1, tCondition2, tCondition3;
473  #if _VERBOSE
474  cout << "unaryOdometry2D between " << plane1.id << " " << plane2.id << "\n";
475  #endif
476  //++nCheckConditions;
477 
478  if( fabs(plane1.v3normal(0) - plane2.v3normal(0) ) > 0.05 ){
479  return false;
480  }
481 
482  if( plane1.v3normal(0) > 0.98 ){
483  if( fabs(plane1.d - plane2.d) < 0.12 ){
484  return false;
485  }
486  }
487 
488  #if _VERBOSE
489  cout << "pass 2D \n";
490  #endif
491 
492  // Constraints for odometry
493  if( fabs(plane1.d - plane2.d) > configLocaliser.dist_d){
494  #if _VERBOSE
495  cout << "depthUnaryConstraint false " << fabs(plane1.d - plane2.d) << " > " << configLocaliser.dist_d << "\n";
496  #endif
497  return false;
498  }
499 
500  if( plane1.v3normal .dot (plane2.v3normal) < configLocaliser.angle){
501  #if _VERBOSE
502  cout << "angleUnaryConstraint false " << plane1.v3normal .dot (plane2.v3normal) << " < " << configLocaliser.angle << "\n";
503  #endif
504  return false;
505  }
506 
507 // ++totalUnary;
508 // // Semantic label condition
509 // if( !plane1.label.empty() && !plane2.label.empty() ){
510 //// ++semanticPair;
511 // if( plane1.label != plane2.label){
512 //// ++rejectSemantic;
513 // return false;
514 // }
515 // }
516 
517 // if( !plane1.label.empty() || !plane2.label.empty() ){
518 // if( plane1.label.empty() || plane2.label.empty() )
519 // return false;
520 //// ++semanticPair;
521 // if( plane1.label != plane2.label){
522 //// ++rejectSemantic;
523 // return false;
524 // }
525 // }
526 
527  // Main color
528 // if(plane1.bDominantColor)
529  {
530 //cout << "thresholds " << configLocaliser.color_threshold << " " << configLocaliser.intensity_threshold << endl;
531  //++nCheckConditions;
532 // if(configLocaliser.color_threshold > 0)
533 // tCondition1 = pcl::getTime();
534 
535 // if( fabs(plane1.v3colorNrgb[0] - plane2.v3colorNrgb[0]) > configLocaliser.color_threshold ||
536 // fabs(plane1.v3colorNrgb[1] - plane2.v3colorNrgb[1]) > configLocaliser.color_threshold ||
537 //// fabs(plane1.v3colorNrgb[2] - plane2.v3colorNrgb[2]) > configLocaliser.color_threshold ||//)
538 // fabs(plane1.dominantIntensity - plane2.dominantIntensity) > configLocaliser.intensity_threshold)
539 // ;
540 // tCondition2 = pcl::getTime();
541 //
542  if( BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) > configLocaliser.hue_threshold ){
543  #if _VERBOSE
544  cout << "Hist_H false_ " << BhattacharyyaDist_(plane1.hist_H, plane2.hist_H) << " > " << configLocaliser.hue_threshold << "\n";
545  #endif
546 // ;
547 // tCondition3 = pcl::getTime();
548 
549 // time1 += tCondition2 - tCondition1;
550 // time2 += tCondition3 - tCondition2;
551  return false;
552  }
553  }
554 
555  //++nCheckConditions;
556  if(plane1.bFromStructure && plane2.bFromStructure){//cout << "True: both structure\n";
557  return true;}
558 
559  double rel_areas = plane1.areaHull/ plane2.areaHull;
560 // double rel_areas = plane1.areaVoxels/ plane2.areaVoxels;
561 //if(plane1.id==6 && plane2.id==8)
562 //cout << "rel_areas " << rel_areas << endl;
563  double rel_elong = plane1.elongation / plane2.elongation;
564 // if(plane1.id==6 && plane2.id==8)
565 //cout << "rel_elong " << rel_elong << endl;
566  #if _VERBOSE
567  cout << "elong " << plane1.elongation << " " << plane2.elongation << endl;
568  #endif
569 
570  // If the plane has been fully detected use a narrower threshold for the comparison
571  if(plane1.bFullExtent && plane2.bFullExtent)
572  {
573  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
574  #if _VERBOSE
575  cout << "False: rel_areas full " << rel_areas << endl;
576  #endif
577  return false;}
578  //++nCheckConditions;
579 
580  // We can use a narrower limit for elong
581  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
582  #if _VERBOSE
583  cout << "False: rel_elong full " << rel_elong << endl;
584  #endif
585  return false;}
586  //++nCheckConditions;
587  }
588  else
589  {
590  if(plane1.bFullExtent)
591  {
592  if( rel_areas < configLocaliser.area_full_threshold_inv || rel_areas > configLocaliser.area_threshold ){
593  #if _VERBOSE
594  cout << "rel_areas RefFull " << rel_areas << endl;
595  #endif
596  return false;}
597  }
598  else if(plane2.bFullExtent)
599  {
600  if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_full_threshold ){
601  #if _VERBOSE
602  cout << "rel_areas CheckFull " << rel_areas << endl;
603  #endif
604  return false;}
605  }
606  else if( rel_areas < configLocaliser.area_threshold_inv || rel_areas > configLocaliser.area_threshold ){
607  #if _VERBOSE
608  cout << "rel_areas simple " << rel_areas << endl;
609  #endif
610  return false;}
611  //++nCheckConditions;
612 
613  if( rel_elong < configLocaliser.elongation_threshold_inv || rel_elong > configLocaliser.elongation_threshold ){
614  #if _VERBOSE
615  cout << "rel_elong simple " << rel_elong << endl;
616  #endif
617  return false;}
618  //++nCheckConditions;
619  }
620 
621  // TODO. Use the inferred semantic information to control the search
622  #if _VERBOSE
623  cout << "UNARY TRUE" << endl;
624  #endif
625 
626  return true;
627 }
628 
629 /**!
630  * Compares the relation between Ref-neigRef with the relation between Check-neigCheck. Returns true if both geometries are similar
631 */
632 bool SubgraphMatcher::evalBinaryConstraints(Plane &Ref, Plane &neigRef, Plane &Check, Plane &neigCheck)
633 {
634  // Check intensity relations
635 // float relIntensity = (neigRef.dominantIntensity/Ref.dominantIntensity) / (neigCheck.dominantIntensity/Check.dominantIntensity);
636 // cout << "Binary intensity relations " << dif_height << endl;
637 // if( relIntensity > 0.5 && relIntensity < 2)
638 // {
639 // return false;
640 // }
641 
642  // Check height
643  if(neigRef.areaHull < 2 && neigCheck.areaHull < 2)
644  {
645  double dif_height = fabs(Ref.v3normal.dot(neigRef.v3center - Ref.v3center) - Check.v3normal.dot(neigCheck.v3center - Check.v3center));
646 // if(Ref.id==6 && Check.id==8)
647 // cout << "dif_height " << dif_height << endl;
648  float threshold_dist_dependent = configLocaliser.height_threshold * (Ref.v3center - neigRef.v3center).norm() * 0.4;
649  float height_threshold = std::max(configLocaliser.height_threshold, threshold_dist_dependent);
650  if(dif_height > height_threshold){
651 // if(dif_height > configLocaliser.height_threshold){
652  #if _VERBOSE
653  cout << "Binary false: Ref dif_height " << dif_height << endl;
654  #endif
655  return false;}
656  //++nCheckConditions;
657  }
658  else
659  {
660  double dif_height = fabs(Ref.v3normal.dot(neigRef.v3center - Ref.v3center) - Check.v3normal.dot(neigCheck.v3center - Check.v3center));
661 // if(Ref.id==6 && Check.id==8)
662 // cout << "dif_height " << dif_height << endl;
663 // float threshold_dist_dependent = configLocaliser.height_threshold * (Ref.v3center - neigRef.v3center).norm() * 0.4;
664  float height_threshold = (neigRef.areaHull + neigCheck.areaHull)*configLocaliser.height_threshold;
665  if(dif_height > height_threshold){
666 // if(dif_height > configLocaliser.height_threshold){
667  #if _VERBOSE
668  cout << "Binary false BIG Area: Ref dif_height " << dif_height << endl;
669  #endif
670  return false;}
671  //++nCheckConditions;
672  }
673 
674  if(Ref.areaHull < 2 && Check.areaHull < 2)
675  {
676  double dif_height2 = fabs(neigRef.v3normal.dot(Ref.v3center - neigRef.v3center) - neigCheck.v3normal.dot(Check.v3center - neigCheck.v3center));
677 // if(Ref.id==6 && Check.id==8)
678 // cout << "dif_height2 " << dif_height2 << endl;
679  float threshold_dist_dependent = configLocaliser.height_threshold * (Ref.v3center - neigRef.v3center).norm() * 0.4;
680  float height_threshold = std::max(configLocaliser.height_threshold, threshold_dist_dependent);
681  if(dif_height2 > height_threshold){
682 // if(dif_height2 > configLocaliser.height_threshold){
683  #if _VERBOSE
684  cout << "Binary false: neigRef dif_height2 " << dif_height2 << endl;
685  #endif
686  return false;}
687  //++nCheckConditions;
688  }
689  else
690  {
691  double dif_height2 = fabs(neigRef.v3normal.dot(Ref.v3center - neigRef.v3center) - neigCheck.v3normal.dot(Check.v3center - neigCheck.v3center));
692 // if(Ref.id==6 && Check.id==8)
693 // cout << "dif_height " << dif_height << endl;
694 // float threshold_dist_dependent = configLocaliser.height_threshold * (Ref.v3center - neigRef.v3center).norm() * 0.4;
695  float height_threshold = (Ref.areaHull + Check.areaHull)*configLocaliser.height_threshold;
696  if(dif_height2 > height_threshold){
697 // if(dif_height > configLocaliser.height_threshold){
698  #if _VERBOSE
699  cout << "Binary false: neigRef dif_height2 " << dif_height2 << endl;
700  #endif
701  return false;}
702  //++nCheckConditions;
703  }
704 
705  // Normal
706  double dif_normal = fabs(RAD2DEG( acos( Ref.v3normal.dot(neigRef.v3normal)) - acos( Check.v3normal.dot(neigCheck.v3normal)) ) );
707 // if( dif_normal > configLocaliser.angle_threshold ){
708  if( dif_normal > std::max(configLocaliser.angle_threshold,2*(Ref.v3center - neigRef.v3center).norm()) ){
709  #if _VERBOSE
710  cout << "Binary false: angle " << dif_normal << " with " << neigRef.id << endl;
711  #endif
712  return false;}
713  //++nCheckConditions;
714 
715  // Relative distance
716  double rel_dist_centers = sqrt( (Ref.v3center - neigRef.v3center).dot(Ref.v3center - neigRef.v3center) / ((Check.v3center - neigCheck.v3center).dot(Check.v3center - neigCheck.v3center)) );
717 
718  // If the plane has been fully detected use a narrower threshold for the comparison
719  bool RefBothFull = (Ref.bFullExtent && neigRef.bFullExtent);// ? true : false;
720  bool CheckBothFull = (Check.bFullExtent && neigCheck.bFullExtent);// ? true : false;
721 
722  if(configLocaliser.use_completeness)
723  {
724  if(RefBothFull && CheckBothFull)
725  {
726  if( rel_dist_centers < configLocaliser.dist_threshold_inv || rel_dist_centers > configLocaliser.dist_threshold ){
727  #if _VERBOSE
728  cout << "Binary false: dist_centers1 with " << neigRef.id << endl;
729  #endif
730  return false;}
731  //++nCheckConditions;
732  }
733  else if(RefBothFull || CheckBothFull)
734  {
735  if( rel_dist_centers < configLocaliser.dist_threshold_inv || rel_dist_centers > configLocaliser.dist_threshold ){
736  #if _VERBOSE
737  cout << "Binary false: dist_centers2 with " << neigRef.id << endl;
738  #endif
739  return false;}
740  //++nCheckConditions;
741  }
742  else if( !Ref.bFromStructure && !Check.bFromStructure && !neigRef.bFromStructure && !neigCheck.bFromStructure )
743  if( rel_dist_centers < configLocaliser.dist_threshold_inv || rel_dist_centers > configLocaliser.dist_threshold ){
744  #if _VERBOSE
745  cout << "Binary false: dist_centers3 with " << neigRef.id << endl;
746  #endif
747  return false;}
748  //++nCheckConditions;
749  }
750  else
751  {
752  if( Ref.areaVoxels< 1 && neigCheck.areaVoxels< 1 && Check.areaVoxels< 1 && neigRef.areaVoxels< 1 ){ // Use the restriction only when all the planes involved are smaller than 1m2
753  if( rel_dist_centers < configLocaliser.dist_threshold_inv || rel_dist_centers > configLocaliser.dist_threshold){
754  #if _VERBOSE
755  cout << "Binary false: dist_centers4 with " << neigRef.id << endl;
756  #endif
757  return false;}
758  //++nCheckConditions;
759  }
760  }
761 
762  // We do not check ppal direction constraint -> It has demonstrated to be very little distinctive
763  #if _VERBOSE
764  cout << "BINARY TRUE\n" << endl;
765  #endif
766 
767  return true;
768 }
769 
770 bool isSubgraphContained(map<unsigned, unsigned> &contained, map<unsigned, unsigned> &container)
771 {
772  if( contained.size() > container.size() )
773  return false;
774 
775  //is Subgraph Contained?
776  for(map<unsigned, unsigned>::iterator it = contained.begin(); it != contained.end(); it++)
777  if(container.count(it->first) == 0)
778  return false;
779  else if(container[it->first] != it->second)
780  return false;
781 
782  #if _VERBOSE
783  cout << "Repeated sequence. Comparing:\n";
784  for(map<unsigned, unsigned>::iterator it = contained.begin(); it != contained.end(); it++)
785  cout << it->first << " " << it->second << endl;
786  cout << "with\n";
787  for(map<unsigned, unsigned>::iterator it = container.begin(); it != container.end(); it++)
788  cout << it->first << " " << it->second << endl;
789  #endif
790 
791  return true;
792 }
793 
794 
795 /**!
796  * Recursive function that checks all the relations (direct and crossed) in the neighborhood of a plane.
797  * This function make redundant checks and therefore is NOT efficient at all
798  */
799 // TODO. A possible way to make this more efficient is by making an exclusion LUT according to single relations between planes
800 void SubgraphMatcher::exploreSubgraphTreeR(set<unsigned> &sourcePlanes, set<unsigned> &targetPlanes, map<unsigned, unsigned> &matched)
801 {
802  #if _VERBOSE
803  cout << "matched: " << matched.size() << "\n";
804  for(map<unsigned, unsigned>::iterator it = matched.begin(); it != matched.end(); it++)
805  cout << it->first << " - " << it->second << " =" << subgraphTrg->pPBM->vPlanes[it->second].label << endl;
806  cout << "sourcePlanes: " << sourcePlanes.size() << ": ";
807  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
808  cout << *it1 << " ";
809  cout << "\ntargetPlanes " << targetPlanes.size() << ": ";
810  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
811  cout << *it2 << " ";
812 // cout << subgraphTrg->pPBM->vPlanes[*it2].label << " ";
813  cout << endl;
814  #endif
815 
816 // // Stop the search when we find a match of maximum size
817 // if( winnerMatch.size() == subgraphSrc->subgraphPlanesIdx.size() || winnerMatch.size() == subgraphTrg->subgraphPlanesIdx.size() )
818 // return;
819 
820  unsigned requiredMatches = max(configLocaliser.min_planes_recognition, static_cast<unsigned>(winnerMatch.size()));
821 // if( sourcePlanes.empty() ||
822 // targetPlanes.empty() ||
823 // if( (matched.size() + min(sourcePlanes.size(),targetPlanes.size())) <= requiredMatches )
824 //// static_cast<int>(sourcePlanes.size() ) < requiredMatches ||
825 //// static_cast<int>(targetPlanes.size() ) < requiredMatches ) // New condition to speed up the search when there are not a minimum number of candidates
826 // {
827 //// cout << "End branch recursive search. matched " << matched.size() << " prev winner " << winnerMatch.size() << endl;
828 //// if(matched.size() > winnerMatch.size())
829 //// winnerMatch = matched;
830 // #if _VERBOSE
831 // cout << "End branch recursive search. Too short " << matched.size() << " prev winner " << winnerMatch.size() << endl;
832 // #endif
833 // return;
834 // }
835 
836  while(!sourcePlanes.empty())
837  {
838  set<unsigned>::iterator it1 = sourcePlanes.begin();
839  #if _VERBOSE
840  cout << "Compare " << *it1 << " Compare Compare Compare Compare Compare " << endl;
841  #endif
842 
843  if( (matched.size() + min(sourcePlanes.size(),targetPlanes.size())) <= requiredMatches )
844  {
845  #if _VERBOSE
846  cout << "End branch recursive search. Too short " << matched.size() << " prev winner " << winnerMatch.size() << endl;
847  #endif
848  return;
849  }
850 
851 // if( (calcAreaMatched(matched) + calcAreaUnmatched(sourcePlanes)) < areaWinnerMatch )
852 // {
853 // #if _VERBOSE
854 // cout << "End branch recursive search. Small Area " << matched.size() << " prev winner " << winnerMatch.size() << endl;
855 // #endif
856 // return;
857 // }
858 
859  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
860  {
861  #if _VERBOSE
862  cout << " " << *it1 << " with " << *it2 << endl;
863  #endif
864 // bool alreadyEval = false;
865 // for(unsigned i=0; i<alreadyExplored.size(); i++)
866 // {
867 // map<unsigned, unsigned> checkMatch = matched;
868 // checkMatch[*it1] = *it2;
869 // if( matched.size() + min(sourcePlanes.size(),targetPlanes.size()) < alreadyExplored[i].size())
870 // {
871 // if( isSubgraphContained(checkMatch, alreadyExplored[i]) )
872 // {
873 //// cout << "Combination already evaluated\n";
874 // alreadyEval = true;
875 // break;
876 // }
877 // }
878 //
879 // }
880 // if(alreadyEval)
881 // continue;
882 
883  // Check that it1 and it2 correspond to the same plane
884 // if(hashUnaryConstraints[*it1][*it2] == -1)
885 // hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
886 // if( !evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true : false ) )
887  if( hashUnaryConstraints[*it1][*it2] != 1 )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true : false ) )
888  continue;
889 
890  bool binaryFail = false;
891  for(map<unsigned, unsigned>::iterator it_matched = matched.begin(); it_matched != matched.end(); it_matched++)
892  if( !evalBinaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphSrc->pPBM->vPlanes[it_matched->first], subgraphTrg->pPBM->vPlanes[*it2], subgraphTrg->pPBM->vPlanes[it_matched->second]) )
893  {
894  binaryFail = true;
895  break;
896  }
897  if(binaryFail)
898  continue;
899 
900 // cout << "Match edge\n";
901  // If this point is reached, the planes it1 and it2 are candidates to be the same
902  set<unsigned> nextSrcPlanes = sourcePlanes;
903  nextSrcPlanes.erase(*it1);
904  set<unsigned> nextTrgPlanes = targetPlanes;
905  nextTrgPlanes.erase(*it2);
906  map<unsigned, unsigned> nextMatched = matched;
907  nextMatched[*it1] = *it2;
908 
909  alreadyExplored.push_back(nextMatched);
910 
911  exploreSubgraphTreeR(nextSrcPlanes, nextTrgPlanes, nextMatched);
912 
913 // // CHANGE to be As in the algorithm of our article
914 // if(matched.size() > bestCombination.size())
915 // return bestCombination;
916 
917  }
918  sourcePlanes.erase(it1);
919  } // End while
920 
921  if(matched.size() > winnerMatch.size()){
922  float areaMatched = calcAreaMatched(matched);
923 // if(areaMatched > areaWinnerMatch){
924  #if _VERBOSE
925  cout << "End branch recursive search. matched " << matched.size() << " A " << areaMatched << " prev winner " << winnerMatch.size() << " A " << areaWinnerMatch << endl;
926  #endif
927  areaWinnerMatch = areaMatched;
928  winnerMatch = matched;}
929 }
930 
931 void SubgraphMatcher::exploreSubgraphTreeR_Area(set<unsigned> &sourcePlanes, set<unsigned> &targetPlanes, map<unsigned, unsigned> &matched)
932 {
933  #if _VERBOSE
934  cout << "matched: " << matched.size() << "\n";
935  for(map<unsigned, unsigned>::iterator it = matched.begin(); it != matched.end(); it++)
936  cout << it->first << " - " << it->second << " =" << subgraphTrg->pPBM->vPlanes[it->second].label << endl;
937  cout << "sourcePlanes: " << sourcePlanes.size() << ": ";
938  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
939  cout << *it1 << " ";
940  cout << "\ntargetPlanes " << targetPlanes.size() << ": ";
941  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
942  cout << *it2 << " ";
943 // cout << subgraphTrg->pPBM->vPlanes[*it2].label << " ";
944  cout << endl;
945  #endif
946 
947 // // Stop the search when we find a match of maximum size
948 // if( winnerMatch.size() == subgraphSrc->subgraphPlanesIdx.size() || winnerMatch.size() == subgraphTrg->subgraphPlanesIdx.size() )
949 // return;
950 
951  float matchedArea = calcAreaMatched(matched);
952 
953  float unmatchedArea = 0; // Pass as value parameter
954  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
955  unmatchedArea += subgraphSrc->pPBM->vPlanes[*it1].areaHull;
956 
957 // unsigned requiredMatches = max(configLocaliser.min_planes_recognition, static_cast<unsigned>(winnerMatch.size()));
958 
959  while(!sourcePlanes.empty())
960  {
961  set<unsigned>::iterator it1 = sourcePlanes.begin();
962  #if _VERBOSE
963  cout << "Compare " << *it1 << " Compare Compare Compare Compare Compare " << endl;
964  #endif
965 
966  if( (matched.size() + min(sourcePlanes.size(),targetPlanes.size())) <= configLocaliser.min_planes_recognition )
967  if( (matchedArea + unmatchedArea) < areaWinnerMatch )
968  {
969  #if _VERBOSE
970  cout << "End branch recursive search. Too short " << matched.size() << " prev winner " << winnerMatch.size() << endl;
971  #endif
972  return;
973  }
974 
975 // if( (calcAreaMatched(matched) + calcAreaUnmatched(sourcePlanes)) < areaWinnerMatch )
976 // {
977 // #if _VERBOSE
978 // cout << "End branch recursive search. Small Area " << matched.size() << " prev winner " << winnerMatch.size() << endl;
979 // #endif
980 // return;
981 // }
982 
983  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
984  {
985  #if _VERBOSE
986  cout << " " << *it1 << " with " << *it2 << endl;
987  #endif
988 // bool alreadyEval = false;
989 // for(unsigned i=0; i<alreadyExplored.size(); i++)
990 // {
991 // map<unsigned, unsigned> checkMatch = matched;
992 // checkMatch[*it1] = *it2;
993 // if( matched.size() + min(sourcePlanes.size(),targetPlanes.size()) < alreadyExplored[i].size())
994 // {
995 // if( isSubgraphContained(checkMatch, alreadyExplored[i]) )
996 // {
997 //// cout << "Combination already evaluated\n";
998 // alreadyEval = true;
999 // break;
1000 // }
1001 // }
1002 //
1003 // }
1004 // if(alreadyEval)
1005 // continue;
1006 
1007  // Check that it1 and it2 correspond to the same plane
1008 // if(hashUnaryConstraints[*it1][*it2] == -1)
1009 // hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1010 // if( !evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true : false ) )
1011  if( hashUnaryConstraints[*it1][*it2] != 1 )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true : false ) )
1012  continue;
1013 
1014  bool binaryFail = false;
1015  for(map<unsigned, unsigned>::iterator it_matched = matched.begin(); it_matched != matched.end(); it_matched++)
1016  if( !evalBinaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphSrc->pPBM->vPlanes[it_matched->first], subgraphTrg->pPBM->vPlanes[*it2], subgraphTrg->pPBM->vPlanes[it_matched->second]) )
1017  {
1018  binaryFail = true;
1019  break;
1020  }
1021  if(binaryFail)
1022  continue;
1023 
1024 // cout << "Match edge\n";
1025  // If this point is reached, the planes it1 and it2 are candidates to be the same
1026  set<unsigned> nextSrcPlanes = sourcePlanes;
1027  nextSrcPlanes.erase(*it1);
1028  set<unsigned> nextTrgPlanes = targetPlanes;
1029  nextTrgPlanes.erase(*it2);
1030  map<unsigned, unsigned> nextMatched = matched;
1031  nextMatched[*it1] = *it2;
1032 
1033  alreadyExplored.push_back(nextMatched);
1034 
1035  exploreSubgraphTreeR_Area(nextSrcPlanes, nextTrgPlanes, nextMatched);
1036 
1037 // // CHANGE to be As in the algorithm of our article
1038 // if(matched.size() > bestCombination.size())
1039 // return bestCombination;
1040 
1041  }
1042  sourcePlanes.erase(it1);
1043  } // End while
1044 
1045 // if(matched.size() > winnerMatch.size()){
1046  float areaMatched = calcAreaMatched(matched);
1047  if(areaMatched > areaWinnerMatch){
1048  #if _VERBOSE
1049  cout << "End branch recursive search. matched " << matched.size() << " A " << areaMatched << " prev winner " << winnerMatch.size() << " A " << areaWinnerMatch << endl;
1050  #endif
1051  areaWinnerMatch = areaMatched;
1052  winnerMatch = matched;}
1053 }
1054 
1055 float SubgraphMatcher::calcAreaMatched(std::map<unsigned,unsigned> &matched_planes)
1056 {
1057  float areaMatched = 0;
1058  for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
1059  areaMatched += subgraphSrc->pPBM->vPlanes[it->first].areaHull;
1060 
1061  return areaMatched;
1062 }
1063 
1064 float SubgraphMatcher::calcAreaUnmatched(std::set<unsigned> &unmatched_planes)
1065 {
1066  float areaUnatched = 0;
1067  for(set<unsigned>::iterator it = unmatched_planes.begin(); it != unmatched_planes.end(); it++)
1068  areaUnatched += subgraphSrc->pPBM->vPlanes[*it].areaHull;
1069 
1070  return areaUnatched;
1071 }
1072 
1073 //std::map<unsigned,unsigned> SubgraphMatcher::compareSubgraphs(Subgraph &subgraphSource, Subgraph &subgraphTarget)
1074 //{
1075 ////cout << "SubgraphMatcher::compareSubgraphs... \n";
1076 // subgraphSrc = &subgraphSource;
1077 // subgraphTrg = &subgraphTarget;
1078 // map<unsigned, unsigned> matched;
1079 //
1080 // areaWinnerMatch = 0;
1081 // winnerMatch.clear();
1082 // alreadyExplored.clear(); // MODIFICAR: Tener en cuenta caminos explorados cuando exploramos grafos vecinos
1083 // std::set<unsigned> sourcePlanes = subgraphSrc->subgraphPlanesIdx;
1084 // std::set<unsigned> targetPlanes = subgraphTrg->subgraphPlanesIdx;
1085 //
1086 // hashUnaryConstraints = std::vector<std::vector<int8_t> >(subgraphSrc->pPBM->vPlanes.size(), std::vector<int8_t>(subgraphTrg->pPBM->vPlanes.size()) );
1087 // for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
1088 // for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
1089 // hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1090 //
1091 // exploreSubgraphTreeR(sourcePlanes, targetPlanes, matched);
1092 //// exploreSubgraphTreeR_Area(sourcePlanes, targetPlanes, matched);
1093 //#if _VERBOSE
1094 // cout << "Area winnerMatch " << areaWinnerMatch << endl;
1095 //#endif
1096 //
1097 // return winnerMatch;
1098 //}
1099 
1100 std::map<unsigned,unsigned> SubgraphMatcher::compareSubgraphs(Subgraph &subgraphSource, Subgraph &subgraphTarget, const int option)
1101 {
1102 //cout << "SubgraphMatcher::compareSubgraphs... \n";
1103  subgraphSrc = &subgraphSource;
1104  subgraphTrg = &subgraphTarget;
1105  map<unsigned, unsigned> matched;
1106 
1107  areaWinnerMatch = 0;
1108  winnerMatch.clear();
1109  alreadyExplored.clear(); // MODIFICAR: Tener en cuenta caminos explorados cuando exploramos grafos vecinos
1110  std::set<unsigned> sourcePlanes = subgraphSrc->subgraphPlanesIdx;
1111  std::set<unsigned> targetPlanes = subgraphTrg->subgraphPlanesIdx;
1112 
1113 #if _VERBOSE
1114  cout << "Source planes: ";
1115  for(set<unsigned>::iterator it2 = sourcePlanes.begin(); it2 != sourcePlanes.end(); it2++)
1116  cout << " " << *it2;
1117  cout << endl;
1118  cout << "Target planes: ";
1119  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
1120  cout << " " << *it2;
1121  cout << endl;
1122 #endif
1123 
1124  // Fill Hash table of unary constraints
1125  hashUnaryConstraints = std::vector<std::vector<int8_t> >(subgraphSrc->pPBM->vPlanes.size(), std::vector<int8_t>(subgraphTrg->pPBM->vPlanes.size()) );
1126  if(option == 0) // Default subgraph matcher
1127  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
1128  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
1129  hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1130  else if(option == 1) // Odometry graph matcher
1131  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
1132  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
1133  hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraintsOdometry(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1134  else if(option == 2) // Default graph matcher restricted to planar movement (fix plane x=const)
1135  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
1136  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
1137  hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraints2D(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1138  else if(option == 3) // Odometry graph matcher restricted to planar movement (fix plane x=const)
1139  for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 != sourcePlanes.end(); it1++)
1140  for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 != targetPlanes.end(); it2++)
1141  hashUnaryConstraints[*it1][*it2] = (evalUnaryConstraintsOdometry2D(subgraphSrc->pPBM->vPlanes[*it1], subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1142 
1143  exploreSubgraphTreeR(sourcePlanes, targetPlanes, matched);
1144 // exploreSubgraphTreeR_Area(sourcePlanes, targetPlanes, matched);
1145 #if _VERBOSE
1146  cout << "Area winnerMatch " << areaWinnerMatch << endl;
1147 #endif
1148 
1149  return winnerMatch;
1150 }
#define min(a, b)
A class used to store a planar feature (Plane for short).
Definition: Plane.h:48
Scalar * iterator
Definition: eigen_plugins.h:23
float elongation
Definition: Plane.h:137
JCOPY_OPTION option
Definition: transupp.h:121
bool bFullExtent
Definition: Plane.h:140
double time2
float areaVoxels
Definition: Plane.h:138
Eigen::Vector3f v3normal
Definition: Plane.h:132
double BhattacharyyaDist_(std::vector< float > &hist1, std::vector< float > &hist2)
bool bFromStructure
Definition: Plane.h:141
float areaHull
Definition: Plane.h:139
#define RAD2DEG
bool isSubgraphContained(map< unsigned, unsigned > &contained, map< unsigned, unsigned > &container)
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:117
std::vector< float > hist_H
Definition: Plane.h:153
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:131
std::set< unsigned > subgraphPlanesIdx
Definition: Subgraph.h:62
double time1
A class used to store a Plane-based Map (PbMap).
Definition: PbMap.h:45
CONTAINER::Scalar norm(const CONTAINER &v)



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