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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019