MRPT  1.9.9
checkerboard_ocamcalib_detector.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <stack> // Precompiled headers
13 
14 // Note for MRPT: what follows below is a modified part of the "OCamCalib
15 // Toolbox":
16 // See:
17 // http://robotics.ethz.ch/~scaramuzza/Davide_Scaramuzza_files/Research/OcamCalib_Tutorial.htm
18 // Modifications include:
19 // - Clean up of code and update to use STL containers, and smart pointers.
20 // - Addition of a new method to detect a number of checkerboards.
21 // - Modification of the dilation algorithm - see do_special_dilation().
22 //
23 // Original copyright note:
24 /************************************************************************************\
25  This is improved variant of chessboard corner detection algorithm that
26  uses a graph of connected quads. It is based on the code contributed
27  by Vladimir Vezhnevets and Philip Gruebele.
28  Here is the copyright notice from the original Vladimir's code:
29  ===============================================================
30 
31  The algorithms developed and implemented by Vezhnevets Vldimir
32  aka Dead Moroz (vvp@graphics.cs.msu.ru)
33  See http://graphics.cs.msu.su/en/research/calibration/opencv.html
34  for detailed information.
35 
36  Reliability additions and modifications made by Philip Gruebele.
37  <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>
38 
39  His code was adapted for use with low resolution and omnidirectional cameras
40  by Martin Rufli during his Master Thesis under supervision of Davide
41 Scaramuzza, at the ETH Zurich. Further enhancements include:
42  - Increased chance of correct corner matching.
43  - Corner matching over all dilation runs.
44 
45 If you use this code, please cite the following articles:
46 
47 1. Scaramuzza, D., Martinelli, A. and Siegwart, R. (2006), A Toolbox for Easily
48 Calibrating Omnidirectional Cameras, Proceedings of the IEEE/RSJ International
49 Conference on Intelligent Robots and Systems (IROS 2006), Beijing, China,
50 October 2006.
51 2. Scaramuzza, D., Martinelli, A. and Siegwart, R., (2006). "A Flexible
52 Technique for Accurate Omnidirectional Camera Calibration and Structure from
53 Motion", Proceedings of IEEE International Conference of Vision Systems
54 (ICVS'06), New York, January 5-7, 2006.
55 3. Rufli, M., Scaramuzza, D., and Siegwart, R. (2008), Automatic Detection of
56 Checkerboards on Blurred and Distorted Images, Proceedings of the IEEE/RSJ
57 International Conference on Intelligent Robots and Systems (IROS 2008), Nice,
58 France, September 2008.
59 
60 \************************************************************************************/
61 
62 #include "checkerboard_ocamcalib_detector.h"
63 
64 #include <array>
65 #include <map>
66 
67 #if MRPT_HAS_OPENCV
68 
69 using namespace mrpt;
70 using namespace mrpt::math;
71 using namespace mrpt::img;
72 using namespace std;
73 
74 //===========================================================================
75 // CODE STARTS HERE
76 //===========================================================================
77 // Defines
78 #define MAX_CONTOUR_APPROX 7
79 
80 // JL: Refactored code from within cvFindChessboardCorners3() and alternative
81 // algorithm:
82 bool do_special_dilation(
83  CImage& thresh_img, const int dilations, IplConvKernel* kernel_cross,
84  IplConvKernel* kernel_rect, IplConvKernel* kernel_diag1,
85  IplConvKernel* kernel_diag2, IplConvKernel* kernel_horz,
86  IplConvKernel* kernel_vert)
87 {
88  cv::Mat m = thresh_img.asCvMat<cv::Mat>(SHALLOW_COPY);
89  IplImage i = cvIplImage(m);
90  IplImage* ipl = &i;
91 
92  bool isLast = false;
93 
94  switch (dilations)
95  {
96  case 37:
97  cvDilate(ipl, ipl, kernel_cross, 1);
98  isLast = true;
99  [[fallthrough]];
100  case 36:
101  cvErode(ipl, ipl, kernel_rect, 1);
102  [[fallthrough]];
103  case 35:
104  cvDilate(ipl, ipl, kernel_vert, 1);
105  [[fallthrough]];
106  case 34:
107  cvDilate(ipl, ipl, kernel_vert, 1);
108  [[fallthrough]];
109  case 33:
110  cvDilate(ipl, ipl, kernel_vert, 1);
111  [[fallthrough]];
112  case 32:
113  cvDilate(ipl, ipl, kernel_vert, 1);
114  [[fallthrough]];
115  case 31:
116  cvDilate(ipl, ipl, kernel_vert, 1);
117  break;
118 
119  case 30:
120  cvDilate(ipl, ipl, kernel_cross, 1);
121  [[fallthrough]];
122  case 29:
123  cvErode(ipl, ipl, kernel_rect, 1);
124  [[fallthrough]];
125  case 28:
126  cvDilate(ipl, ipl, kernel_horz, 1);
127  [[fallthrough]];
128  case 27:
129  cvDilate(ipl, ipl, kernel_horz, 1);
130  [[fallthrough]];
131  case 26:
132  cvDilate(ipl, ipl, kernel_horz, 1);
133  [[fallthrough]];
134  case 25:
135  cvDilate(ipl, ipl, kernel_horz, 1);
136  [[fallthrough]];
137  case 24:
138  cvDilate(ipl, ipl, kernel_horz, 1);
139  break;
140 
141  case 23:
142  cvDilate(ipl, ipl, kernel_diag2, 1);
143  [[fallthrough]];
144  case 22:
145  cvDilate(ipl, ipl, kernel_diag1, 1);
146  [[fallthrough]];
147  case 21:
148  cvDilate(ipl, ipl, kernel_diag2, 1);
149  [[fallthrough]];
150  case 20:
151  cvDilate(ipl, ipl, kernel_diag1, 1);
152  [[fallthrough]];
153  case 19:
154  cvDilate(ipl, ipl, kernel_diag2, 1);
155  [[fallthrough]];
156  case 18:
157  cvDilate(ipl, ipl, kernel_diag1, 1);
158  break;
159 
160  case 17:
161  cvDilate(ipl, ipl, kernel_diag2, 1);
162  [[fallthrough]];
163  case 16:
164  cvDilate(ipl, ipl, kernel_diag2, 1);
165  [[fallthrough]];
166  case 15:
167  cvDilate(ipl, ipl, kernel_diag2, 1);
168  [[fallthrough]];
169  case 14:
170  cvDilate(ipl, ipl, kernel_diag2, 1);
171  break;
172 
173  case 13:
174  cvDilate(ipl, ipl, kernel_diag1, 1);
175  [[fallthrough]];
176  case 12:
177  cvDilate(ipl, ipl, kernel_diag1, 1);
178  [[fallthrough]];
179  case 11:
180  cvDilate(ipl, ipl, kernel_diag1, 1);
181  [[fallthrough]];
182  case 10:
183  cvDilate(ipl, ipl, kernel_diag1, 1);
184  break;
185 
186  case 9:
187  cvDilate(ipl, ipl, kernel_cross, 1);
188  [[fallthrough]];
189  case 8:
190  cvErode(ipl, ipl, kernel_rect, 1);
191  [[fallthrough]];
192  case 7:
193  cvDilate(ipl, ipl, kernel_cross, 1);
194  [[fallthrough]];
195  case 6:
196  cvDilate(ipl, ipl, kernel_diag2, 1);
197  isLast = true;
198  [[fallthrough]];
199  case 5:
200  cvDilate(ipl, ipl, kernel_diag1, 1);
201  [[fallthrough]];
202  case 4:
203  cvDilate(ipl, ipl, kernel_rect, 1);
204  [[fallthrough]];
205  case 3:
206  cvErode(ipl, ipl, kernel_cross, 1);
207  [[fallthrough]];
208  case 2:
209  cvDilate(ipl, ipl, kernel_rect, 1);
210  [[fallthrough]];
211  case 1:
212  cvDilate(ipl, ipl, kernel_cross, 1);
213  [[fallthrough]];
214  case 0: /* first try: do nothing to the image */
215  break;
216  };
217 
218  return isLast;
219 }
220 
221 //===========================================================================
222 // MAIN FUNCTION
223 //===========================================================================
224 // Return: -1: errors, 0: not found, 1: found OK
225 int cvFindChessboardCorners3(
226  const CImage& img_, CvSize pattern_size,
227  std::vector<CvPoint2D32f>& out_corners)
228 {
229  // PART 0: INITIALIZATION
230  //-----------------------------------------------------------------------
231  // Initialize variables
232  int flags = 1; // not part of the function call anymore!
233  size_t max_count = 0;
234  int max_dilation_run_ID = -1;
235  // const int min_dilations = 0; // JL: was: 1
236  // const int max_dilations = 23; // JL: see do_special_dilation()
237  int found = 0;
238 
239  vector<CvCBQuad::Ptr> quads; // CvCBQuad **quads = 0;
240  vector<CvCBQuad::Ptr> quad_group; // CvCBQuad **quad_group = 0;
241  vector<CvCBCorner::Ptr> corners; // CvCBCorner *corners = 0;
242  vector<CvCBQuad::Ptr>
243  output_quad_group; // CvCBQuad **output_quad_group = 0;
244 
245  // debug trial. Martin Rufli, 28. Ocober, 2008
246  int block_size = 0;
247 
248  // Further initializations
249  int quad_count, group_idx;
250 
251  if (pattern_size.width < 2 || pattern_size.height < 2)
252  {
253  std::cerr << "Pattern should have at least 2x2 size" << endl;
254  return -1;
255  }
256  if (pattern_size.width > 127 || pattern_size.height > 127)
257  {
258  std::cerr << "Pattern should not have a size larger than 127 x 127"
259  << endl;
260  return -1;
261  }
262 
263  // Assure it's a grayscale image:
264  const CImage img(img_, FAST_REF_OR_CONVERT_TO_GRAY);
265 
266  CImage thresh_img(
267  img.getWidth(), img.getHeight(),
268  CH_GRAY); // = cvCreateMat( img->rows, img->cols, CV_8UC1 );
269  CImage thresh_img_save(
270  img.getWidth(), img.getHeight(),
271  CH_GRAY); // = cvCreateMat( img->rows, img->cols, CV_8UC1 );
272 
273  // JL: Move these constructors out of the loops:
274  IplConvKernel* kernel_cross =
275  cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_CROSS, nullptr);
276  IplConvKernel* kernel_rect =
277  cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT, nullptr);
278 
279  static int kernel_diag1_vals[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
280  IplConvKernel* kernel_diag1 = cvCreateStructuringElementEx(
281  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_diag1_vals);
282  static int kernel_diag2_vals[9] = {0, 0, 1, 0, 1, 0, 1, 0, 0};
283  IplConvKernel* kernel_diag2 = cvCreateStructuringElementEx(
284  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_diag2_vals);
285  static int kernel_horz_vals[9] = {0, 0, 0, 1, 1, 1, 0, 0, 0};
286  IplConvKernel* kernel_horz = cvCreateStructuringElementEx(
287  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_horz_vals);
288  static int kernel_vert_vals[9] = {0, 1, 0, 0, 1, 0, 0, 1, 0};
289  IplConvKernel* kernel_vert = cvCreateStructuringElementEx(
290  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_vert_vals);
291 
292  // For image binarization (thresholding)
293  // we use an adaptive threshold with a gaussian mask
294  // ATTENTION: Gaussian thresholding takes MUCH more time than Mean
295  // thresholding!
296  block_size = cvRound(MIN(img.getWidth(), img.getHeight()) * 0.2) | 1;
297 
298  cv::adaptiveThreshold(
299  img.asCvMat<cv::Mat>(SHALLOW_COPY),
300  thresh_img.asCvMat<cv::Mat>(SHALLOW_COPY), 255,
301  CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, block_size, 0);
302 
303  thresh_img_save = thresh_img.makeDeepCopy();
304 
305  // PART 1: FIND LARGEST PATTERN
306  //-----------------------------------------------------------------------
307  // Checker patterns are tried to be found by dilating the background and
308  // then applying a canny edge finder on the closed contours (checkers).
309  // Try one dilation run, but if the pattern is not found, repeat until
310  // max_dilations is reached.
311  // for( int dilations = min_dilations; dilations <= max_dilations;
312  // dilations++ )
313 
314  bool last_dilation = false;
315 
316  for (int dilations = 0; !last_dilation; dilations++)
317  {
318  // Calling "cvCopy" again is much faster than rerunning
319  // "cvAdaptiveThreshold"
320  thresh_img = thresh_img_save.makeDeepCopy();
321 
322  // Dilate squares:
323  last_dilation = do_special_dilation(
324  thresh_img, dilations, kernel_cross, kernel_rect, kernel_diag1,
325  kernel_diag2, kernel_horz, kernel_vert);
326 
327  // In order to find rectangles that go to the edge, we draw a white
328  // line around the image edge. Otherwise FindContours will miss those
329  // clipped rectangle contours. The border color will be the image mean,
330  // because otherwise we risk screwing up filters like cvSmooth()
331  cv::rectangle(
332  thresh_img.asCvMatRef(), cv::Point(0, 0),
333  cv::Point(thresh_img.getWidth() - 1, thresh_img.getHeight() - 1),
334  CV_RGB(255, 255, 255), 3, 8);
335 
336  // Generate quadrangles in the following function
337  // "quad_count" is the number of cound quadrangles
338  quad_count = icvGenerateQuads(
339  quads, corners, thresh_img, flags, dilations, true);
340  if (quad_count <= 0) continue;
341 
342  // The following function finds and assigns neighbor quads to every
343  // quadrangle in the immediate vicinity fulfilling certain
344  // prerequisites
345  mrFindQuadNeighbors2(quads, dilations);
346 
347  // The connected quads will be organized in groups. The following loop
348  // increases a "group_idx" identifier.
349  // The function "icvFindConnectedQuads assigns all connected quads
350  // a unique group ID.
351  // If more quadrangles were assigned to a given group (i.e. connected)
352  // than are expected by the input variable "pattern_size", the
353  // function "icvCleanFoundConnectedQuads" erases the surplus
354  // quadrangles by minimizing the convex hull of the remaining pattern.
355  for (group_idx = 0;; group_idx++)
356  {
357  icvFindConnectedQuads(quads, quad_group, group_idx, dilations);
358 
359  if (quad_group.empty()) break;
360 
361  icvCleanFoundConnectedQuads(quad_group, pattern_size);
362  size_t count = quad_group.size();
363 
364  // MARTIN's Code
365  // To save computational time, only proceed, if the number of
366  // found quads during this dilation run is larger than the
367  // largest previous found number
368  if (count /*>=*/ > max_count)
369  {
370  // cout << "CHECKERBOARD: Best found at dilation=" << dilations
371  // << endl;
372 
373  // set max_count to its new value
374  max_count = count;
375  max_dilation_run_ID = dilations;
376 
377  // The following function labels all corners of every quad
378  // with a row and column entry.
379  // "count" specifies the number of found quads in "quad_group"
380  // with group identifier "group_idx"
381  // The last parameter is set to "true", because this is the
382  // first function call and some initializations need to be
383  // made.
384  mrLabelQuadGroup(quad_group, pattern_size, true);
385 
386  // END------------------------------------------------------------------------
387 
388  // Allocate memory
389  // output_quad_group.resize( (pattern_size.height+2) *
390  // (pattern_size.width+2) ); // = (CvCBQuad**)cvAlloc(
391  // sizeof(output_quad_group[0]) * ((pattern_size.height+2) *
392  // (pattern_size.width+2)) );
393  // The following function copies every member of "quad_group"
394  // to "output_quad_group", because "quad_group" will be
395  // overwritten during the next loop pass.
396  // "output_quad_group" is a true copy of "quad_group" and
397  // later used for output
398  output_quad_group = quad_group; // mrCopyQuadGroup( quad_group,
399  // output_quad_group, max_count
400  // );
401  }
402  }
403  }
404 
405  // If enough corners have been found already, then there is no need for PART
406  // 2 ->EXIT
407  // JLBC for MRPT: Don't save to Matlab files (mrWriteCorners), but to
408  // "CvPoint2D32f *out_corners":
409  // Return true on success in finding all the quads.
410  found = myQuads2Points(output_quad_group, pattern_size, out_corners);
411 
412  if (found != -1 && found != 1)
413  {
414  // PART 2: AUGMENT LARGEST PATTERN
415  //-----------------------------------------------------------------------
416  // Instead of saving all found quads of all dilation runs from PART 1,
417  // we
418  // just recompute them again, but skipping the dilation run which
419  // produced the maximum number of found quadrangles.
420  // In essence the first section of PART 2 is identical to the first
421  // section of PART 1.
422  // for( int dilations = max_dilations; dilations >= min_dilations;
423  // dilations-- )
424  last_dilation = false;
425  for (int dilations = 0; !last_dilation; dilations++)
426  {
427  // if(max_dilation_run_ID == dilations)
428  // continue;
429 
430  // Calling "cvCopy" again is much faster than rerunning
431  // "cvAdaptiveThreshold"
432  thresh_img = thresh_img_save.makeDeepCopy();
433 
434  // Dilate squares:
435  last_dilation = do_special_dilation(
436  thresh_img, dilations, kernel_cross, kernel_rect, kernel_diag1,
437  kernel_diag2, kernel_horz, kernel_vert);
438 
439  cv::rectangle(
440  thresh_img.asCvMatRef(), cv::Point(0, 0),
441  cv::Point(
442  thresh_img.getWidth() - 1, thresh_img.getHeight() - 1),
443  CV_RGB(255, 255, 255), 3, 8);
444 
445  quad_count = icvGenerateQuads(
446  quads, corners, thresh_img, flags, dilations, false);
447  if (quad_count <= 0) continue;
448 
449  // END------------------------------------------------------------------------
450 
451  // MARTIN's Code
452  // The following loop is executed until no more newly found quads
453  // can be matched to one of the border corners of the largest found
454  // pattern from PART 1.
455  // The function "mrAugmentBestRun" tests whether a quad can be
456  // linked
457  // to the existng pattern.
458  // The function "mrLabelQuadGroup" then labels the newly added
459  // corners
460  // with the respective row and column entries.
461  int feedBack = -1;
462  while (feedBack == -1)
463  {
464  feedBack = mrAugmentBestRun(
465  quads, dilations, output_quad_group, max_dilation_run_ID);
466 
467  // if we have found a new matching quad
468  if (feedBack == -1)
469  {
470  // increase max_count by one
471  max_count = max_count + 1;
472  mrLabelQuadGroup(output_quad_group, pattern_size, false);
473 
474  // write the found corners to output array
475  // Go to //__END__, if enough corners have been found
476  found = myQuads2Points(
477  output_quad_group, pattern_size, out_corners);
478  // found = mrWriteCorners( output_quad_group, max_count,
479  // pattern_size, min_number_of_corners);
480  if (found == -1 || found == 1)
481  {
482  // JL: was a "goto exit;", but, have you seen
483  // http://xkcd.com/292/ ??? ;-)
484  last_dilation =
485  true; // This will break the outer for loop
486  break;
487  }
488  }
489  }
490 
491  } // end for "dilations"
492 
493  } // JL: Was label "exit:", but again, http://xkcd.com/292/ ;-)
494 
495  // Free mem:
496  cvReleaseStructuringElement(&kernel_cross);
497  cvReleaseStructuringElement(&kernel_rect);
498  cvReleaseStructuringElement(&kernel_diag1);
499  cvReleaseStructuringElement(&kernel_diag2);
500  cvReleaseStructuringElement(&kernel_horz);
501  cvReleaseStructuringElement(&kernel_vert);
502 
503  /*
504  // MARTIN:
505  found = mrWriteCorners( output_quad_group, max_count, pattern_size,
506  min_number_of_corners);
507  */
508 
509  // If a linking problem was encountered, throw an error message
510  if (found == -1)
511  {
512  std::cerr << "While linking the corners a problem was encountered. No "
513  "corner sequence is returned. "
514  << endl;
515  return -1;
516  }
517 
518  // Return found
519  // Found can have the values
520  // -1 -> Error or corner linking problem, see std::cerr
521  // 0 -> Not enough corners were found
522  // 1 -> Enough corners were found
523  return found;
524 }
525 
526 double triangleArea(
527  double x0, double y0, double x1, double y1, double x2, double y2)
528 {
529  return std::abs(0.5 * (x0 * (y1 - y2) + x1 * (y2 - y0) + x2 * (y0 - y1)));
530 }
531 
532 double median(const std::vector<double>& vec)
533 {
534  std::vector<double> v = vec; // Copy for sorting
535  const size_t n = v.size() / 2;
536  nth_element(v.begin(), v.begin() + n, v.end());
537  return v[n];
538 }
539 
540 //===========================================================================
541 // ERASE OVERHEAD
542 //===========================================================================
543 // If we found too many connected quads, remove those which probably do not
544 // belong.
545 void icvCleanFoundConnectedQuads(
546  std::vector<CvCBQuad::Ptr>& quad_group, const CvSize& pattern_size)
547 {
548  cv::MemStorage temp_storage; // JL: "Modernized" to use C++ STL stuff.
549 
550  CvPoint2D32f center = cvPoint2D32f(0, 0);
551 
552  // Number of quads this pattern should contain
553  const size_t expected_quads_count =
554  ((pattern_size.width + 1) * (pattern_size.height + 1) + 1) / 2;
555 
556  // If we have more quadrangles than we should, try to eliminate duplicates
557  // or ones which don't belong to the pattern rectangle. Else go to the end
558  // of the function
559  const size_t nQuads = quad_group.size();
560  if (nQuads <= expected_quads_count) return; // Nothing to be done.
561 
562  // Create an array of quadrangle centers
563  vector<CvPoint2D32f> centers(nQuads);
564  temp_storage = cv::MemStorage(cvCreateMemStorage(0));
565 
566  // make also the list of squares areas, so we can discriminate by
567  // too-large/small quads:
568  // (Added by JLBC, JUN-2014)
569  std::vector<double> quad_areas(nQuads);
570  double min_area = DBL_MAX, max_area = -DBL_MAX, mean_area = 0.0;
571 
572  for (size_t i = 0; i < nQuads; i++)
573  {
574  CvPoint2D32f ci = cvPoint2D32f(0, 0);
575  CvCBQuad::Ptr& q = quad_group[i];
576 
577  for (size_t j = 0; j < 4; j++)
578  {
579  CvPoint2D32f pt = q->corners[j]->pt;
580  ci.x += pt.x;
581  ci.y += pt.y;
582  }
583 
584  ci.x *= 0.25f;
585  ci.y *= 0.25f;
586 
587  // Quad area:
588  const double a =
589  triangleArea(
590  q->corners[0]->pt.x, q->corners[0]->pt.y, q->corners[1]->pt.x,
591  q->corners[1]->pt.y, q->corners[2]->pt.x, q->corners[2]->pt.y) +
592  triangleArea(
593  q->corners[0]->pt.x, q->corners[0]->pt.y, q->corners[2]->pt.x,
594  q->corners[2]->pt.y, q->corners[3]->pt.x, q->corners[3]->pt.y);
595 
596  q->area = a;
597  quad_areas[i] = a;
598  mean_area += a;
599  if (a < min_area) min_area = a;
600  if (a > max_area) max_area = a;
601 
602  // Centers(i), is the geometric center of quad(i)
603  // Center, is the center of all found quads
604  centers[i] = ci;
605  center.x += ci.x;
606  center.y += ci.y;
607  }
608  center.x /= nQuads;
609  center.y /= nQuads;
610  mean_area /= nQuads;
611  const double median_area = median(quad_areas);
612 
613  // ration: area/median:
614  for (size_t i = 0; i < nQuads; i++)
615  {
616  quad_group[i]->area_ratio = quad_group[i]->area / median_area;
617  }
618 
619  // If we have more quadrangles than we should, we try to eliminate bad
620  // ones based on minimizing the bounding box. We iteratively remove the
621  // point which reduces the size of the bounding box of the blobs the most
622  // (since we want the rectangle to be as small as possible) remove the
623  // quadrange that causes the biggest reduction in pattern size until we
624  // have the correct number
625 
626  // JLBC (2014): Additional preliminary filter: remove quads that are too
627  // small or too large
628 
629  // In the following, use "quad_group.size()" since the size will change with
630  // iterations
631  while (quad_group.size() > expected_quads_count)
632  {
633  double min_box_area = DBL_MAX;
634  int min_box_area_index = -1;
635 
636  // For each point, check area:
637  int most_outlier_idx = -1;
638  double most_outlier_ratio = 1.0;
639  for (size_t skip = 0; skip < quad_group.size(); skip++)
640  {
641  double ar = quad_group[skip]->area_ratio;
642  if (ar > 1.0) ar = 1.0 / ar;
643 
644  if (ar < most_outlier_ratio)
645  {
646  most_outlier_ratio = ar;
647  most_outlier_idx = skip;
648  }
649  }
650 
651  if (most_outlier_idx >= 0)
652  {
653  min_box_area_index = most_outlier_idx;
654  }
655 
656  if (min_box_area_index == -1) // if the previous filter didn't work:
657  {
658  // For each point, calculate box area without that point
659  for (size_t skip = 0; skip < quad_group.size(); skip++)
660  {
661  // get bounding rectangle
662  CvPoint2D32f temp = centers[skip];
663  centers[skip] = center;
664  CvMat pointMat =
665  cvMat(1, quad_group.size(), CV_32FC2, &centers[0]);
666  CvSeq* hull =
667  cvConvexHull2(&pointMat, temp_storage, CV_CLOCKWISE, 1);
668  centers[skip] = temp;
669  double hull_area = fabs(cvContourArea(hull, CV_WHOLE_SEQ));
670 
671  // remember smallest box area
672  if (hull_area < min_box_area)
673  {
674  min_box_area = hull_area;
675  min_box_area_index = skip;
676  }
677  cvClearMemStorage(temp_storage);
678  }
679  }
680 
681  CvCBQuad::Ptr& q0 = quad_group[min_box_area_index];
682 
683  // remove any references to this quad as a neighbor
684  for (size_t i = 0; i < quad_group.size(); i++)
685  {
686  CvCBQuad::Ptr& q = quad_group[i];
687 
688  for (size_t j = 0; j < 4; j++)
689  {
690  if (q->neighbors[j] == q0)
691  {
692  q->neighbors[j].reset(); // = 0;
693  q->count--;
694  for (size_t k = 0; k < 4; k++)
695  if (q0->neighbors[k] == q)
696  {
697  q0->neighbors[k].reset(); // = 0;
698  q0->count--;
699  break;
700  }
701  break;
702  }
703  }
704  }
705 
706  // remove the quad by copying th last quad in the list into its place
707  quad_group.erase(quad_group.begin() + min_box_area_index);
708  centers.erase(centers.begin() + min_box_area_index);
709  }
710 }
711 
712 //===========================================================================
713 // FIND COONECTED QUADS
714 //===========================================================================
715 void icvFindConnectedQuads(
716  std::vector<CvCBQuad::Ptr>& quad, std::vector<CvCBQuad::Ptr>& out_group,
717  const int group_idx, const int dilation)
718 {
719  MRPT_UNUSED_PARAM(dilation);
720  // initializations
721  out_group.clear();
722 
723  const size_t quad_count = quad.size();
724 
725  // Scan the array for a first unlabeled quad
726  for (size_t i = 0; i < quad_count; i++)
727  {
728  if (quad[i]->count < 0 || quad[i]->group_idx >= 0) continue;
729 
730  // Recursively find a group of connected quads starting from the seed
731  // quad[i]
732  CvCBQuad::Ptr& q = quad[i];
733 
734  std::stack<CvCBQuad::Ptr> seqStack;
735 
736  seqStack.push(q); // cvSeqPush( stack, &q );
737 
738  q->group_idx = group_idx;
739  out_group.push_back(q); // out_group[count++] = q;
740 
741  while (!seqStack.empty())
742  {
743  q = seqStack.top();
744  seqStack.pop(); // cvSeqPop( stack, &q );
745 
746  for (size_t k = 0; k < 4; k++)
747  {
748  CvCBQuad::Ptr& neighbor = q->neighbors[k];
749 
750  // If he neighbor exists and the neighbor has more than 0
751  // neighbors and the neighbor has not been classified yet.
752  if (neighbor && neighbor->count > 0 && neighbor->group_idx < 0)
753  {
754  neighbor->group_idx = group_idx;
755  seqStack.push(neighbor); // cvSeqPush( stack, &neighbor );
756  out_group.push_back(
757  neighbor); // out_group[count++] = neighbor;
758  }
759  }
760  }
761 
762  break;
763  }
764 }
765 
766 //===========================================================================
767 // LABEL CORNER WITH ROW AND COLUMN //DONE
768 //===========================================================================
769 void mrLabelQuadGroup(
770  std::vector<CvCBQuad::Ptr>& quad_group, const CvSize& pattern_size,
771  bool firstRun)
772 {
773  const size_t count = quad_group.size();
774 
775  // If this is the first function call, a seed quad needs to be selected
776  if (firstRun == true)
777  {
778  // Search for the (first) quad with the maximum number of neighbors
779  // (usually 4). This will be our starting point.
780  int max_id = -1;
781  int max_number = -1;
782  for (size_t i = 0; i < count; i++)
783  {
784  CvCBQuad* q = quad_group[i].get();
785  if (q->count > max_number)
786  {
787  max_number = q->count;
788  max_id = i;
789 
790  if (max_number == 4) break;
791  }
792  }
793 
794  // Mark the starting quad's (per definition) upper left corner with
795  //(0,0) and then proceed clockwise
796  // The following labeling sequence enshures a "right coordinate system"
797  CvCBQuad* q = quad_group[max_id].get();
798 
799  q->labeled = true;
800  q->corners[0]->row = 0;
801  q->corners[0]->column = 0;
802  q->corners[1]->row = 0;
803  q->corners[1]->column = 1;
804  q->corners[2]->row = 1;
805  q->corners[2]->column = 1;
806  q->corners[3]->row = 1;
807  q->corners[3]->column = 0;
808  }
809 
810  // Mark all other corners with their respective row and column
811  bool flag_changed = true;
812  while (flag_changed == true)
813  {
814  // First reset the flag to "false"
815  flag_changed = false;
816 
817  // Go through all quads top down is faster, since unlabeled quads will
818  // be inserted at the end of the list
819  for (int i = int(count - 1); i >= 0; i--)
820  {
821  // Check whether quad "i" has been labeled already
822  if ((quad_group[i])->labeled == false)
823  {
824  // Check its neighbors, whether some of them have been labeled
825  // already
826  for (size_t j = 0; j < 4; j++)
827  {
828  // Check whether the neighbor exists (i.e. is not the NULL
829  // pointer)
830  if ((quad_group[i])->neighbors[j])
831  {
832  CvCBQuad::Ptr& quadNeighborJ =
833  quad_group[i]->neighbors[j];
834 
835  // Only proceed, if neighbor "j" was labeled
836  if (quadNeighborJ->labeled == true)
837  {
838  // For every quad it could happen to pass here
839  // multiple times. We therefore "break" later.
840  // Check whitch of the neighbors corners is
841  // connected to the current quad
842  int connectedNeighborCornerId = -1;
843  for (int k = 0; k < 4; k++)
844  {
845  if (quadNeighborJ->neighbors[k] ==
846  quad_group[i])
847  {
848  connectedNeighborCornerId = k;
849 
850  // there is only one, therefore
851  break;
852  }
853  }
854 
855  // For the following calculations we need the row
856  // and column of the connected neighbor corner and
857  // all other corners of the connected quad "j",
858  // clockwise (CW)
859  CvCBCorner::Ptr& conCorner =
860  quadNeighborJ
861  ->corners[connectedNeighborCornerId];
862  CvCBCorner::Ptr& conCornerCW1 =
863  quadNeighborJ->corners
864  [(connectedNeighborCornerId + 1) % 4];
865  CvCBCorner::Ptr& conCornerCW2 =
866  quadNeighborJ->corners
867  [(connectedNeighborCornerId + 2) % 4];
868  CvCBCorner::Ptr& conCornerCW3 =
869  quadNeighborJ->corners
870  [(connectedNeighborCornerId + 3) % 4];
871 
872  (quad_group[i])->corners[j]->row = conCorner->row;
873  (quad_group[i])->corners[j]->column =
874  conCorner->column;
875  (quad_group[i])->corners[(j + 1) % 4]->row =
876  conCorner->row - conCornerCW2->row +
877  conCornerCW3->row;
878  (quad_group[i])->corners[(j + 1) % 4]->column =
879  conCorner->column - conCornerCW2->column +
880  conCornerCW3->column;
881  (quad_group[i])->corners[(j + 2) % 4]->row =
882  conCorner->row + conCorner->row -
883  conCornerCW2->row;
884  (quad_group[i])->corners[(j + 2) % 4]->column =
885  conCorner->column + conCorner->column -
886  conCornerCW2->column;
887  (quad_group[i])->corners[(j + 3) % 4]->row =
888  conCorner->row - conCornerCW2->row +
889  conCornerCW1->row;
890  (quad_group[i])->corners[(j + 3) % 4]->column =
891  conCorner->column - conCornerCW2->column +
892  conCornerCW1->column;
893 
894  // Mark this quad as labeled
895  (quad_group[i])->labeled = true;
896 
897  // Changes have taken place, set the flag
898  flag_changed = true;
899 
900  // once is enough!
901  break;
902  }
903  }
904  }
905  }
906  }
907  }
908 
909  // All corners are marked with row and column
910  // Record the minimal and maximal row and column indices
911  // It is unlikely that more than 8bit checkers are used per dimension, if
912  // there are
913  // an error would have been thrown at the beginning of
914  // "cvFindChessboardCorners2"
915  int min_row = 127;
916  int max_row = -127;
917  int min_column = 127;
918  int max_column = -127;
919 
920  for (size_t i = 0; i < count; i++)
921  {
922  const CvCBQuad::Ptr& q = quad_group[i];
923 
924  for (size_t j = 0; j < 4; j++)
925  {
926  if ((q->corners[j])->row > max_row) max_row = (q->corners[j])->row;
927 
928  if ((q->corners[j])->row < min_row) min_row = (q->corners[j])->row;
929 
930  if ((q->corners[j])->column > max_column)
931  max_column = (q->corners[j])->column;
932 
933  if ((q->corners[j])->column < min_column)
934  min_column = (q->corners[j])->column;
935  }
936  }
937 
938  // Label all internal corners with "needsNeighbor" = false
939  // Label all external corners with "needsNeighbor" = true,
940  // except if in a given dimension the pattern size is reached
941  for (int i = min_row; i <= max_row; i++)
942  {
943  for (int j = min_column; j <= max_column; j++)
944  {
945  // A flag that indicates, wheter a row/column combination is
946  // executed multiple times
947  bool flagg = false;
948 
949  // Remember corner and quad
950  int cornerID = 0;
951  int quadID = 0;
952 
953  for (size_t k = 0; k < count; k++)
954  {
955  for (size_t l = 0; l < 4; l++)
956  {
957  if (((quad_group[k])->corners[l]->row == i) &&
958  ((quad_group[k])->corners[l]->column == j))
959  {
960  if (flagg == true)
961  {
962  // Passed at least twice through here
963  (quad_group[k])->corners[l]->needsNeighbor = false;
964  (quad_group[quadID])
965  ->corners[cornerID]
966  ->needsNeighbor = false;
967  }
968  else
969  {
970  // Mark with needs a neighbor, but note the
971  // address
972  (quad_group[k])->corners[l]->needsNeighbor = true;
973  cornerID = l;
974  quadID = k;
975  }
976 
977  // set the flag to true
978  flagg = true;
979  }
980  }
981  }
982  }
983  }
984 
985  // Complete Linking:
986  // sometimes not all corners were properly linked in "mrFindQuadNeighbors2",
987  // but after labeling each corner with its respective row and column, it is
988  // possible to match them anyway.
989  for (int i = min_row; i <= max_row; i++)
990  {
991  for (int j = min_column; j <= max_column; j++)
992  {
993  // the following "number" indicates the number of corners which
994  // correspond to the given (i,j) value
995  // 1 is a border corner or a conrer which still needs a neighbor
996  // 2 is a fully connected internal corner
997  // >2 something went wrong during labeling, report a warning
998  int number = 1;
999 
1000  // remember corner and quad
1001  int cornerID = 0;
1002  int quadID = 0;
1003 
1004  for (size_t k = 0; k < count; k++)
1005  {
1006  for (size_t l = 0; l < 4; l++)
1007  {
1008  if (((quad_group[k])->corners[l]->row == i) &&
1009  ((quad_group[k])->corners[l]->column == j))
1010  {
1011  if (number == 1)
1012  {
1013  // First corner, note its ID
1014  cornerID = l;
1015  quadID = k;
1016  }
1017 
1018  else if (number == 2)
1019  {
1020  // Second corner, check wheter this and the
1021  // first one have equal coordinates, else
1022  // interpolate
1023  float delta_x =
1024  (quad_group[k])->corners[l]->pt.x -
1025  (quad_group[quadID])->corners[cornerID]->pt.x;
1026  float delta_y =
1027  (quad_group[k])->corners[l]->pt.y -
1028  (quad_group[quadID])->corners[cornerID]->pt.y;
1029 
1030  if (delta_x != 0 || delta_y != 0)
1031  {
1032  // Interpolate
1033  (quad_group[k])->corners[l]->pt.x =
1034  (quad_group[k])->corners[l]->pt.x -
1035  delta_x / 2;
1036  (quad_group[quadID])->corners[cornerID]->pt.x =
1037  (quad_group[quadID])
1038  ->corners[cornerID]
1039  ->pt.x +
1040  delta_x / 2;
1041  (quad_group[k])->corners[l]->pt.y =
1042  (quad_group[k])->corners[l]->pt.y -
1043  delta_y / 2;
1044  (quad_group[quadID])->corners[cornerID]->pt.y =
1045  (quad_group[quadID])
1046  ->corners[cornerID]
1047  ->pt.y +
1048  delta_y / 2;
1049  }
1050  }
1051  else if (number > 2)
1052  {
1053  // Something went wrong during row/column labeling
1054  // Report a Warning
1055  // ->Implemented in the function "mrWriteCorners"
1056  }
1057 
1058  // increase the number by one
1059  number = number + 1;
1060  }
1061  }
1062  }
1063  }
1064  }
1065 
1066  // Bordercorners don't need any neighbors, if the pattern size in the
1067  // respective direction is reached
1068  // The only time we can make shure that the target pattern size is reached
1069  // in a given
1070  // dimension, is when the larger side has reached the target size in the
1071  // maximal
1072  // direction, or if the larger side is larger than the smaller target size
1073  // and the
1074  // smaller side equals the smaller target size
1075  int largerDimPattern = max(pattern_size.height, pattern_size.width);
1076  int smallerDimPattern = min(pattern_size.height, pattern_size.width);
1077  bool flagSmallerDim1 = false;
1078  bool flagSmallerDim2 = false;
1079 
1080  if ((largerDimPattern + 1) == max_column - min_column)
1081  {
1082  flagSmallerDim1 = true;
1083  // We found out that in the column direction the target pattern size is
1084  // reached
1085  // Therefore border column corners do not need a neighbor anymore
1086  // Go through all corners
1087  for (size_t k = 0; k < count; k++)
1088  {
1089  for (size_t l = 0; l < 4; l++)
1090  {
1091  if ((quad_group[k])->corners[l]->column == min_column ||
1092  (quad_group[k])->corners[l]->column == max_column)
1093  {
1094  // Needs no neighbor anymore
1095  (quad_group[k])->corners[l]->needsNeighbor = false;
1096  }
1097  }
1098  }
1099  }
1100 
1101  if ((largerDimPattern + 1) == max_row - min_row)
1102  {
1103  flagSmallerDim2 = true;
1104  // We found out that in the column direction the target pattern size is
1105  // reached
1106  // Therefore border column corners do not need a neighbor anymore
1107  // Go through all corners
1108  for (size_t k = 0; k < count; k++)
1109  {
1110  for (size_t l = 0; l < 4; l++)
1111  {
1112  if ((quad_group[k])->corners[l]->row == min_row ||
1113  (quad_group[k])->corners[l]->row == max_row)
1114  {
1115  // Needs no neighbor anymore
1116  (quad_group[k])->corners[l]->needsNeighbor = false;
1117  }
1118  }
1119  }
1120  }
1121 
1122  // Check the two flags:
1123  // - If one is true and the other false, then the pattern target
1124  // size was reached in in one direction -> We can check, whether the
1125  // target
1126  // pattern size is also reached in the other direction
1127  // - If both are set to true, then we deal with a square board -> do
1128  // nothing
1129  // - If both are set to false -> There is a possibility that the larger
1130  // side is
1131  // larger than the smaller target size -> Check and if true, then check
1132  // whether
1133  // the other side has the same size as the smaller target size
1134  if ((flagSmallerDim1 == false && flagSmallerDim2 == true))
1135  {
1136  // Larger target pattern size is in row direction, check wheter smaller
1137  // target
1138  // pattern size is reached in column direction
1139  if ((smallerDimPattern + 1) == max_column - min_column)
1140  {
1141  for (size_t k = 0; k < count; k++)
1142  {
1143  for (int l = 0; l < 4; l++)
1144  {
1145  if ((quad_group[k])->corners[l]->column == min_column ||
1146  (quad_group[k])->corners[l]->column == max_column)
1147  {
1148  // Needs no neighbor anymore
1149  (quad_group[k])->corners[l]->needsNeighbor = false;
1150  }
1151  }
1152  }
1153  }
1154  }
1155 
1156  if ((flagSmallerDim1 == true && flagSmallerDim2 == false))
1157  {
1158  // Larger target pattern size is in column direction, check wheter
1159  // smaller target
1160  // pattern size is reached in row direction
1161  if ((smallerDimPattern + 1) == max_row - min_row)
1162  {
1163  for (size_t k = 0; k < count; k++)
1164  {
1165  for (size_t l = 0; l < 4; l++)
1166  {
1167  if ((quad_group[k])->corners[l]->row == min_row ||
1168  (quad_group[k])->corners[l]->row == max_row)
1169  {
1170  // Needs no neighbor anymore
1171  (quad_group[k])->corners[l]->needsNeighbor = false;
1172  }
1173  }
1174  }
1175  }
1176  }
1177 
1178  if ((flagSmallerDim1 == false && flagSmallerDim2 == false) &&
1179  smallerDimPattern + 1 < max_column - min_column)
1180  {
1181  // Larger target pattern size is in column direction, check wheter
1182  // smaller target
1183  // pattern size is reached in row direction
1184  if ((smallerDimPattern + 1) == max_row - min_row)
1185  {
1186  for (size_t k = 0; k < count; k++)
1187  {
1188  for (size_t l = 0; l < 4; l++)
1189  {
1190  if ((quad_group[k])->corners[l]->row == min_row ||
1191  (quad_group[k])->corners[l]->row == max_row)
1192  {
1193  // Needs no neighbor anymore
1194  (quad_group[k])->corners[l]->needsNeighbor = false;
1195  }
1196  }
1197  }
1198  }
1199  }
1200 
1201  if ((flagSmallerDim1 == false && flagSmallerDim2 == false) &&
1202  smallerDimPattern + 1 < max_row - min_row)
1203  {
1204  // Larger target pattern size is in row direction, check wheter smaller
1205  // target
1206  // pattern size is reached in column direction
1207  if ((smallerDimPattern + 1) == max_column - min_column)
1208  {
1209  for (size_t k = 0; k < count; k++)
1210  {
1211  for (size_t l = 0; l < 4; l++)
1212  {
1213  if ((quad_group[k])->corners[l]->column == min_column ||
1214  (quad_group[k])->corners[l]->column == max_column)
1215  {
1216  // Needs no neighbor anymore
1217  (quad_group[k])->corners[l]->needsNeighbor = false;
1218  }
1219  }
1220  }
1221  }
1222  }
1223 }
1224 
1225 //===========================================================================
1226 // GIVE A GROUP IDX
1227 //===========================================================================
1228 // This function replaces mrFindQuadNeighbors, which in turn replaced
1229 // icvFindQuadNeighbors
1230 void mrFindQuadNeighbors2(std::vector<CvCBQuad::Ptr>& quads, int dilation)
1231 {
1232  // Thresh dilation is used to counter the effect of dilation on the
1233  // distance between 2 neighboring corners. Since the distance below is
1234  // computed as its square, we do here the same. Additionally, we take the
1235  // conservative assumption that dilation was performed using the 3x3 CROSS
1236  // kernel, which coresponds to the 4-neighborhood.
1237  const float thresh_dilation = (float)(2 * dilation + 3) *
1238  (2 * dilation + 3) *
1239  2; // the "*2" is for the x and y component
1240  float dx, dy, dist;
1241  // int cur_quad_group = -1;
1242 
1243  const size_t quad_count = quads.size();
1244 
1245  // Find quad neighbors
1246  for (size_t idx = 0; idx < quad_count; idx++)
1247  {
1248  CvCBQuad::Ptr& cur_quad = quads[idx];
1249 
1250  // Go through all quadrangles and label them in groups
1251  // For each corner of this quadrangle
1252  for (size_t i = 0; i < 4; i++)
1253  {
1254  CvPoint2D32f pt;
1255  float min_dist = FLT_MAX;
1256  int closest_corner_idx = -1;
1257  CvCBQuad::Ptr closest_quad;
1258 
1259  if (cur_quad->neighbors[i]) continue;
1260 
1261  pt = cur_quad->corners[i]->pt;
1262 
1263  // Find the closest corner in all other quadrangles
1264  for (size_t k = 0; k < quad_count; k++)
1265  {
1266  if (k == idx) continue;
1267 
1268  for (size_t j = 0; j < 4; j++)
1269  {
1270  // If it already has a neighbor
1271  if (quads[k]->neighbors[j]) continue;
1272 
1273  dx = pt.x - quads[k]->corners[j]->pt.x;
1274  dy = pt.y - quads[k]->corners[j]->pt.y;
1275  dist = dx * dx + dy * dy;
1276 
1277  // The following "if" checks, whether "dist" is the
1278  // shortest so far and smaller than the smallest
1279  // edge length of the current and target quads
1280  if (dist < min_dist &&
1281  dist <= (cur_quad->edge_len + thresh_dilation) &&
1282  dist <= (quads[k]->edge_len + thresh_dilation))
1283  {
1284  // First Check everything from the viewpoint of the
1285  // current quad
1286  // compute midpoints of "parallel" quad sides 1
1287  float x1 = (cur_quad->corners[i]->pt.x +
1288  cur_quad->corners[(i + 1) % 4]->pt.x) /
1289  2;
1290  float y1 = (cur_quad->corners[i]->pt.y +
1291  cur_quad->corners[(i + 1) % 4]->pt.y) /
1292  2;
1293  float x2 = (cur_quad->corners[(i + 2) % 4]->pt.x +
1294  cur_quad->corners[(i + 3) % 4]->pt.x) /
1295  2;
1296  float y2 = (cur_quad->corners[(i + 2) % 4]->pt.y +
1297  cur_quad->corners[(i + 3) % 4]->pt.y) /
1298  2;
1299  // compute midpoints of "parallel" quad sides 2
1300  float x3 = (cur_quad->corners[i]->pt.x +
1301  cur_quad->corners[(i + 3) % 4]->pt.x) /
1302  2;
1303  float y3 = (cur_quad->corners[i]->pt.y +
1304  cur_quad->corners[(i + 3) % 4]->pt.y) /
1305  2;
1306  float x4 = (cur_quad->corners[(i + 1) % 4]->pt.x +
1307  cur_quad->corners[(i + 2) % 4]->pt.x) /
1308  2;
1309  float y4 = (cur_quad->corners[(i + 1) % 4]->pt.y +
1310  cur_quad->corners[(i + 2) % 4]->pt.y) /
1311  2;
1312 
1313  // MARTIN: Heuristic
1314  // For the corner "j" of quad "k" to be considered,
1315  // it needs to be on the same side of the two lines as
1316  // corner "i". This is given, if the cross product has
1317  // the same sign for both computations below:
1318  float a1 = x1 - x2;
1319  float b1 = y1 - y2;
1320  // the current corner
1321  float c11 = cur_quad->corners[i]->pt.x - x2;
1322  float d11 = cur_quad->corners[i]->pt.y - y2;
1323  // the candidate corner
1324  float c12 = quads[k]->corners[j]->pt.x - x2;
1325  float d12 = quads[k]->corners[j]->pt.y - y2;
1326  float sign11 = a1 * d11 - c11 * b1;
1327  float sign12 = a1 * d12 - c12 * b1;
1328 
1329  float a2 = x3 - x4;
1330  float b2 = y3 - y4;
1331  // the current corner
1332  float c21 = cur_quad->corners[i]->pt.x - x4;
1333  float d21 = cur_quad->corners[i]->pt.y - y4;
1334  // the candidate corner
1335  float c22 = quads[k]->corners[j]->pt.x - x4;
1336  float d22 = quads[k]->corners[j]->pt.y - y4;
1337  float sign21 = a2 * d21 - c21 * b2;
1338  float sign22 = a2 * d22 - c22 * b2;
1339 
1340  // Then make shure that two border quads of the same row
1341  // or
1342  // column don't link. Check from the current corner's
1343  // view,
1344  // whether the corner diagonal from the candidate corner
1345  // is also on the same side of the two lines as the
1346  // current
1347  // corner and the candidate corner.
1348  float c13 = quads[k]->corners[(j + 2) % 4]->pt.x - x2;
1349  float d13 = quads[k]->corners[(j + 2) % 4]->pt.y - y2;
1350  float c23 = quads[k]->corners[(j + 2) % 4]->pt.x - x4;
1351  float d23 = quads[k]->corners[(j + 2) % 4]->pt.y - y4;
1352  float sign13 = a1 * d13 - c13 * b1;
1353  float sign23 = a2 * d23 - c23 * b2;
1354 
1355  // Then check everything from the viewpoint of the
1356  // candidate quad
1357  // compute midpoints of "parallel" quad sides 1
1358  float u1 = (quads[k]->corners[j]->pt.x +
1359  quads[k]->corners[(j + 1) % 4]->pt.x) /
1360  2;
1361  float v1 = (quads[k]->corners[j]->pt.y +
1362  quads[k]->corners[(j + 1) % 4]->pt.y) /
1363  2;
1364  float u2 = (quads[k]->corners[(j + 2) % 4]->pt.x +
1365  quads[k]->corners[(j + 3) % 4]->pt.x) /
1366  2;
1367  float v2 = (quads[k]->corners[(j + 2) % 4]->pt.y +
1368  quads[k]->corners[(j + 3) % 4]->pt.y) /
1369  2;
1370  // compute midpoints of "parallel" quad sides 2
1371  float u3 = (quads[k]->corners[j]->pt.x +
1372  quads[k]->corners[(j + 3) % 4]->pt.x) /
1373  2;
1374  float v3 = (quads[k]->corners[j]->pt.y +
1375  quads[k]->corners[(j + 3) % 4]->pt.y) /
1376  2;
1377  float u4 = (quads[k]->corners[(j + 1) % 4]->pt.x +
1378  quads[k]->corners[(j + 2) % 4]->pt.x) /
1379  2;
1380  float v4 = (quads[k]->corners[(j + 1) % 4]->pt.y +
1381  quads[k]->corners[(j + 2) % 4]->pt.y) /
1382  2;
1383 
1384  // MARTIN: Heuristic
1385  // for the corner "j" of quad "k" to be considered, it
1386  // needs to be on the same side of the two lines as
1387  // corner "i". This is again given, if the cross
1388  // product has the same sign for both computations
1389  // below:
1390  float a3 = u1 - u2;
1391  float b3 = v1 - v2;
1392  // the current corner
1393  float c31 = cur_quad->corners[i]->pt.x - u2;
1394  float d31 = cur_quad->corners[i]->pt.y - v2;
1395  // the candidate corner
1396  float c32 = quads[k]->corners[j]->pt.x - u2;
1397  float d32 = quads[k]->corners[j]->pt.y - v2;
1398  float sign31 = a3 * d31 - c31 * b3;
1399  float sign32 = a3 * d32 - c32 * b3;
1400 
1401  float a4 = u3 - u4;
1402  float b4 = v3 - v4;
1403  // the current corner
1404  float c41 = cur_quad->corners[i]->pt.x - u4;
1405  float d41 = cur_quad->corners[i]->pt.y - v4;
1406  // the candidate corner
1407  float c42 = quads[k]->corners[j]->pt.x - u4;
1408  float d42 = quads[k]->corners[j]->pt.y - v4;
1409  float sign41 = a4 * d41 - c41 * b4;
1410  float sign42 = a4 * d42 - c42 * b4;
1411 
1412  // Then make shure that two border quads of the same row
1413  // or
1414  // column don't link. Check from the candidate corner's
1415  // view,
1416  // whether the corner diagonal from the current corner
1417  // is also on the same side of the two lines as the
1418  // current
1419  // corner and the candidate corner.
1420  float c33 = cur_quad->corners[(i + 2) % 4]->pt.x - u2;
1421  float d33 = cur_quad->corners[(i + 2) % 4]->pt.y - v2;
1422  float c43 = cur_quad->corners[(i + 2) % 4]->pt.x - u4;
1423  float d43 = cur_quad->corners[(i + 2) % 4]->pt.y - v4;
1424  float sign33 = a3 * d33 - c33 * b3;
1425  float sign43 = a4 * d43 - c43 * b4;
1426 
1427  // Check whether conditions are fulfilled
1428  if (((sign11 < 0 && sign12 < 0) ||
1429  (sign11 > 0 && sign12 > 0)) &&
1430  ((sign21 < 0 && sign22 < 0) ||
1431  (sign21 > 0 && sign22 > 0)) &&
1432  ((sign31 < 0 && sign32 < 0) ||
1433  (sign31 > 0 && sign32 > 0)) &&
1434  ((sign41 < 0 && sign42 < 0) ||
1435  (sign41 > 0 && sign42 > 0)) &&
1436  ((sign11 < 0 && sign13 < 0) ||
1437  (sign11 > 0 && sign13 > 0)) &&
1438  ((sign21 < 0 && sign23 < 0) ||
1439  (sign21 > 0 && sign23 > 0)) &&
1440  ((sign31 < 0 && sign33 < 0) ||
1441  (sign31 > 0 && sign33 > 0)) &&
1442  ((sign41 < 0 && sign43 < 0) ||
1443  (sign41 > 0 && sign43 > 0)))
1444 
1445  {
1446  closest_corner_idx = j;
1447  closest_quad = quads[k];
1448  min_dist = dist;
1449  }
1450  }
1451  }
1452  }
1453 
1454  // Have we found a matching corner point?
1455  if (closest_corner_idx >= 0 && min_dist < FLT_MAX)
1456  {
1457  CvCBCorner::Ptr& closest_corner =
1458  closest_quad->corners[closest_corner_idx];
1459 
1460  // Make shure that the closest quad does not have the current
1461  // quad as neighbor already
1462  bool skip = false;
1463  for (size_t j = 0; !skip && j < 4; j++)
1464  skip = closest_quad->neighbors[j] == cur_quad;
1465 
1466  if (skip) continue;
1467 
1468  // We've found one more corner - remember it
1469  closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1470  closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1471 
1472  cur_quad->count++;
1473  cur_quad->neighbors[i] = closest_quad;
1474  cur_quad->corners[i] = closest_corner;
1475 
1476  closest_quad->count++;
1477  closest_quad->neighbors[closest_corner_idx] = cur_quad;
1478  closest_quad->corners[closest_corner_idx] = closest_corner;
1479  }
1480  }
1481  }
1482 }
1483 
1484 //===========================================================================
1485 // AUGMENT PATTERN WITH ADDITIONAL QUADS
1486 //===========================================================================
1487 // The first part of the function is basically a copy of
1488 // "mrFindQuadNeighbors2"
1489 // The comparisons between two points and two lines could be computed in their
1490 // own function
1491 int mrAugmentBestRun(
1492  std::vector<CvCBQuad::Ptr>& new_quads, int new_dilation,
1493  std::vector<CvCBQuad::Ptr>& old_quads, int old_dilation)
1494 {
1495  // thresh dilation is used to counter the effect of dilation on the
1496  // distance between 2 neighboring corners. Since the distance below is
1497  // computed as its square, we do here the same. Additionally, we take the
1498  // conservative assumption that dilation was performed using the 3x3 CROSS
1499  // kernel, which coresponds to the 4-neighborhood.
1500  //
1501  // the "*2" is for the x and y component
1502  // int idx, i, k, j; // the "3" is for initial corner mismatch
1503  const float thresh_dilation =
1504  (float)(2 * new_dilation + 3) * (2 * old_dilation + 3) * 2;
1505  float dx, dy, dist;
1506 
1507  // Search all old quads which have a neighbor that needs to be linked
1508  for (size_t idx = 0; idx < old_quads.size(); idx++)
1509  {
1510  CvCBQuad::Ptr cur_quad = old_quads[idx];
1511 
1512  // For each corner of this quadrangle
1513  for (int i = 0; i < 4; i++)
1514  {
1515  CvPoint2D32f pt;
1516  float min_dist = FLT_MAX;
1517  int closest_corner_idx = -1;
1518  CvCBQuad::Ptr closest_quad;
1519  // CvCBCorner *closest_corner = 0;
1520 
1521  // If cur_quad corner[i] is already linked, continue
1522  if (cur_quad->corners[i]->needsNeighbor == false) continue;
1523 
1524  pt = cur_quad->corners[i]->pt;
1525 
1526  // Look for a match in all new_quads' corners
1527  for (size_t k = 0; k < new_quads.size(); k++)
1528  {
1529  // Only look at unlabeled new quads
1530  if (new_quads[k]->labeled == true) continue;
1531 
1532  for (int j = 0; j < 4; j++)
1533  {
1534  // Only proceed if they are less than dist away from each
1535  // other
1536  dx = pt.x - new_quads[k]->corners[j]->pt.x;
1537  dy = pt.y - new_quads[k]->corners[j]->pt.y;
1538  dist = dx * dx + dy * dy;
1539 
1540  if ((dist < min_dist) &&
1541  dist <= (cur_quad->edge_len + thresh_dilation) &&
1542  dist <= (new_quads[k]->edge_len + thresh_dilation))
1543  {
1544  // First Check everything from the viewpoint of the
1545  // current quad compute midpoints of "parallel" quad
1546  // sides 1
1547  float x1 = (cur_quad->corners[i]->pt.x +
1548  cur_quad->corners[(i + 1) % 4]->pt.x) /
1549  2;
1550  float y1 = (cur_quad->corners[i]->pt.y +
1551  cur_quad->corners[(i + 1) % 4]->pt.y) /
1552  2;
1553  float x2 = (cur_quad->corners[(i + 2) % 4]->pt.x +
1554  cur_quad->corners[(i + 3) % 4]->pt.x) /
1555  2;
1556  float y2 = (cur_quad->corners[(i + 2) % 4]->pt.y +
1557  cur_quad->corners[(i + 3) % 4]->pt.y) /
1558  2;
1559  // compute midpoints of "parallel" quad sides 2
1560  float x3 = (cur_quad->corners[i]->pt.x +
1561  cur_quad->corners[(i + 3) % 4]->pt.x) /
1562  2;
1563  float y3 = (cur_quad->corners[i]->pt.y +
1564  cur_quad->corners[(i + 3) % 4]->pt.y) /
1565  2;
1566  float x4 = (cur_quad->corners[(i + 1) % 4]->pt.x +
1567  cur_quad->corners[(i + 2) % 4]->pt.x) /
1568  2;
1569  float y4 = (cur_quad->corners[(i + 1) % 4]->pt.y +
1570  cur_quad->corners[(i + 2) % 4]->pt.y) /
1571  2;
1572 
1573  // MARTIN: Heuristic
1574  // For the corner "j" of quad "k" to be considered,
1575  // it needs to be on the same side of the two lines as
1576  // corner "i". This is given, if the cross product has
1577  // the same sign for both computations below:
1578  float a1 = x1 - x2;
1579  float b1 = y1 - y2;
1580  // the current corner
1581  float c11 = cur_quad->corners[i]->pt.x - x2;
1582  float d11 = cur_quad->corners[i]->pt.y - y2;
1583  // the candidate corner
1584  float c12 = new_quads[k]->corners[j]->pt.x - x2;
1585  float d12 = new_quads[k]->corners[j]->pt.y - y2;
1586  float sign11 = a1 * d11 - c11 * b1;
1587  float sign12 = a1 * d12 - c12 * b1;
1588 
1589  float a2 = x3 - x4;
1590  float b2 = y3 - y4;
1591  // the current corner
1592  float c21 = cur_quad->corners[i]->pt.x - x4;
1593  float d21 = cur_quad->corners[i]->pt.y - y4;
1594  // the candidate corner
1595  float c22 = new_quads[k]->corners[j]->pt.x - x4;
1596  float d22 = new_quads[k]->corners[j]->pt.y - y4;
1597  float sign21 = a2 * d21 - c21 * b2;
1598  float sign22 = a2 * d22 - c22 * b2;
1599 
1600  // Also make shure that two border quads of the same row
1601  // or
1602  // column don't link. Check from the current corner's
1603  // view,
1604  // whether the corner diagonal from the candidate corner
1605  // is also on the same side of the two lines as the
1606  // current
1607  // corner and the candidate corner.
1608  float c13 =
1609  new_quads[k]->corners[(j + 2) % 4]->pt.x - x2;
1610  float d13 =
1611  new_quads[k]->corners[(j + 2) % 4]->pt.y - y2;
1612  float c23 =
1613  new_quads[k]->corners[(j + 2) % 4]->pt.x - x4;
1614  float d23 =
1615  new_quads[k]->corners[(j + 2) % 4]->pt.y - y4;
1616  float sign13 = a1 * d13 - c13 * b1;
1617  float sign23 = a2 * d23 - c23 * b2;
1618 
1619  // Second: Then check everything from the viewpoint of
1620  // the candidate quad. Compute midpoints of "parallel"
1621  // quad sides 1
1622  float u1 = (new_quads[k]->corners[j]->pt.x +
1623  new_quads[k]->corners[(j + 1) % 4]->pt.x) /
1624  2;
1625  float v1 = (new_quads[k]->corners[j]->pt.y +
1626  new_quads[k]->corners[(j + 1) % 4]->pt.y) /
1627  2;
1628  float u2 = (new_quads[k]->corners[(j + 2) % 4]->pt.x +
1629  new_quads[k]->corners[(j + 3) % 4]->pt.x) /
1630  2;
1631  float v2 = (new_quads[k]->corners[(j + 2) % 4]->pt.y +
1632  new_quads[k]->corners[(j + 3) % 4]->pt.y) /
1633  2;
1634  // compute midpoints of "parallel" quad sides 2
1635  float u3 = (new_quads[k]->corners[j]->pt.x +
1636  new_quads[k]->corners[(j + 3) % 4]->pt.x) /
1637  2;
1638  float v3 = (new_quads[k]->corners[j]->pt.y +
1639  new_quads[k]->corners[(j + 3) % 4]->pt.y) /
1640  2;
1641  float u4 = (new_quads[k]->corners[(j + 1) % 4]->pt.x +
1642  new_quads[k]->corners[(j + 2) % 4]->pt.x) /
1643  2;
1644  float v4 = (new_quads[k]->corners[(j + 1) % 4]->pt.y +
1645  new_quads[k]->corners[(j + 2) % 4]->pt.y) /
1646  2;
1647 
1648  // MARTIN: Heuristic
1649  // For the corner "j" of quad "k" to be considered,
1650  // it needs to be on the same side of the two lines as
1651  // corner "i". This is given, if the cross product has
1652  // the same sign for both computations below:
1653  float a3 = u1 - u2;
1654  float b3 = v1 - v2;
1655  // the current corner
1656  float c31 = cur_quad->corners[i]->pt.x - u2;
1657  float d31 = cur_quad->corners[i]->pt.y - v2;
1658  // the candidate corner
1659  float c32 = new_quads[k]->corners[j]->pt.x - u2;
1660  float d32 = new_quads[k]->corners[j]->pt.y - v2;
1661  float sign31 = a3 * d31 - c31 * b3;
1662  float sign32 = a3 * d32 - c32 * b3;
1663 
1664  float a4 = u3 - u4;
1665  float b4 = v3 - v4;
1666  // the current corner
1667  float c41 = cur_quad->corners[i]->pt.x - u4;
1668  float d41 = cur_quad->corners[i]->pt.y - v4;
1669  // the candidate corner
1670  float c42 = new_quads[k]->corners[j]->pt.x - u4;
1671  float d42 = new_quads[k]->corners[j]->pt.y - v4;
1672  float sign41 = a4 * d41 - c41 * b4;
1673  float sign42 = a4 * d42 - c42 * b4;
1674 
1675  // Also make shure that two border quads of the same row
1676  // or
1677  // column don't link. Check from the candidate corner's
1678  // view,
1679  // whether the corner diagonal from the current corner
1680  // is also on the same side of the two lines as the
1681  // current
1682  // corner and the candidate corner.
1683  float c33 = cur_quad->corners[(i + 2) % 4]->pt.x - u2;
1684  float d33 = cur_quad->corners[(i + 2) % 4]->pt.y - v2;
1685  float c43 = cur_quad->corners[(i + 2) % 4]->pt.x - u4;
1686  float d43 = cur_quad->corners[(i + 2) % 4]->pt.y - v4;
1687  float sign33 = a3 * d33 - c33 * b3;
1688  float sign43 = a4 * d43 - c43 * b4;
1689 
1690  // This time we also need to make shure, that no quad
1691  // is linked to a quad of another dilation run which
1692  // may lie INSIDE it!!!
1693  // Third: Therefore check everything from the viewpoint
1694  // of the current quad compute midpoints of "parallel"
1695  // quad sides 1
1696  float x5 = cur_quad->corners[i]->pt.x;
1697  float y5 = cur_quad->corners[i]->pt.y;
1698  float x6 = cur_quad->corners[(i + 1) % 4]->pt.x;
1699  float y6 = cur_quad->corners[(i + 1) % 4]->pt.y;
1700  // compute midpoints of "parallel" quad sides 2
1701  float x7 = x5;
1702  float y7 = y5;
1703  float x8 = cur_quad->corners[(i + 3) % 4]->pt.x;
1704  float y8 = cur_quad->corners[(i + 3) % 4]->pt.y;
1705 
1706  // MARTIN: Heuristic
1707  // For the corner "j" of quad "k" to be considered,
1708  // it needs to be on the other side of the two lines
1709  // than
1710  // corner "i". This is given, if the cross product has
1711  // a different sign for both computations below:
1712  float a5 = x6 - x5;
1713  float b5 = y6 - y5;
1714  // the current corner
1715  float c51 = cur_quad->corners[(i + 2) % 4]->pt.x - x5;
1716  float d51 = cur_quad->corners[(i + 2) % 4]->pt.y - y5;
1717  // the candidate corner
1718  float c52 = new_quads[k]->corners[j]->pt.x - x5;
1719  float d52 = new_quads[k]->corners[j]->pt.y - y5;
1720  float sign51 = a5 * d51 - c51 * b5;
1721  float sign52 = a5 * d52 - c52 * b5;
1722 
1723  float a6 = x8 - x7;
1724  float b6 = y8 - y7;
1725  // the current corner
1726  float c61 = cur_quad->corners[(i + 2) % 4]->pt.x - x7;
1727  float d61 = cur_quad->corners[(i + 2) % 4]->pt.y - y7;
1728  // the candidate corner
1729  float c62 = new_quads[k]->corners[j]->pt.x - x7;
1730  float d62 = new_quads[k]->corners[j]->pt.y - y7;
1731  float sign61 = a6 * d61 - c61 * b6;
1732  float sign62 = a6 * d62 - c62 * b6;
1733 
1734  // Fourth: Then check everything from the viewpoint of
1735  // the candidate quad compute midpoints of "parallel"
1736  // quad sides 1
1737  float u5 = new_quads[k]->corners[j]->pt.x;
1738  float v5 = new_quads[k]->corners[j]->pt.y;
1739  float u6 = new_quads[k]->corners[(j + 1) % 4]->pt.x;
1740  float v6 = new_quads[k]->corners[(j + 1) % 4]->pt.y;
1741  // compute midpoints of "parallel" quad sides 2
1742  float u7 = u5;
1743  float v7 = v5;
1744  float u8 = new_quads[k]->corners[(j + 3) % 4]->pt.x;
1745  float v8 = new_quads[k]->corners[(j + 3) % 4]->pt.y;
1746 
1747  // MARTIN: Heuristic
1748  // For the corner "j" of quad "k" to be considered,
1749  // it needs to be on the other side of the two lines
1750  // than
1751  // corner "i". This is given, if the cross product has
1752  // a different sign for both computations below:
1753  float a7 = u6 - u5;
1754  float b7 = v6 - v5;
1755  // the current corner
1756  float c71 = cur_quad->corners[i]->pt.x - u5;
1757  float d71 = cur_quad->corners[i]->pt.y - v5;
1758  // the candidate corner
1759  float c72 =
1760  new_quads[k]->corners[(j + 2) % 4]->pt.x - u5;
1761  float d72 =
1762  new_quads[k]->corners[(j + 2) % 4]->pt.y - v5;
1763  float sign71 = a7 * d71 - c71 * b7;
1764  float sign72 = a7 * d72 - c72 * b7;
1765 
1766  float a8 = u8 - u7;
1767  float b8 = v8 - v7;
1768  // the current corner
1769  float c81 = cur_quad->corners[i]->pt.x - u7;
1770  float d81 = cur_quad->corners[i]->pt.y - v7;
1771  // the candidate corner
1772  float c82 =
1773  new_quads[k]->corners[(j + 2) % 4]->pt.x - u7;
1774  float d82 =
1775  new_quads[k]->corners[(j + 2) % 4]->pt.y - v7;
1776  float sign81 = a8 * d81 - c81 * b8;
1777  float sign82 = a8 * d82 - c82 * b8;
1778 
1779  // Check whether conditions are fulfilled
1780  if (((sign11 < 0 && sign12 < 0) ||
1781  (sign11 > 0 && sign12 > 0)) &&
1782  ((sign21 < 0 && sign22 < 0) ||
1783  (sign21 > 0 && sign22 > 0)) &&
1784  ((sign31 < 0 && sign32 < 0) ||
1785  (sign31 > 0 && sign32 > 0)) &&
1786  ((sign41 < 0 && sign42 < 0) ||
1787  (sign41 > 0 && sign42 > 0)) &&
1788  ((sign11 < 0 && sign13 < 0) ||
1789  (sign11 > 0 && sign13 > 0)) &&
1790  ((sign21 < 0 && sign23 < 0) ||
1791  (sign21 > 0 && sign23 > 0)) &&
1792  ((sign31 < 0 && sign33 < 0) ||
1793  (sign31 > 0 && sign33 > 0)) &&
1794  ((sign41 < 0 && sign43 < 0) ||
1795  (sign41 > 0 && sign43 > 0)) &&
1796  ((sign51 < 0 && sign52 > 0) ||
1797  (sign51 > 0 && sign52 < 0)) &&
1798  ((sign61 < 0 && sign62 > 0) ||
1799  (sign61 > 0 && sign62 < 0)) &&
1800  ((sign71 < 0 && sign72 > 0) ||
1801  (sign71 > 0 && sign72 < 0)) &&
1802  ((sign81 < 0 && sign82 > 0) ||
1803  (sign81 > 0 && sign82 < 0)))
1804  {
1805  closest_corner_idx = j;
1806  closest_quad = new_quads[k];
1807  min_dist = dist;
1808  }
1809  }
1810  }
1811  }
1812 
1813  // Have we found a matching corner point?
1814  if (closest_corner_idx >= 0 && min_dist < FLT_MAX)
1815  {
1816  CvCBCorner::Ptr& closest_corner =
1817  closest_quad->corners[closest_corner_idx];
1818  closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1819  closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1820 
1821  // We've found one more corner - remember it
1822  // ATTENTION: write the corner x and y coordinates separately,
1823  // else the crucial row/column entries will be overwritten !!!
1824  cur_quad->corners[i]->pt.x = closest_corner->pt.x;
1825  cur_quad->corners[i]->pt.y = closest_corner->pt.y;
1826  cur_quad->neighbors[i] = closest_quad;
1827  closest_quad->corners[closest_corner_idx]->pt.x =
1828  closest_corner->pt.x;
1829  closest_quad->corners[closest_corner_idx]->pt.y =
1830  closest_corner->pt.y;
1831 
1832  // Label closest quad as labeled. In this way we exclude it
1833  // being considered again during the next loop iteration
1834  closest_quad->labeled = true;
1835 
1836  // We have a new member of the final pattern, copy it over
1837  CvCBQuad::Ptr newQuad = CvCBQuad::Ptr(new CvCBQuad);
1838 
1839  newQuad->count = 1;
1840  newQuad->edge_len = closest_quad->edge_len;
1841  newQuad->group_idx =
1842  cur_quad->group_idx; // the same as the current quad
1843  newQuad->labeled = false; // do it right afterwards
1844 
1845  // We only know one neighbor for shure, initialize rest with
1846  // the nullptr pointer
1847  newQuad->neighbors[closest_corner_idx] = cur_quad;
1848  newQuad->neighbors[(closest_corner_idx + 1) % 4]
1849  .reset(); // = nullptr;
1850  newQuad->neighbors[(closest_corner_idx + 2) % 4]
1851  .reset(); // = nullptr;
1852  newQuad->neighbors[(closest_corner_idx + 3) % 4]
1853  .reset(); // = nullptr;
1854 
1855  for (int j = 0; j < 4; j++)
1856  {
1857  newQuad->corners[j] = CvCBCorner::Ptr(new CvCBCorner);
1858  newQuad->corners[j]->pt.x = closest_quad->corners[j]->pt.x;
1859  newQuad->corners[j]->pt.y = closest_quad->corners[j]->pt.y;
1860  }
1861 
1862  old_quads.push_back(newQuad);
1863  cur_quad->neighbors[i] = newQuad;
1864 
1865  // Start the function again
1866  return -1;
1867  }
1868  }
1869  }
1870 
1871  // Finished, don't start the function again
1872  return 1;
1873 }
1874 
1875 //===========================================================================
1876 // GENERATE QUADRANGLES
1877 //===========================================================================
1878 int icvGenerateQuads(
1879  vector<CvCBQuad::Ptr>& out_quads, vector<CvCBCorner::Ptr>& out_corners,
1880  const CImage& image, int flags, int dilation, bool firstRun)
1881 {
1882  MRPT_UNUSED_PARAM(dilation);
1883  // Initializations
1884  int quad_count = 0;
1885 
1886  // Create temporary storage for contours and the sequence of pointers to
1887  // found quadrangles
1888  cv::MemStorage temp_storage = cv::MemStorage(cvCreateMemStorage(0));
1889 
1890  CvSeq* src_contour = nullptr;
1891  CvSeq* root; // cv::Seq<> root; //
1892  CvContourEx* board = nullptr;
1893  CvContourScanner scanner;
1894 
1895  // Empiric sower bound for the size of allowable quadrangles.
1896  // MARTIN, modified: Added "*0.1" in order to find smaller quads.
1897  const int min_size =
1898  cvRound(image.getWidth() * image.getHeight() * .03 * 0.01 * 0.92 * 0.1);
1899 
1900  root = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage);
1901 
1902  // Initialize contour retrieving routine
1903  cv::Mat im_mat = image.asCvMat<cv::Mat>(SHALLOW_COPY);
1904  IplImage im_ipl = cvIplImage(im_mat);
1905  scanner = cvStartFindContours(
1906  &im_ipl, temp_storage, sizeof(CvContourEx), CV_RETR_CCOMP,
1907  CV_CHAIN_APPROX_SIMPLE);
1908 
1909  // Get all the contours one by one
1910  while ((src_contour = cvFindNextContour(scanner)) != nullptr)
1911  {
1912  CvSeq* dst_contour = nullptr;
1913  CvRect rect = ((CvContour*)src_contour)->rect;
1914 
1915  // Reject contours with a too small perimeter and contours which are
1916  // completely surrounded by another contour
1917  // MARTIN: If this function is called during PART 1, then the parameter
1918  // "first run"
1919  // is set to "true". This guarantees, that only "nice" squares are
1920  // detected.
1921  // During PART 2, we allow the polygonial approcimation function below
1922  // to
1923  // approximate more freely, which can result in recognized "squares"
1924  // that are in
1925  // reality multiple blurred and sticked together squares.
1926  if (CV_IS_SEQ_HOLE(src_contour) && rect.width * rect.height >= min_size)
1927  {
1928  int min_approx_level = 2, max_approx_level;
1929  if (firstRun == true)
1930  max_approx_level = 3;
1931  else
1932  max_approx_level = MAX_CONTOUR_APPROX;
1933  int approx_level;
1934  for (approx_level = min_approx_level;
1935  approx_level <= max_approx_level; approx_level++)
1936  {
1937  dst_contour = cvApproxPoly(
1938  src_contour, sizeof(CvContour), temp_storage,
1939  CV_POLY_APPROX_DP, (float)approx_level);
1940 
1941  // We call this again on its own output, because sometimes
1942  // cvApproxPoly() does not simplify as much as it should.
1943  dst_contour = cvApproxPoly(
1944  dst_contour, sizeof(CvContour), temp_storage,
1945  CV_POLY_APPROX_DP, (float)approx_level);
1946 
1947  if (dst_contour->total == 4) break;
1948  }
1949 
1950  // Reject non-quadrangles
1951  if (dst_contour->total == 4 && cvCheckContourConvexity(dst_contour))
1952  {
1953  CvPoint pt[4];
1954  // double d1, d2; //, p = cvContourPerimeter(dst_contour);
1955  // double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1956  // double dx, dy;
1957 
1958  for (int i = 0; i < 4; i++)
1959  pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1960 
1961  // dx = pt[0].x - pt[2].x;
1962  // dy = pt[0].y - pt[2].y;
1963  // d1 = sqrt(dx*dx + dy*dy);
1964  //
1965  // dx = pt[1].x - pt[3].x;
1966  // dy = pt[1].y - pt[3].y;
1967  // d2 = sqrt(dx*dx + dy*dy);
1968 
1969  // PHILIPG: Only accept those quadrangles which are more
1970  // square than rectangular and which are big enough
1971  // double d3, d4;
1972  // dx = pt[0].x - pt[1].x;
1973  // dy = pt[0].y - pt[1].y;
1974  // d3 = sqrt(dx*dx + dy*dy);
1975  // dx = pt[1].x - pt[2].x;
1976  // dy = pt[1].y - pt[2].y;
1977  // d4 = sqrt(dx*dx + dy*dy);
1978  if (true) //!(flags & CV_CALIB_CB_FILTER_QUADS) ||
1979  // d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size
1980  // &&
1981  // d1 >= 0.15 * p && d2 >= 0.15 * p )
1982  {
1983  auto* parent = (CvContourEx*)(src_contour->v_prev);
1984  parent->counter++;
1985  if (!board || board->counter < parent->counter)
1986  board = parent;
1987  dst_contour->v_prev = (CvSeq*)parent;
1988  cvSeqPush(root, &dst_contour);
1989  }
1990  }
1991  }
1992  }
1993 
1994  // Finish contour retrieving
1995  cvEndFindContours(&scanner);
1996 
1997  // Allocate quad & corner buffers
1998  out_quads.clear();
1999  for (int q = 0; q < root->total; q++)
2000  out_quads.push_back(CvCBQuad::Ptr(new CvCBQuad));
2001 
2002  out_corners.clear();
2003  for (int q = 0; q < 4 * root->total; q++)
2004  out_corners.push_back(CvCBCorner::Ptr(new CvCBCorner));
2005 
2006  // Create array of quads structures
2007  for (int idx = 0; idx < root->total; idx++)
2008  {
2009  CvCBQuad::Ptr& q = out_quads[quad_count];
2010  src_contour = *(CvSeq**)cvGetSeqElem(root, idx);
2011  if ((flags & cv::CALIB_CB_FILTER_QUADS) &&
2012  src_contour->v_prev != (CvSeq*)board)
2013  continue;
2014 
2015  // Reset group ID
2016  // memset( q, 0, sizeof(*q) );
2017  q->group_idx = -1;
2018  assert(src_contour->total == 4);
2019  for (int i = 0; i < 4; i++)
2020  {
2021  CvPoint2D32f pt =
2022  cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
2023  CvCBCorner::Ptr& corner = out_corners
2024  [quad_count * 4 + i]; // &(*out_corners)[quad_count*4
2025  // + i];
2026 
2027  // memset( corner, 0, sizeof(*corner) );
2028  corner->pt = pt;
2029  q->corners[i] = corner;
2030  }
2031  q->edge_len = FLT_MAX;
2032  for (int i = 0; i < 4; i++)
2033  {
2034  float dx = q->corners[i]->pt.x - q->corners[(i + 1) & 3]->pt.x;
2035  float dy = q->corners[i]->pt.y - q->corners[(i + 1) & 3]->pt.y;
2036  float d = dx * dx + dy * dy;
2037  if (q->edge_len > d) q->edge_len = d;
2038  }
2039  quad_count++;
2040  }
2041 
2042  if (cvGetErrStatus() < 0)
2043  {
2044  out_quads.clear();
2045  // if( out_corners ) cvFree( out_corners );
2046  out_corners.clear();
2047  quad_count = 0;
2048  }
2049 
2050  cvClearSeq(root);
2051 
2052  return quad_count;
2053 }
2054 
2055 // Return 1 on success in finding all the quads, 0 on didn't, -1 on error.
2056 int myQuads2Points(
2057  const std::vector<CvCBQuad::Ptr>& output_quads, const CvSize& pattern_size,
2058  std::vector<CvPoint2D32f>& out_corners)
2059 {
2060  // Initialize
2061  out_corners.clear();
2062 
2063  bool flagRow = false;
2064  bool flagColumn = false;
2065  int maxPattern_sizeRow = -1;
2066  int maxPattern_sizeColumn = -1;
2067 
2068  // Compute the minimum and maximum row / column ID
2069  // (it is unlikely that more than 8bit checkers are used per dimension)
2070  int min_row = 127;
2071  int max_row = -127;
2072  int min_column = 127;
2073  int max_column = -127;
2074 
2075  for (size_t i = 0; i < output_quads.size(); i++)
2076  {
2077  const CvCBQuad::Ptr& q = output_quads[i];
2078 
2079  for (int j = 0; j < 4; j++)
2080  {
2081  if ((q->corners[j])->row > max_row) max_row = (q->corners[j])->row;
2082  if ((q->corners[j])->row < min_row) min_row = (q->corners[j])->row;
2083  if ((q->corners[j])->column > max_column)
2084  max_column = (q->corners[j])->column;
2085  if ((q->corners[j])->column < min_column)
2086  min_column = (q->corners[j])->column;
2087  }
2088  }
2089 
2090  // If in a given direction the target pattern size is reached, we know
2091  // exactly how
2092  // the checkerboard is oriented.
2093  // Else we need to prepare enough "dummy" corners for the worst case.
2094  for (size_t i = 0; i < output_quads.size(); i++)
2095  {
2096  const CvCBQuad::Ptr& q = output_quads[i];
2097 
2098  for (int j = 0; j < 4; j++)
2099  {
2100  if ((q->corners[j])->column == max_column &&
2101  (q->corners[j])->row != min_row &&
2102  (q->corners[j])->row != max_row)
2103  {
2104  if ((q->corners[j]->needsNeighbor) == false)
2105  {
2106  // We know, that the target pattern size is reached
2107  // in column direction
2108  flagColumn = true;
2109  }
2110  }
2111  if ((q->corners[j])->row == max_row &&
2112  (q->corners[j])->column != min_column &&
2113  (q->corners[j])->column != max_column)
2114  {
2115  if ((q->corners[j]->needsNeighbor) == false)
2116  {
2117  // We know, that the target pattern size is reached
2118  // in row direction
2119  flagRow = true;
2120  }
2121  }
2122  }
2123  }
2124 
2125  if (flagColumn == true)
2126  {
2127  if (max_column - min_column == pattern_size.width + 1)
2128  {
2129  maxPattern_sizeColumn = pattern_size.width;
2130  maxPattern_sizeRow = pattern_size.height;
2131  }
2132  else
2133  {
2134  maxPattern_sizeColumn = pattern_size.height;
2135  maxPattern_sizeRow = pattern_size.width;
2136  }
2137  }
2138  else if (flagRow == true)
2139  {
2140  if (max_row - min_row == pattern_size.width + 1)
2141  {
2142  maxPattern_sizeRow = pattern_size.width;
2143  maxPattern_sizeColumn = pattern_size.height;
2144  }
2145  else
2146  {
2147  maxPattern_sizeRow = pattern_size.height;
2148  maxPattern_sizeColumn = pattern_size.width;
2149  }
2150  }
2151  else
2152  {
2153  // If target pattern size is not reached in at least one of the two
2154  // directions, then we do not know where the remaining corners are
2155  // located. Account for this.
2156  maxPattern_sizeColumn = max(pattern_size.width, pattern_size.height);
2157  maxPattern_sizeRow = max(pattern_size.width, pattern_size.height);
2158  }
2159 
2160  // JL: Check sizes:
2161  if (maxPattern_sizeRow * maxPattern_sizeColumn !=
2162  pattern_size.width * pattern_size.height)
2163  return 0; // Bad...
2164  // and also, swap rows/columns so we always have consistently the points in
2165  // the order: first all columns in a row, then the next row, etc...
2166  bool do_swap_col_row = maxPattern_sizeRow != pattern_size.height;
2167 
2168  if (do_swap_col_row)
2169  {
2170  std::swap(min_row, min_column);
2171  std::swap(maxPattern_sizeRow, maxPattern_sizeColumn);
2172  }
2173 
2174  // Write the corners in increasing order to "out_corners"
2175 
2176  for (int i = min_row + 1; i < maxPattern_sizeRow + min_row + 1; i++)
2177  {
2178  for (int j = min_column + 1; j < maxPattern_sizeColumn + min_column + 1;
2179  j++)
2180  {
2181  // Reset the iterator
2182  int iter = 1;
2183 
2184  for (size_t k = 0; k < output_quads.size(); k++)
2185  {
2186  for (int l = 0; l < 4; l++)
2187  {
2188  int r = output_quads[k]->corners[l]->row;
2189  int c = output_quads[k]->corners[l]->column;
2190  if (do_swap_col_row) std::swap(r, c);
2191 
2192  if (r == i && c == j)
2193  {
2194  // Only write corners to the output file, which are
2195  // connected
2196  // i.e. only if iter == 2
2197  if (iter == 2)
2198  {
2199  // The respective row and column have been found,
2200  // save point:
2201  out_corners.push_back(
2202  output_quads[k]->corners[l]->pt);
2203  }
2204 
2205  // If the iterator is larger than two, this means that
2206  // more than
2207  // two corners have the same row / column entries. Then
2208  // some
2209  // linking errors must have occured and we should not
2210  // use the found
2211  // pattern
2212  if (iter > 2) return -1;
2213 
2214  iter++;
2215  }
2216  }
2217  }
2218 
2219  // If the respective row / column is non - existent or is a border
2220  // corner
2221  if (iter == 1 || iter == 2)
2222  {
2223  // cornersX << -1;
2224  // cornersX << " ";
2225  // cornersY << -1;
2226  // cornersY << " ";
2227  }
2228  }
2229  }
2230 
2231  // All corners found?
2232  return (out_corners.size() ==
2233  size_t(pattern_size.width) * size_t(pattern_size.height))
2234  ? 1
2235  : 0;
2236 }
2237 
2238 // Make unique all the (smart pointers-pointed) objects in the list and
2239 // neighbors lists.
2240 void quadListMakeUnique(std::vector<CvCBQuad::Ptr>& quads)
2241 {
2242  std::map<CvCBQuad*, size_t> pointer2index;
2243  for (size_t i = 0; i < quads.size(); i++) pointer2index[quads[i].get()] = i;
2244 
2245  vector<std::array<size_t, 4>> neig_indices(quads.size());
2246  for (size_t i = 0; i < quads.size(); i++)
2247  for (size_t j = 0; j < 4; j++)
2248  neig_indices[i][j] = pointer2index[quads[i]->neighbors[j].get()];
2249 
2250  std::vector<CvCBQuad::Ptr> new_quads = quads;
2251  std::for_each(new_quads.begin(), new_quads.end(), [](CvCBQuad::Ptr& ptr) {
2252  ptr = CvCBQuad::Ptr(new CvCBQuad(*ptr));
2253  });
2254  for (size_t i = 0; i < new_quads.size(); i++)
2255  for (size_t j = 0; j < 4; j++)
2256  new_quads[i]->neighbors[j] = new_quads[neig_indices[i][j]];
2257 }
2258 
2259 //===========================================================================
2260 // END OF FILE (Of "OCamCalib Toolbox" code)
2261 //===========================================================================
2262 
2263 #endif // MRPT_HAS_OPENCV
mrFindQuadNeighbors2
void mrFindQuadNeighbors2(std::vector< CvCBQuad::Ptr > &quads, int dilation)
Definition: checkerboard_ocamcalib_detector.cpp:1230
CvCBCorner::Ptr
std::shared_ptr< CvCBCorner > Ptr
Definition: checkerboard_ocamcalib_detector.h:39
mrpt::img::SHALLOW_COPY
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:75
mrLabelQuadGroup
void mrLabelQuadGroup(std::vector< CvCBQuad::Ptr > &quad_group, const CvSize &pattern_size, bool firstRun)
Definition: checkerboard_ocamcalib_detector.cpp:769
icvCleanFoundConnectedQuads
void icvCleanFoundConnectedQuads(std::vector< CvCBQuad::Ptr > &quad_group, const CvSize &pattern_size)
Definition: checkerboard_ocamcalib_detector.cpp:545
mrpt::img::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
std
STL namespace.
mrpt::img::CImage::asCvMat
void asCvMat(cv::Mat &out_img, copy_type_t copy_type) const
Makes a shallow or deep copy of this image into the provided cv::Mat.
Definition: CImage.cpp:217
icvGenerateQuads
int icvGenerateQuads(vector< CvCBQuad::Ptr > &out_quads, vector< CvCBCorner::Ptr > &out_corners, const CImage &image, int flags, int dilation, bool firstRun)
Definition: checkerboard_ocamcalib_detector.cpp:1878
mrAugmentBestRun
int mrAugmentBestRun(std::vector< CvCBQuad::Ptr > &new_quads, int new_dilation, std::vector< CvCBQuad::Ptr > &old_quads, int old_dilation)
Definition: checkerboard_ocamcalib_detector.cpp:1491
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::img::CImage::getWidth
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
median
double median(const std::vector< double > &vec)
Definition: checkerboard_ocamcalib_detector.cpp:532
std::shared_ptr
Definition: TTypeName.h:21
do_special_dilation
bool do_special_dilation(CImage &thresh_img, const int dilations, IplConvKernel *kernel_cross, IplConvKernel *kernel_rect, IplConvKernel *kernel_diag1, IplConvKernel *kernel_diag2, IplConvKernel *kernel_horz, IplConvKernel *kernel_vert)
Definition: checkerboard_ocamcalib_detector.cpp:82
triangleArea
double triangleArea(double x0, double y0, double x1, double y1, double x2, double y2)
Definition: checkerboard_ocamcalib_detector.cpp:526
myQuads2Points
int myQuads2Points(const std::vector< CvCBQuad::Ptr > &output_quads, const CvSize &pattern_size, std::vector< CvPoint2D32f > &out_corners)
Definition: checkerboard_ocamcalib_detector.cpp:2056
MAX_CONTOUR_APPROX
#define MAX_CONTOUR_APPROX
Definition: checkerboard_ocamcalib_detector.cpp:78
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
checkerboard_ocamcalib_detector.h
IplImage
struct _IplImage IplImage
Definition: img/CImage.h:22
icvFindConnectedQuads
void icvFindConnectedQuads(std::vector< CvCBQuad::Ptr > &quad, std::vector< CvCBQuad::Ptr > &out_group, const int group_idx, const int dilation)
Definition: checkerboard_ocamcalib_detector.cpp:715
CvCBQuad::Ptr
std::shared_ptr< CvCBQuad > Ptr
Definition: checkerboard_ocamcalib_detector.h:55
CvCBQuad::corners
CvCBCorner::Ptr corners[4]
Definition: checkerboard_ocamcalib_detector.h:61
quadListMakeUnique
void quadListMakeUnique(std::vector< CvCBQuad::Ptr > &quads)
Definition: checkerboard_ocamcalib_detector.cpp:2240
cvFindChessboardCorners3
int cvFindChessboardCorners3(const CImage &img_, CvSize pattern_size, std::vector< CvPoint2D32f > &out_corners)
Definition: checkerboard_ocamcalib_detector.cpp:225
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at mié mar 25 23:05:41 CET 2020