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-2018, 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
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(
882  RAD2DEG(
883  acos(Ref.v3normal.dot(neigRef.v3normal)) -
884  acos(Check.v3normal.dot(neigCheck.v3normal))));
885  // if( dif_normal > configLocaliser.angle_threshold ){
886  if (dif_normal > std::max(
887  configLocaliser.angle_threshold,
888  2 * (Ref.v3center - neigRef.v3center).norm()))
889  {
890 #if _VERBOSE
891  cout << "Binary false: angle " << dif_normal << " with " << neigRef.id
892  << endl;
893 #endif
894  return false;
895  }
896  //++nCheckConditions;
897 
898  // Relative distance
899  double rel_dist_centers = sqrt(
900  (Ref.v3center - neigRef.v3center).dot(Ref.v3center - neigRef.v3center) /
901  ((Check.v3center - neigCheck.v3center)
902  .dot(Check.v3center - neigCheck.v3center)));
903 
904  // If the plane has been fully detected use a narrower threshold for the
905  // comparison
906  bool RefBothFull =
907  (Ref.bFullExtent && neigRef.bFullExtent); // ? true : false;
908  bool CheckBothFull =
909  (Check.bFullExtent && neigCheck.bFullExtent); // ? true : false;
910 
911  if (configLocaliser.use_completeness)
912  {
913  if (RefBothFull && CheckBothFull)
914  {
915  if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
916  rel_dist_centers > configLocaliser.dist_threshold)
917  {
918 #if _VERBOSE
919  cout << "Binary false: dist_centers1 with " << neigRef.id
920  << endl;
921 #endif
922  return false;
923  }
924  //++nCheckConditions;
925  }
926  else if (RefBothFull || CheckBothFull)
927  {
928  if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
929  rel_dist_centers > configLocaliser.dist_threshold)
930  {
931 #if _VERBOSE
932  cout << "Binary false: dist_centers2 with " << neigRef.id
933  << endl;
934 #endif
935  return false;
936  }
937  //++nCheckConditions;
938  }
939  else if (
940  !Ref.bFromStructure && !Check.bFromStructure &&
941  !neigRef.bFromStructure && !neigCheck.bFromStructure)
942  if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
943  rel_dist_centers > configLocaliser.dist_threshold)
944  {
945 #if _VERBOSE
946  cout << "Binary false: dist_centers3 with " << neigRef.id
947  << endl;
948 #endif
949  return false;
950  }
951  //++nCheckConditions;
952  }
953  else
954  {
955  if (Ref.areaVoxels < 1 && neigCheck.areaVoxels < 1 &&
956  Check.areaVoxels < 1 && neigRef.areaVoxels < 1)
957  { // Use the restriction only when all the planes involved are smaller
958  // than 1m2
959  if (rel_dist_centers < configLocaliser.dist_threshold_inv ||
960  rel_dist_centers > configLocaliser.dist_threshold)
961  {
962 #if _VERBOSE
963  cout << "Binary false: dist_centers4 with " << neigRef.id
964  << endl;
965 #endif
966  return false;
967  }
968  //++nCheckConditions;
969  }
970  }
971 
972 // We do not check ppal direction constraint -> It has demonstrated to be very
973 // little distinctive
974 #if _VERBOSE
975  cout << "BINARY TRUE\n" << endl;
976 #endif
977 
978  return true;
979 }
980 
982  map<unsigned, unsigned>& contained, map<unsigned, unsigned>& container)
983 {
984  if (contained.size() > container.size()) return false;
985 
986  // is Subgraph Contained?
987  for (map<unsigned, unsigned>::iterator it = contained.begin();
988  it != contained.end(); it++)
989  if (container.count(it->first) == 0)
990  return false;
991  else if (container[it->first] != it->second)
992  return false;
993 
994 #if _VERBOSE
995  cout << "Repeated sequence. Comparing:\n";
996  for (map<unsigned, unsigned>::iterator it = contained.begin();
997  it != contained.end(); it++)
998  cout << it->first << " " << it->second << endl;
999  cout << "with\n";
1000  for (map<unsigned, unsigned>::iterator it = container.begin();
1001  it != container.end(); it++)
1002  cout << it->first << " " << it->second << endl;
1003 #endif
1004 
1005  return true;
1006 }
1007 
1008 /**!
1009  * Recursive function that checks all the relations (direct and crossed) in the
1010  * neighborhood of a plane.
1011  * This function make redundant checks and therefore is NOT efficient at all
1012  */
1013 // TODO. A possible way to make this more efficient is by making an exclusion
1014 // LUT according to single relations between planes
1015 void SubgraphMatcher::exploreSubgraphTreeR(
1016  set<unsigned>& sourcePlanes, set<unsigned>& targetPlanes,
1017  map<unsigned, unsigned>& matched)
1018 {
1019 #if _VERBOSE
1020  cout << "matched: " << matched.size() << "\n";
1021  for (map<unsigned, unsigned>::iterator it = matched.begin();
1022  it != matched.end(); it++)
1023  cout << it->first << " - " << it->second << " ="
1024  << subgraphTrg->pPBM->vPlanes[it->second].label << endl;
1025  cout << "sourcePlanes: " << sourcePlanes.size() << ": ";
1026  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1027  it1 != sourcePlanes.end(); it1++)
1028  cout << *it1 << " ";
1029  cout << "\ntargetPlanes " << targetPlanes.size() << ": ";
1030  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1031  it2 != targetPlanes.end(); it2++)
1032  cout << *it2 << " ";
1033  // cout << subgraphTrg->pPBM->vPlanes[*it2].label << " ";
1034  cout << endl;
1035 #endif
1036 
1037  // // Stop the search when we find a match of maximum size
1038  // if( winnerMatch.size() == subgraphSrc->subgraphPlanesIdx.size() ||
1039  // winnerMatch.size() == subgraphTrg->subgraphPlanesIdx.size() )
1040  // return;
1041 
1042  unsigned requiredMatches =
1043  max(configLocaliser.min_planes_recognition,
1044  static_cast<unsigned>(winnerMatch.size()));
1045  // if( sourcePlanes.empty() ||
1046  // targetPlanes.empty() ||
1047  // if( (matched.size() + min(sourcePlanes.size(),targetPlanes.size())) <=
1048  // requiredMatches )
1049  //// static_cast<int>(sourcePlanes.size() ) < requiredMatches ||
1050  //// static_cast<int>(targetPlanes.size() ) < requiredMatches ) // New
1051  /// condition to speed up the search when there are not a minimum number of
1052  /// candidates
1053  // {
1054  //// cout << "End branch recursive search. matched " << matched.size() << "
1055  /// prev winner " << winnerMatch.size() << endl;
1056  //// if(matched.size() > winnerMatch.size())
1057  //// winnerMatch = matched;
1058  // #if _VERBOSE
1059  // cout << "End branch recursive search. Too short " << matched.size()
1060  // << " prev winner " << winnerMatch.size() << endl;
1061  // #endif
1062  // return;
1063  // }
1064 
1065  while (!sourcePlanes.empty())
1066  {
1067  set<unsigned>::iterator it1 = sourcePlanes.begin();
1068 #if _VERBOSE
1069  cout << "Compare " << *it1
1070  << " Compare Compare Compare Compare Compare " << endl;
1071 #endif
1072 
1073  if ((matched.size() + min(sourcePlanes.size(), targetPlanes.size())) <=
1074  requiredMatches)
1075  {
1076 #if _VERBOSE
1077  cout << "End branch recursive search. Too short " << matched.size()
1078  << " prev winner " << winnerMatch.size() << endl;
1079 #endif
1080  return;
1081  }
1082 
1083  // if( (calcAreaMatched(matched) + calcAreaUnmatched(sourcePlanes)) <
1084  // areaWinnerMatch )
1085  // {
1086  // #if _VERBOSE
1087  // cout << "End branch recursive search. Small Area " <<
1088  // matched.size() << " prev winner " << winnerMatch.size() <<
1089  // endl;
1090  // #endif
1091  // return;
1092  // }
1093 
1094  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1095  it2 != targetPlanes.end(); it2++)
1096  {
1097 #if _VERBOSE
1098  cout << " " << *it1 << " with " << *it2 << endl;
1099 #endif
1100  // bool alreadyEval = false;
1101  // for(unsigned i=0; i<alreadyExplored.size(); i++)
1102  // {
1103  // map<unsigned, unsigned> checkMatch = matched;
1104  // checkMatch[*it1] = *it2;
1105  // if( matched.size() +
1106  // min(sourcePlanes.size(),targetPlanes.size()) <
1107  // alreadyExplored[i].size())
1108  // {
1109  // if( isSubgraphContained(checkMatch, alreadyExplored[i])
1110  // )
1111  // {
1112  //// cout << "Combination already evaluated\n";
1113  // alreadyEval = true;
1114  // break;
1115  // }
1116  // }
1117  //
1118  // }
1119  // if(alreadyEval)
1120  // continue;
1121 
1122  // Check that it1 and it2 correspond to the same plane
1123  // if(hashUnaryConstraints[*it1][*it2] == -1)
1124  // hashUnaryConstraints[*it1][*it2] =
1125  // (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1],
1126  // subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1127  // false ) ? 1 : 0);
1128  // if( !evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1],
1129  // subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false
1130  // ) )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true :
1131  // false ) )
1132  if (hashUnaryConstraints[*it1][*it2] != 1) //(FloorPlane != -1 &&
1133  // FloorPlaneMap != -1) ?
1134  // true : false ) )
1135  continue;
1136 
1137  bool binaryFail = false;
1138  for (map<unsigned, unsigned>::iterator it_matched = matched.begin();
1139  it_matched != matched.end(); it_matched++)
1140  if (!evalBinaryConstraints(
1141  subgraphSrc->pPBM->vPlanes[*it1],
1142  subgraphSrc->pPBM->vPlanes[it_matched->first],
1143  subgraphTrg->pPBM->vPlanes[*it2],
1144  subgraphTrg->pPBM->vPlanes[it_matched->second]))
1145  {
1146  binaryFail = true;
1147  break;
1148  }
1149  if (binaryFail) continue;
1150 
1151  // cout << "Match edge\n";
1152  // If this point is reached, the planes it1 and it2 are candidates
1153  // to be the same
1154  set<unsigned> nextSrcPlanes = sourcePlanes;
1155  nextSrcPlanes.erase(*it1);
1156  set<unsigned> nextTrgPlanes = targetPlanes;
1157  nextTrgPlanes.erase(*it2);
1158  map<unsigned, unsigned> nextMatched = matched;
1159  nextMatched[*it1] = *it2;
1160 
1161  alreadyExplored.push_back(nextMatched);
1162 
1163  exploreSubgraphTreeR(nextSrcPlanes, nextTrgPlanes, nextMatched);
1164 
1165  // // CHANGE to be As in the algorithm of our article
1166  // if(matched.size() > bestCombination.size())
1167  // return bestCombination;
1168  }
1169  sourcePlanes.erase(it1);
1170  } // End while
1171 
1172  if (matched.size() > winnerMatch.size())
1173  {
1174  float areaMatched = calcAreaMatched(matched);
1175 // if(areaMatched > areaWinnerMatch){
1176 #if _VERBOSE
1177  cout << "End branch recursive search. matched " << matched.size()
1178  << " A " << areaMatched << " prev winner " << winnerMatch.size()
1179  << " A " << areaWinnerMatch << endl;
1180 #endif
1181  areaWinnerMatch = areaMatched;
1182  winnerMatch = matched;
1183  }
1184 }
1185 
1186 void SubgraphMatcher::exploreSubgraphTreeR_Area(
1187  set<unsigned>& sourcePlanes, set<unsigned>& targetPlanes,
1188  map<unsigned, unsigned>& matched)
1189 {
1190 #if _VERBOSE
1191  cout << "matched: " << matched.size() << "\n";
1192  for (map<unsigned, unsigned>::iterator it = matched.begin();
1193  it != matched.end(); it++)
1194  cout << it->first << " - " << it->second << " ="
1195  << subgraphTrg->pPBM->vPlanes[it->second].label << endl;
1196  cout << "sourcePlanes: " << sourcePlanes.size() << ": ";
1197  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1198  it1 != sourcePlanes.end(); it1++)
1199  cout << *it1 << " ";
1200  cout << "\ntargetPlanes " << targetPlanes.size() << ": ";
1201  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1202  it2 != targetPlanes.end(); it2++)
1203  cout << *it2 << " ";
1204  // cout << subgraphTrg->pPBM->vPlanes[*it2].label << " ";
1205  cout << endl;
1206 #endif
1207 
1208  // // Stop the search when we find a match of maximum size
1209  // if( winnerMatch.size() == subgraphSrc->subgraphPlanesIdx.size() ||
1210  // winnerMatch.size() == subgraphTrg->subgraphPlanesIdx.size() )
1211  // return;
1212 
1213  float matchedArea = calcAreaMatched(matched);
1214 
1215  float unmatchedArea = 0; // Pass as value parameter
1216  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1217  it1 != sourcePlanes.end(); it1++)
1218  unmatchedArea += subgraphSrc->pPBM->vPlanes[*it1].areaHull;
1219 
1220  // unsigned requiredMatches = max(configLocaliser.min_planes_recognition,
1221  // static_cast<unsigned>(winnerMatch.size()));
1222 
1223  while (!sourcePlanes.empty())
1224  {
1225  set<unsigned>::iterator it1 = sourcePlanes.begin();
1226 #if _VERBOSE
1227  cout << "Compare " << *it1
1228  << " Compare Compare Compare Compare Compare " << endl;
1229 #endif
1230 
1231  if ((matched.size() + min(sourcePlanes.size(), targetPlanes.size())) <=
1232  configLocaliser.min_planes_recognition)
1233  if ((matchedArea + unmatchedArea) < areaWinnerMatch)
1234  {
1235 #if _VERBOSE
1236  cout << "End branch recursive search. Too short "
1237  << matched.size() << " prev winner " << winnerMatch.size()
1238  << endl;
1239 #endif
1240  return;
1241  }
1242 
1243  // if( (calcAreaMatched(matched) + calcAreaUnmatched(sourcePlanes)) <
1244  // areaWinnerMatch )
1245  // {
1246  // #if _VERBOSE
1247  // cout << "End branch recursive search. Small Area " <<
1248  // matched.size() << " prev winner " << winnerMatch.size() <<
1249  // endl;
1250  // #endif
1251  // return;
1252  // }
1253 
1254  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1255  it2 != targetPlanes.end(); it2++)
1256  {
1257 #if _VERBOSE
1258  cout << " " << *it1 << " with " << *it2 << endl;
1259 #endif
1260  // bool alreadyEval = false;
1261  // for(unsigned i=0; i<alreadyExplored.size(); i++)
1262  // {
1263  // map<unsigned, unsigned> checkMatch = matched;
1264  // checkMatch[*it1] = *it2;
1265  // if( matched.size() +
1266  // min(sourcePlanes.size(),targetPlanes.size()) <
1267  // alreadyExplored[i].size())
1268  // {
1269  // if( isSubgraphContained(checkMatch, alreadyExplored[i])
1270  // )
1271  // {
1272  //// cout << "Combination already evaluated\n";
1273  // alreadyEval = true;
1274  // break;
1275  // }
1276  // }
1277  //
1278  // }
1279  // if(alreadyEval)
1280  // continue;
1281 
1282  // Check that it1 and it2 correspond to the same plane
1283  // if(hashUnaryConstraints[*it1][*it2] == -1)
1284  // hashUnaryConstraints[*it1][*it2] =
1285  // (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1],
1286  // subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1287  // false ) ? 1 : 0);
1288  // if( !evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1],
1289  // subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false
1290  // ) )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true :
1291  // false ) )
1292  if (hashUnaryConstraints[*it1][*it2] != 1) //(FloorPlane != -1 &&
1293  // FloorPlaneMap != -1) ?
1294  // true : false ) )
1295  continue;
1296 
1297  bool binaryFail = false;
1298  for (map<unsigned, unsigned>::iterator it_matched = matched.begin();
1299  it_matched != matched.end(); it_matched++)
1300  if (!evalBinaryConstraints(
1301  subgraphSrc->pPBM->vPlanes[*it1],
1302  subgraphSrc->pPBM->vPlanes[it_matched->first],
1303  subgraphTrg->pPBM->vPlanes[*it2],
1304  subgraphTrg->pPBM->vPlanes[it_matched->second]))
1305  {
1306  binaryFail = true;
1307  break;
1308  }
1309  if (binaryFail) continue;
1310 
1311  // cout << "Match edge\n";
1312  // If this point is reached, the planes it1 and it2 are candidates
1313  // to be the same
1314  set<unsigned> nextSrcPlanes = sourcePlanes;
1315  nextSrcPlanes.erase(*it1);
1316  set<unsigned> nextTrgPlanes = targetPlanes;
1317  nextTrgPlanes.erase(*it2);
1318  map<unsigned, unsigned> nextMatched = matched;
1319  nextMatched[*it1] = *it2;
1320 
1321  alreadyExplored.push_back(nextMatched);
1322 
1323  exploreSubgraphTreeR_Area(
1324  nextSrcPlanes, nextTrgPlanes, nextMatched);
1325 
1326  // // CHANGE to be As in the algorithm of our article
1327  // if(matched.size() > bestCombination.size())
1328  // return bestCombination;
1329  }
1330  sourcePlanes.erase(it1);
1331  } // End while
1332 
1333  // if(matched.size() > winnerMatch.size()){
1334  float areaMatched = calcAreaMatched(matched);
1335  if (areaMatched > areaWinnerMatch)
1336  {
1337 #if _VERBOSE
1338  cout << "End branch recursive search. matched " << matched.size()
1339  << " A " << areaMatched << " prev winner " << winnerMatch.size()
1340  << " A " << areaWinnerMatch << endl;
1341 #endif
1342  areaWinnerMatch = areaMatched;
1343  winnerMatch = matched;
1344  }
1345 }
1346 
1347 float SubgraphMatcher::calcAreaMatched(
1348  std::map<unsigned, unsigned>& matched_planes)
1349 {
1350  float areaMatched = 0;
1351  for (map<unsigned, unsigned>::iterator it = matched_planes.begin();
1352  it != matched_planes.end(); it++)
1353  areaMatched += subgraphSrc->pPBM->vPlanes[it->first].areaHull;
1354 
1355  return areaMatched;
1356 }
1357 
1358 float SubgraphMatcher::calcAreaUnmatched(std::set<unsigned>& unmatched_planes)
1359 {
1360  float areaUnatched = 0;
1361  for (set<unsigned>::iterator it = unmatched_planes.begin();
1362  it != unmatched_planes.end(); it++)
1363  areaUnatched += subgraphSrc->pPBM->vPlanes[*it].areaHull;
1364 
1365  return areaUnatched;
1366 }
1367 
1368 // std::map<unsigned,unsigned> SubgraphMatcher::compareSubgraphs(Subgraph
1369 // &subgraphSource, Subgraph &subgraphTarget)
1370 //{
1371 ////cout << "SubgraphMatcher::compareSubgraphs... \n";
1372 // subgraphSrc = &subgraphSource;
1373 // subgraphTrg = &subgraphTarget;
1374 // map<unsigned, unsigned> matched;
1375 //
1376 // areaWinnerMatch = 0;
1377 // winnerMatch.clear();
1378 // alreadyExplored.clear(); // MODIFICAR: Tener en cuenta caminos explorados
1379 // cuando exploramos grafos vecinos
1380 // std::set<unsigned> sourcePlanes = subgraphSrc->subgraphPlanesIdx;
1381 // std::set<unsigned> targetPlanes = subgraphTrg->subgraphPlanesIdx;
1382 //
1383 // hashUnaryConstraints = std::vector<std::vector<int8_t>
1384 // >(subgraphSrc->pPBM->vPlanes.size(),
1385 // std::vector<int8_t>(subgraphTrg->pPBM->vPlanes.size()) );
1386 // for(set<unsigned>::iterator it1 = sourcePlanes.begin(); it1 !=
1387 // sourcePlanes.end(); it1++)
1388 // for(set<unsigned>::iterator it2 = targetPlanes.begin(); it2 !=
1389 // targetPlanes.end(); it2++)
1390 // hashUnaryConstraints[*it1][*it2] =
1391 // (evalUnaryConstraints(subgraphSrc->pPBM->vPlanes[*it1],
1392 // subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM, false ) ? 1 : 0);
1393 //
1394 // exploreSubgraphTreeR(sourcePlanes, targetPlanes, matched);
1395 //// exploreSubgraphTreeR_Area(sourcePlanes, targetPlanes, matched);
1396 //#if _VERBOSE
1397 // cout << "Area winnerMatch " << areaWinnerMatch << endl;
1398 //#endif
1399 //
1400 // return winnerMatch;
1401 //}
1402 
1403 std::map<unsigned, unsigned> SubgraphMatcher::compareSubgraphs(
1404  Subgraph& subgraphSource, Subgraph& subgraphTarget, const int option)
1405 {
1406  // cout << "SubgraphMatcher::compareSubgraphs... \n";
1407  subgraphSrc = &subgraphSource;
1408  subgraphTrg = &subgraphTarget;
1409  map<unsigned, unsigned> matched;
1410 
1411  areaWinnerMatch = 0;
1412  winnerMatch.clear();
1413  alreadyExplored.clear(); // MODIFICAR: Tener en cuenta caminos explorados
1414  // cuando exploramos grafos vecinos
1415  std::set<unsigned> sourcePlanes = subgraphSrc->subgraphPlanesIdx;
1416  std::set<unsigned> targetPlanes = subgraphTrg->subgraphPlanesIdx;
1417 
1418 #if _VERBOSE
1419  cout << "Source planes: ";
1420  for (set<unsigned>::iterator it2 = sourcePlanes.begin();
1421  it2 != sourcePlanes.end(); it2++)
1422  cout << " " << *it2;
1423  cout << endl;
1424  cout << "Target planes: ";
1425  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1426  it2 != targetPlanes.end(); it2++)
1427  cout << " " << *it2;
1428  cout << endl;
1429 #endif
1430 
1431  // Fill Hash table of unary constraints
1432  hashUnaryConstraints = std::vector<std::vector<int8_t>>(
1433  subgraphSrc->pPBM->vPlanes.size(),
1434  std::vector<int8_t>(subgraphTrg->pPBM->vPlanes.size()));
1435  if (option == 0) // Default subgraph matcher
1436  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1437  it1 != sourcePlanes.end(); it1++)
1438  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1439  it2 != targetPlanes.end(); it2++)
1440  hashUnaryConstraints[*it1][*it2] =
1441  (evalUnaryConstraints(
1442  subgraphSrc->pPBM->vPlanes[*it1],
1443  subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1444  false)
1445  ? 1
1446  : 0);
1447  else if (option == 1) // Odometry graph matcher
1448  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1449  it1 != sourcePlanes.end(); it1++)
1450  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1451  it2 != targetPlanes.end(); it2++)
1452  hashUnaryConstraints[*it1][*it2] =
1453  (evalUnaryConstraintsOdometry(
1454  subgraphSrc->pPBM->vPlanes[*it1],
1455  subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1456  false)
1457  ? 1
1458  : 0);
1459  else if (option == 2) // Default graph matcher restricted to planar
1460  // movement (fix plane x=const)
1461  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1462  it1 != sourcePlanes.end(); it1++)
1463  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1464  it2 != targetPlanes.end(); it2++)
1465  hashUnaryConstraints[*it1][*it2] =
1466  (evalUnaryConstraints2D(
1467  subgraphSrc->pPBM->vPlanes[*it1],
1468  subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1469  false)
1470  ? 1
1471  : 0);
1472  else if (option == 3) // Odometry graph matcher restricted to planar
1473  // movement (fix plane x=const)
1474  for (set<unsigned>::iterator it1 = sourcePlanes.begin();
1475  it1 != sourcePlanes.end(); it1++)
1476  for (set<unsigned>::iterator it2 = targetPlanes.begin();
1477  it2 != targetPlanes.end(); it2++)
1478  hashUnaryConstraints[*it1][*it2] =
1479  (evalUnaryConstraintsOdometry2D(
1480  subgraphSrc->pPBM->vPlanes[*it1],
1481  subgraphTrg->pPBM->vPlanes[*it2], *subgraphTrg->pPBM,
1482  false)
1483  ? 1
1484  : 0);
1485 
1486  exploreSubgraphTreeR(sourcePlanes, targetPlanes, matched);
1487 // exploreSubgraphTreeR_Area(sourcePlanes, targetPlanes, matched);
1488 #if _VERBOSE
1489  cout << "Area winnerMatch " << areaWinnerMatch << endl;
1490 #endif
1491 
1492  return winnerMatch;
1493 }
mrpt::pbmap::Plane::hist_H
std::vector< float > hist_H
Definition: Plane.h:165
mrpt::pbmap::Plane::bFromStructure
bool bFromStructure
Definition: Plane.h:153
mrpt::pbmap::Plane::areaVoxels
float areaVoxels
Definition: Plane.h:150
mrpt::pbmap::Subgraph
Definition: Subgraph.h:34
mrpt::pbmap::Plane::bFullExtent
bool bFullExtent
Definition: Plane.h:152
pbmap-precomp.h
mrpt::pbmap::Plane::areaHull
float areaHull
Definition: Plane.h:151
time2
double time2
Definition: SubgraphMatcher.cpp:21
mrpt::pbmap::Subgraph::subgraphPlanesIdx
std::set< unsigned > subgraphPlanesIdx
Definition: Subgraph.h:67
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:134
mrpt::pbmap::Plane::v3center
Eigen::Vector3f v3center
! Geometric description
Definition: Plane.h:141
BhattacharyyaDist_
double BhattacharyyaDist_(std::vector< float > &hist1, std::vector< float > &hist2)
Definition: SubgraphMatcher.cpp:27
mrpt::pbmap::Plane::elongation
float elongation
Definition: Plane.h:148
mrpt::pbmap::Plane
A class used to store a planar feature (Plane for short).
Definition: Plane.h:46
mrpt::pbmap::PbMap
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:47
isSubgraphContained
bool isSubgraphContained(map< unsigned, unsigned > &contained, map< unsigned, unsigned > &container)
Definition: SubgraphMatcher.cpp:981
SubgraphMatcher.h
mrpt::pbmap::Plane::d
float d
Definition: Plane.h:143
mrpt::pbmap
Definition: ConsistencyTest.h:27
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::pbmap::Plane::v3normal
Eigen::Vector3f v3normal
Definition: Plane.h:142
RAD2DEG
#define RAD2DEG
Definition: core/include/mrpt/core/bits_math.h:62
mrpt::pbmap::Plane::id
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:127
time1
double time1
Definition: PbMapLocaliser.cpp:31
iterator
Scalar * iterator
Definition: eigen_plugins.h:26



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST