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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019