MRPT  2.0.0
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, [[maybe_unused]] const int dilation)
718 {
719  // initializations
720  out_group.clear();
721 
722  const size_t quad_count = quad.size();
723 
724  // Scan the array for a first unlabeled quad
725  for (size_t i = 0; i < quad_count; i++)
726  {
727  if (quad[i]->count < 0 || quad[i]->group_idx >= 0) continue;
728 
729  // Recursively find a group of connected quads starting from the seed
730  // quad[i]
731  CvCBQuad::Ptr& q = quad[i];
732 
733  std::stack<CvCBQuad::Ptr> seqStack;
734 
735  seqStack.push(q); // cvSeqPush( stack, &q );
736 
737  q->group_idx = group_idx;
738  out_group.push_back(q); // out_group[count++] = q;
739 
740  while (!seqStack.empty())
741  {
742  q = seqStack.top();
743  seqStack.pop(); // cvSeqPop( stack, &q );
744 
745  for (size_t k = 0; k < 4; k++)
746  {
747  CvCBQuad::Ptr& neighbor = q->neighbors[k];
748 
749  // If he neighbor exists and the neighbor has more than 0
750  // neighbors and the neighbor has not been classified yet.
751  if (neighbor && neighbor->count > 0 && neighbor->group_idx < 0)
752  {
753  neighbor->group_idx = group_idx;
754  seqStack.push(neighbor); // cvSeqPush( stack, &neighbor );
755  out_group.push_back(
756  neighbor); // out_group[count++] = neighbor;
757  }
758  }
759  }
760 
761  break;
762  }
763 }
764 
765 //===========================================================================
766 // LABEL CORNER WITH ROW AND COLUMN //DONE
767 //===========================================================================
768 void mrLabelQuadGroup(
769  std::vector<CvCBQuad::Ptr>& quad_group, const CvSize& pattern_size,
770  bool firstRun)
771 {
772  const size_t count = quad_group.size();
773 
774  // If this is the first function call, a seed quad needs to be selected
775  if (firstRun == true)
776  {
777  // Search for the (first) quad with the maximum number of neighbors
778  // (usually 4). This will be our starting point.
779  int max_id = -1;
780  int max_number = -1;
781  for (size_t i = 0; i < count; i++)
782  {
783  CvCBQuad* q = quad_group[i].get();
784  if (q->count > max_number)
785  {
786  max_number = q->count;
787  max_id = i;
788 
789  if (max_number == 4) break;
790  }
791  }
792 
793  // Mark the starting quad's (per definition) upper left corner with
794  //(0,0) and then proceed clockwise
795  // The following labeling sequence enshures a "right coordinate system"
796  CvCBQuad* q = quad_group[max_id].get();
797 
798  q->labeled = true;
799  q->corners[0]->row = 0;
800  q->corners[0]->column = 0;
801  q->corners[1]->row = 0;
802  q->corners[1]->column = 1;
803  q->corners[2]->row = 1;
804  q->corners[2]->column = 1;
805  q->corners[3]->row = 1;
806  q->corners[3]->column = 0;
807  }
808 
809  // Mark all other corners with their respective row and column
810  bool flag_changed = true;
811  while (flag_changed == true)
812  {
813  // First reset the flag to "false"
814  flag_changed = false;
815 
816  // Go through all quads top down is faster, since unlabeled quads will
817  // be inserted at the end of the list
818  for (int i = int(count - 1); i >= 0; i--)
819  {
820  // Check whether quad "i" has been labeled already
821  if ((quad_group[i])->labeled == false)
822  {
823  // Check its neighbors, whether some of them have been labeled
824  // already
825  for (size_t j = 0; j < 4; j++)
826  {
827  // Check whether the neighbor exists (i.e. is not the NULL
828  // pointer)
829  if ((quad_group[i])->neighbors[j])
830  {
831  CvCBQuad::Ptr& quadNeighborJ =
832  quad_group[i]->neighbors[j];
833 
834  // Only proceed, if neighbor "j" was labeled
835  if (quadNeighborJ->labeled == true)
836  {
837  // For every quad it could happen to pass here
838  // multiple times. We therefore "break" later.
839  // Check whitch of the neighbors corners is
840  // connected to the current quad
841  int connectedNeighborCornerId = -1;
842  for (int k = 0; k < 4; k++)
843  {
844  if (quadNeighborJ->neighbors[k] ==
845  quad_group[i])
846  {
847  connectedNeighborCornerId = k;
848 
849  // there is only one, therefore
850  break;
851  }
852  }
853 
854  // For the following calculations we need the row
855  // and column of the connected neighbor corner and
856  // all other corners of the connected quad "j",
857  // clockwise (CW)
858  CvCBCorner::Ptr& conCorner =
859  quadNeighborJ
860  ->corners[connectedNeighborCornerId];
861  CvCBCorner::Ptr& conCornerCW1 =
862  quadNeighborJ->corners
863  [(connectedNeighborCornerId + 1) % 4];
864  CvCBCorner::Ptr& conCornerCW2 =
865  quadNeighborJ->corners
866  [(connectedNeighborCornerId + 2) % 4];
867  CvCBCorner::Ptr& conCornerCW3 =
868  quadNeighborJ->corners
869  [(connectedNeighborCornerId + 3) % 4];
870 
871  (quad_group[i])->corners[j]->row = conCorner->row;
872  (quad_group[i])->corners[j]->column =
873  conCorner->column;
874  (quad_group[i])->corners[(j + 1) % 4]->row =
875  conCorner->row - conCornerCW2->row +
876  conCornerCW3->row;
877  (quad_group[i])->corners[(j + 1) % 4]->column =
878  conCorner->column - conCornerCW2->column +
879  conCornerCW3->column;
880  (quad_group[i])->corners[(j + 2) % 4]->row =
881  conCorner->row + conCorner->row -
882  conCornerCW2->row;
883  (quad_group[i])->corners[(j + 2) % 4]->column =
884  conCorner->column + conCorner->column -
885  conCornerCW2->column;
886  (quad_group[i])->corners[(j + 3) % 4]->row =
887  conCorner->row - conCornerCW2->row +
888  conCornerCW1->row;
889  (quad_group[i])->corners[(j + 3) % 4]->column =
890  conCorner->column - conCornerCW2->column +
891  conCornerCW1->column;
892 
893  // Mark this quad as labeled
894  (quad_group[i])->labeled = true;
895 
896  // Changes have taken place, set the flag
897  flag_changed = true;
898 
899  // once is enough!
900  break;
901  }
902  }
903  }
904  }
905  }
906  }
907 
908  // All corners are marked with row and column
909  // Record the minimal and maximal row and column indices
910  // It is unlikely that more than 8bit checkers are used per dimension, if
911  // there are
912  // an error would have been thrown at the beginning of
913  // "cvFindChessboardCorners2"
914  int min_row = 127;
915  int max_row = -127;
916  int min_column = 127;
917  int max_column = -127;
918 
919  for (size_t i = 0; i < count; i++)
920  {
921  const CvCBQuad::Ptr& q = quad_group[i];
922 
923  for (size_t j = 0; j < 4; j++)
924  {
925  if ((q->corners[j])->row > max_row) max_row = (q->corners[j])->row;
926 
927  if ((q->corners[j])->row < min_row) min_row = (q->corners[j])->row;
928 
929  if ((q->corners[j])->column > max_column)
930  max_column = (q->corners[j])->column;
931 
932  if ((q->corners[j])->column < min_column)
933  min_column = (q->corners[j])->column;
934  }
935  }
936 
937  // Label all internal corners with "needsNeighbor" = false
938  // Label all external corners with "needsNeighbor" = true,
939  // except if in a given dimension the pattern size is reached
940  for (int i = min_row; i <= max_row; i++)
941  {
942  for (int j = min_column; j <= max_column; j++)
943  {
944  // A flag that indicates, wheter a row/column combination is
945  // executed multiple times
946  bool flagg = false;
947 
948  // Remember corner and quad
949  int cornerID = 0;
950  int quadID = 0;
951 
952  for (size_t k = 0; k < count; k++)
953  {
954  for (size_t l = 0; l < 4; l++)
955  {
956  if (((quad_group[k])->corners[l]->row == i) &&
957  ((quad_group[k])->corners[l]->column == j))
958  {
959  if (flagg == true)
960  {
961  // Passed at least twice through here
962  (quad_group[k])->corners[l]->needsNeighbor = false;
963  (quad_group[quadID])
964  ->corners[cornerID]
965  ->needsNeighbor = false;
966  }
967  else
968  {
969  // Mark with needs a neighbor, but note the
970  // address
971  (quad_group[k])->corners[l]->needsNeighbor = true;
972  cornerID = l;
973  quadID = k;
974  }
975 
976  // set the flag to true
977  flagg = true;
978  }
979  }
980  }
981  }
982  }
983 
984  // Complete Linking:
985  // sometimes not all corners were properly linked in "mrFindQuadNeighbors2",
986  // but after labeling each corner with its respective row and column, it is
987  // possible to match them anyway.
988  for (int i = min_row; i <= max_row; i++)
989  {
990  for (int j = min_column; j <= max_column; j++)
991  {
992  // the following "number" indicates the number of corners which
993  // correspond to the given (i,j) value
994  // 1 is a border corner or a conrer which still needs a neighbor
995  // 2 is a fully connected internal corner
996  // >2 something went wrong during labeling, report a warning
997  int number = 1;
998 
999  // remember corner and quad
1000  int cornerID = 0;
1001  int quadID = 0;
1002 
1003  for (size_t k = 0; k < count; k++)
1004  {
1005  for (size_t l = 0; l < 4; l++)
1006  {
1007  if (((quad_group[k])->corners[l]->row == i) &&
1008  ((quad_group[k])->corners[l]->column == j))
1009  {
1010  if (number == 1)
1011  {
1012  // First corner, note its ID
1013  cornerID = l;
1014  quadID = k;
1015  }
1016 
1017  else if (number == 2)
1018  {
1019  // Second corner, check wheter this and the
1020  // first one have equal coordinates, else
1021  // interpolate
1022  float delta_x =
1023  (quad_group[k])->corners[l]->pt.x -
1024  (quad_group[quadID])->corners[cornerID]->pt.x;
1025  float delta_y =
1026  (quad_group[k])->corners[l]->pt.y -
1027  (quad_group[quadID])->corners[cornerID]->pt.y;
1028 
1029  if (delta_x != 0 || delta_y != 0)
1030  {
1031  // Interpolate
1032  (quad_group[k])->corners[l]->pt.x =
1033  (quad_group[k])->corners[l]->pt.x -
1034  delta_x / 2;
1035  (quad_group[quadID])->corners[cornerID]->pt.x =
1036  (quad_group[quadID])
1037  ->corners[cornerID]
1038  ->pt.x +
1039  delta_x / 2;
1040  (quad_group[k])->corners[l]->pt.y =
1041  (quad_group[k])->corners[l]->pt.y -
1042  delta_y / 2;
1043  (quad_group[quadID])->corners[cornerID]->pt.y =
1044  (quad_group[quadID])
1045  ->corners[cornerID]
1046  ->pt.y +
1047  delta_y / 2;
1048  }
1049  }
1050  else if (number > 2)
1051  {
1052  // Something went wrong during row/column labeling
1053  // Report a Warning
1054  // ->Implemented in the function "mrWriteCorners"
1055  }
1056 
1057  // increase the number by one
1058  number = number + 1;
1059  }
1060  }
1061  }
1062  }
1063  }
1064 
1065  // Bordercorners don't need any neighbors, if the pattern size in the
1066  // respective direction is reached
1067  // The only time we can make shure that the target pattern size is reached
1068  // in a given
1069  // dimension, is when the larger side has reached the target size in the
1070  // maximal
1071  // direction, or if the larger side is larger than the smaller target size
1072  // and the
1073  // smaller side equals the smaller target size
1074  int largerDimPattern = max(pattern_size.height, pattern_size.width);
1075  int smallerDimPattern = min(pattern_size.height, pattern_size.width);
1076  bool flagSmallerDim1 = false;
1077  bool flagSmallerDim2 = false;
1078 
1079  if ((largerDimPattern + 1) == max_column - min_column)
1080  {
1081  flagSmallerDim1 = true;
1082  // We found out that in the column direction the target pattern size is
1083  // reached
1084  // Therefore border column corners do not need a neighbor anymore
1085  // Go through all corners
1086  for (size_t k = 0; k < count; k++)
1087  {
1088  for (size_t l = 0; l < 4; l++)
1089  {
1090  if ((quad_group[k])->corners[l]->column == min_column ||
1091  (quad_group[k])->corners[l]->column == max_column)
1092  {
1093  // Needs no neighbor anymore
1094  (quad_group[k])->corners[l]->needsNeighbor = false;
1095  }
1096  }
1097  }
1098  }
1099 
1100  if ((largerDimPattern + 1) == max_row - min_row)
1101  {
1102  flagSmallerDim2 = true;
1103  // We found out that in the column direction the target pattern size is
1104  // reached
1105  // Therefore border column corners do not need a neighbor anymore
1106  // Go through all corners
1107  for (size_t k = 0; k < count; k++)
1108  {
1109  for (size_t l = 0; l < 4; l++)
1110  {
1111  if ((quad_group[k])->corners[l]->row == min_row ||
1112  (quad_group[k])->corners[l]->row == max_row)
1113  {
1114  // Needs no neighbor anymore
1115  (quad_group[k])->corners[l]->needsNeighbor = false;
1116  }
1117  }
1118  }
1119  }
1120 
1121  // Check the two flags:
1122  // - If one is true and the other false, then the pattern target
1123  // size was reached in in one direction -> We can check, whether the
1124  // target
1125  // pattern size is also reached in the other direction
1126  // - If both are set to true, then we deal with a square board -> do
1127  // nothing
1128  // - If both are set to false -> There is a possibility that the larger
1129  // side is
1130  // larger than the smaller target size -> Check and if true, then check
1131  // whether
1132  // the other side has the same size as the smaller target size
1133  if ((flagSmallerDim1 == false && flagSmallerDim2 == true))
1134  {
1135  // Larger target pattern size is in row direction, check wheter smaller
1136  // target
1137  // pattern size is reached in column direction
1138  if ((smallerDimPattern + 1) == max_column - min_column)
1139  {
1140  for (size_t k = 0; k < count; k++)
1141  {
1142  for (int l = 0; l < 4; l++)
1143  {
1144  if ((quad_group[k])->corners[l]->column == min_column ||
1145  (quad_group[k])->corners[l]->column == max_column)
1146  {
1147  // Needs no neighbor anymore
1148  (quad_group[k])->corners[l]->needsNeighbor = false;
1149  }
1150  }
1151  }
1152  }
1153  }
1154 
1155  if ((flagSmallerDim1 == true && flagSmallerDim2 == false))
1156  {
1157  // Larger target pattern size is in column direction, check wheter
1158  // smaller target
1159  // pattern size is reached in row direction
1160  if ((smallerDimPattern + 1) == max_row - min_row)
1161  {
1162  for (size_t k = 0; k < count; k++)
1163  {
1164  for (size_t l = 0; l < 4; l++)
1165  {
1166  if ((quad_group[k])->corners[l]->row == min_row ||
1167  (quad_group[k])->corners[l]->row == max_row)
1168  {
1169  // Needs no neighbor anymore
1170  (quad_group[k])->corners[l]->needsNeighbor = false;
1171  }
1172  }
1173  }
1174  }
1175  }
1176 
1177  if ((flagSmallerDim1 == false && flagSmallerDim2 == false) &&
1178  smallerDimPattern + 1 < max_column - min_column)
1179  {
1180  // Larger target pattern size is in column direction, check wheter
1181  // smaller target
1182  // pattern size is reached in row direction
1183  if ((smallerDimPattern + 1) == max_row - min_row)
1184  {
1185  for (size_t k = 0; k < count; k++)
1186  {
1187  for (size_t l = 0; l < 4; l++)
1188  {
1189  if ((quad_group[k])->corners[l]->row == min_row ||
1190  (quad_group[k])->corners[l]->row == max_row)
1191  {
1192  // Needs no neighbor anymore
1193  (quad_group[k])->corners[l]->needsNeighbor = false;
1194  }
1195  }
1196  }
1197  }
1198  }
1199 
1200  if ((flagSmallerDim1 == false && flagSmallerDim2 == false) &&
1201  smallerDimPattern + 1 < max_row - min_row)
1202  {
1203  // Larger target pattern size is in row direction, check wheter smaller
1204  // target
1205  // pattern size is reached in column direction
1206  if ((smallerDimPattern + 1) == max_column - min_column)
1207  {
1208  for (size_t k = 0; k < count; k++)
1209  {
1210  for (size_t l = 0; l < 4; l++)
1211  {
1212  if ((quad_group[k])->corners[l]->column == min_column ||
1213  (quad_group[k])->corners[l]->column == max_column)
1214  {
1215  // Needs no neighbor anymore
1216  (quad_group[k])->corners[l]->needsNeighbor = false;
1217  }
1218  }
1219  }
1220  }
1221  }
1222 }
1223 
1224 //===========================================================================
1225 // GIVE A GROUP IDX
1226 //===========================================================================
1227 // This function replaces mrFindQuadNeighbors, which in turn replaced
1228 // icvFindQuadNeighbors
1229 void mrFindQuadNeighbors2(std::vector<CvCBQuad::Ptr>& quads, int dilation)
1230 {
1231  // Thresh dilation is used to counter the effect of dilation on the
1232  // distance between 2 neighboring corners. Since the distance below is
1233  // computed as its square, we do here the same. Additionally, we take the
1234  // conservative assumption that dilation was performed using the 3x3 CROSS
1235  // kernel, which coresponds to the 4-neighborhood.
1236  const float thresh_dilation = (float)(2 * dilation + 3) *
1237  (2 * dilation + 3) *
1238  2; // the "*2" is for the x and y component
1239  float dx, dy, dist;
1240  // int cur_quad_group = -1;
1241 
1242  const size_t quad_count = quads.size();
1243 
1244  // Find quad neighbors
1245  for (size_t idx = 0; idx < quad_count; idx++)
1246  {
1247  CvCBQuad::Ptr& cur_quad = quads[idx];
1248 
1249  // Go through all quadrangles and label them in groups
1250  // For each corner of this quadrangle
1251  for (size_t i = 0; i < 4; i++)
1252  {
1253  CvPoint2D32f pt;
1254  float min_dist = FLT_MAX;
1255  int closest_corner_idx = -1;
1256  CvCBQuad::Ptr closest_quad;
1257 
1258  if (cur_quad->neighbors[i]) continue;
1259 
1260  pt = cur_quad->corners[i]->pt;
1261 
1262  // Find the closest corner in all other quadrangles
1263  for (size_t k = 0; k < quad_count; k++)
1264  {
1265  if (k == idx) continue;
1266 
1267  for (size_t j = 0; j < 4; j++)
1268  {
1269  // If it already has a neighbor
1270  if (quads[k]->neighbors[j]) continue;
1271 
1272  dx = pt.x - quads[k]->corners[j]->pt.x;
1273  dy = pt.y - quads[k]->corners[j]->pt.y;
1274  dist = dx * dx + dy * dy;
1275 
1276  // The following "if" checks, whether "dist" is the
1277  // shortest so far and smaller than the smallest
1278  // edge length of the current and target quads
1279  if (dist < min_dist &&
1280  dist <= (cur_quad->edge_len + thresh_dilation) &&
1281  dist <= (quads[k]->edge_len + thresh_dilation))
1282  {
1283  // First Check everything from the viewpoint of the
1284  // current quad
1285  // compute midpoints of "parallel" quad sides 1
1286  float x1 = (cur_quad->corners[i]->pt.x +
1287  cur_quad->corners[(i + 1) % 4]->pt.x) /
1288  2;
1289  float y1 = (cur_quad->corners[i]->pt.y +
1290  cur_quad->corners[(i + 1) % 4]->pt.y) /
1291  2;
1292  float x2 = (cur_quad->corners[(i + 2) % 4]->pt.x +
1293  cur_quad->corners[(i + 3) % 4]->pt.x) /
1294  2;
1295  float y2 = (cur_quad->corners[(i + 2) % 4]->pt.y +
1296  cur_quad->corners[(i + 3) % 4]->pt.y) /
1297  2;
1298  // compute midpoints of "parallel" quad sides 2
1299  float x3 = (cur_quad->corners[i]->pt.x +
1300  cur_quad->corners[(i + 3) % 4]->pt.x) /
1301  2;
1302  float y3 = (cur_quad->corners[i]->pt.y +
1303  cur_quad->corners[(i + 3) % 4]->pt.y) /
1304  2;
1305  float x4 = (cur_quad->corners[(i + 1) % 4]->pt.x +
1306  cur_quad->corners[(i + 2) % 4]->pt.x) /
1307  2;
1308  float y4 = (cur_quad->corners[(i + 1) % 4]->pt.y +
1309  cur_quad->corners[(i + 2) % 4]->pt.y) /
1310  2;
1311 
1312  // MARTIN: Heuristic
1313  // For the corner "j" of quad "k" to be considered,
1314  // it needs to be on the same side of the two lines as
1315  // corner "i". This is given, if the cross product has
1316  // the same sign for both computations below:
1317  float a1 = x1 - x2;
1318  float b1 = y1 - y2;
1319  // the current corner
1320  float c11 = cur_quad->corners[i]->pt.x - x2;
1321  float d11 = cur_quad->corners[i]->pt.y - y2;
1322  // the candidate corner
1323  float c12 = quads[k]->corners[j]->pt.x - x2;
1324  float d12 = quads[k]->corners[j]->pt.y - y2;
1325  float sign11 = a1 * d11 - c11 * b1;
1326  float sign12 = a1 * d12 - c12 * b1;
1327 
1328  float a2 = x3 - x4;
1329  float b2 = y3 - y4;
1330  // the current corner
1331  float c21 = cur_quad->corners[i]->pt.x - x4;
1332  float d21 = cur_quad->corners[i]->pt.y - y4;
1333  // the candidate corner
1334  float c22 = quads[k]->corners[j]->pt.x - x4;
1335  float d22 = quads[k]->corners[j]->pt.y - y4;
1336  float sign21 = a2 * d21 - c21 * b2;
1337  float sign22 = a2 * d22 - c22 * b2;
1338 
1339  // Then make shure that two border quads of the same row
1340  // or
1341  // column don't link. Check from the current corner's
1342  // view,
1343  // whether the corner diagonal from the candidate corner
1344  // is also on the same side of the two lines as the
1345  // current
1346  // corner and the candidate corner.
1347  float c13 = quads[k]->corners[(j + 2) % 4]->pt.x - x2;
1348  float d13 = quads[k]->corners[(j + 2) % 4]->pt.y - y2;
1349  float c23 = quads[k]->corners[(j + 2) % 4]->pt.x - x4;
1350  float d23 = quads[k]->corners[(j + 2) % 4]->pt.y - y4;
1351  float sign13 = a1 * d13 - c13 * b1;
1352  float sign23 = a2 * d23 - c23 * b2;
1353 
1354  // Then check everything from the viewpoint of the
1355  // candidate quad
1356  // compute midpoints of "parallel" quad sides 1
1357  float u1 = (quads[k]->corners[j]->pt.x +
1358  quads[k]->corners[(j + 1) % 4]->pt.x) /
1359  2;
1360  float v1 = (quads[k]->corners[j]->pt.y +
1361  quads[k]->corners[(j + 1) % 4]->pt.y) /
1362  2;
1363  float u2 = (quads[k]->corners[(j + 2) % 4]->pt.x +
1364  quads[k]->corners[(j + 3) % 4]->pt.x) /
1365  2;
1366  float v2 = (quads[k]->corners[(j + 2) % 4]->pt.y +
1367  quads[k]->corners[(j + 3) % 4]->pt.y) /
1368  2;
1369  // compute midpoints of "parallel" quad sides 2
1370  float u3 = (quads[k]->corners[j]->pt.x +
1371  quads[k]->corners[(j + 3) % 4]->pt.x) /
1372  2;
1373  float v3 = (quads[k]->corners[j]->pt.y +
1374  quads[k]->corners[(j + 3) % 4]->pt.y) /
1375  2;
1376  float u4 = (quads[k]->corners[(j + 1) % 4]->pt.x +
1377  quads[k]->corners[(j + 2) % 4]->pt.x) /
1378  2;
1379  float v4 = (quads[k]->corners[(j + 1) % 4]->pt.y +
1380  quads[k]->corners[(j + 2) % 4]->pt.y) /
1381  2;
1382 
1383  // MARTIN: Heuristic
1384  // for the corner "j" of quad "k" to be considered, it
1385  // needs to be on the same side of the two lines as
1386  // corner "i". This is again given, if the cross
1387  // product has the same sign for both computations
1388  // below:
1389  float a3 = u1 - u2;
1390  float b3 = v1 - v2;
1391  // the current corner
1392  float c31 = cur_quad->corners[i]->pt.x - u2;
1393  float d31 = cur_quad->corners[i]->pt.y - v2;
1394  // the candidate corner
1395  float c32 = quads[k]->corners[j]->pt.x - u2;
1396  float d32 = quads[k]->corners[j]->pt.y - v2;
1397  float sign31 = a3 * d31 - c31 * b3;
1398  float sign32 = a3 * d32 - c32 * b3;
1399 
1400  float a4 = u3 - u4;
1401  float b4 = v3 - v4;
1402  // the current corner
1403  float c41 = cur_quad->corners[i]->pt.x - u4;
1404  float d41 = cur_quad->corners[i]->pt.y - v4;
1405  // the candidate corner
1406  float c42 = quads[k]->corners[j]->pt.x - u4;
1407  float d42 = quads[k]->corners[j]->pt.y - v4;
1408  float sign41 = a4 * d41 - c41 * b4;
1409  float sign42 = a4 * d42 - c42 * b4;
1410 
1411  // Then make shure that two border quads of the same row
1412  // or
1413  // column don't link. Check from the candidate corner's
1414  // view,
1415  // whether the corner diagonal from the current corner
1416  // is also on the same side of the two lines as the
1417  // current
1418  // corner and the candidate corner.
1419  float c33 = cur_quad->corners[(i + 2) % 4]->pt.x - u2;
1420  float d33 = cur_quad->corners[(i + 2) % 4]->pt.y - v2;
1421  float c43 = cur_quad->corners[(i + 2) % 4]->pt.x - u4;
1422  float d43 = cur_quad->corners[(i + 2) % 4]->pt.y - v4;
1423  float sign33 = a3 * d33 - c33 * b3;
1424  float sign43 = a4 * d43 - c43 * b4;
1425 
1426  // Check whether conditions are fulfilled
1427  if (((sign11 < 0 && sign12 < 0) ||
1428  (sign11 > 0 && sign12 > 0)) &&
1429  ((sign21 < 0 && sign22 < 0) ||
1430  (sign21 > 0 && sign22 > 0)) &&
1431  ((sign31 < 0 && sign32 < 0) ||
1432  (sign31 > 0 && sign32 > 0)) &&
1433  ((sign41 < 0 && sign42 < 0) ||
1434  (sign41 > 0 && sign42 > 0)) &&
1435  ((sign11 < 0 && sign13 < 0) ||
1436  (sign11 > 0 && sign13 > 0)) &&
1437  ((sign21 < 0 && sign23 < 0) ||
1438  (sign21 > 0 && sign23 > 0)) &&
1439  ((sign31 < 0 && sign33 < 0) ||
1440  (sign31 > 0 && sign33 > 0)) &&
1441  ((sign41 < 0 && sign43 < 0) ||
1442  (sign41 > 0 && sign43 > 0)))
1443 
1444  {
1445  closest_corner_idx = j;
1446  closest_quad = quads[k];
1447  min_dist = dist;
1448  }
1449  }
1450  }
1451  }
1452 
1453  // Have we found a matching corner point?
1454  if (closest_corner_idx >= 0 && min_dist < FLT_MAX)
1455  {
1456  CvCBCorner::Ptr& closest_corner =
1457  closest_quad->corners[closest_corner_idx];
1458 
1459  // Make shure that the closest quad does not have the current
1460  // quad as neighbor already
1461  bool skip = false;
1462  for (size_t j = 0; !skip && j < 4; j++)
1463  skip = closest_quad->neighbors[j] == cur_quad;
1464 
1465  if (skip) continue;
1466 
1467  // We've found one more corner - remember it
1468  closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1469  closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1470 
1471  cur_quad->count++;
1472  cur_quad->neighbors[i] = closest_quad;
1473  cur_quad->corners[i] = closest_corner;
1474 
1475  closest_quad->count++;
1476  closest_quad->neighbors[closest_corner_idx] = cur_quad;
1477  closest_quad->corners[closest_corner_idx] = closest_corner;
1478  }
1479  }
1480  }
1481 }
1482 
1483 //===========================================================================
1484 // AUGMENT PATTERN WITH ADDITIONAL QUADS
1485 //===========================================================================
1486 // The first part of the function is basically a copy of
1487 // "mrFindQuadNeighbors2"
1488 // The comparisons between two points and two lines could be computed in their
1489 // own function
1490 int mrAugmentBestRun(
1491  std::vector<CvCBQuad::Ptr>& new_quads, int new_dilation,
1492  std::vector<CvCBQuad::Ptr>& old_quads, int old_dilation)
1493 {
1494  // thresh dilation is used to counter the effect of dilation on the
1495  // distance between 2 neighboring corners. Since the distance below is
1496  // computed as its square, we do here the same. Additionally, we take the
1497  // conservative assumption that dilation was performed using the 3x3 CROSS
1498  // kernel, which coresponds to the 4-neighborhood.
1499  //
1500  // the "*2" is for the x and y component
1501  // int idx, i, k, j; // the "3" is for initial corner mismatch
1502  const float thresh_dilation =
1503  (float)(2 * new_dilation + 3) * (2 * old_dilation + 3) * 2;
1504  float dx, dy, dist;
1505 
1506  // Search all old quads which have a neighbor that needs to be linked
1507  for (size_t idx = 0; idx < old_quads.size(); idx++)
1508  {
1509  CvCBQuad::Ptr cur_quad = old_quads[idx];
1510 
1511  // For each corner of this quadrangle
1512  for (int i = 0; i < 4; i++)
1513  {
1514  CvPoint2D32f pt;
1515  float min_dist = FLT_MAX;
1516  int closest_corner_idx = -1;
1517  CvCBQuad::Ptr closest_quad;
1518  // CvCBCorner *closest_corner = 0;
1519 
1520  // If cur_quad corner[i] is already linked, continue
1521  if (cur_quad->corners[i]->needsNeighbor == false) continue;
1522 
1523  pt = cur_quad->corners[i]->pt;
1524 
1525  // Look for a match in all new_quads' corners
1526  for (size_t k = 0; k < new_quads.size(); k++)
1527  {
1528  // Only look at unlabeled new quads
1529  if (new_quads[k]->labeled == true) continue;
1530 
1531  for (int j = 0; j < 4; j++)
1532  {
1533  // Only proceed if they are less than dist away from each
1534  // other
1535  dx = pt.x - new_quads[k]->corners[j]->pt.x;
1536  dy = pt.y - new_quads[k]->corners[j]->pt.y;
1537  dist = dx * dx + dy * dy;
1538 
1539  if ((dist < min_dist) &&
1540  dist <= (cur_quad->edge_len + thresh_dilation) &&
1541  dist <= (new_quads[k]->edge_len + thresh_dilation))
1542  {
1543  // First Check everything from the viewpoint of the
1544  // current quad compute midpoints of "parallel" quad
1545  // sides 1
1546  float x1 = (cur_quad->corners[i]->pt.x +
1547  cur_quad->corners[(i + 1) % 4]->pt.x) /
1548  2;
1549  float y1 = (cur_quad->corners[i]->pt.y +
1550  cur_quad->corners[(i + 1) % 4]->pt.y) /
1551  2;
1552  float x2 = (cur_quad->corners[(i + 2) % 4]->pt.x +
1553  cur_quad->corners[(i + 3) % 4]->pt.x) /
1554  2;
1555  float y2 = (cur_quad->corners[(i + 2) % 4]->pt.y +
1556  cur_quad->corners[(i + 3) % 4]->pt.y) /
1557  2;
1558  // compute midpoints of "parallel" quad sides 2
1559  float x3 = (cur_quad->corners[i]->pt.x +
1560  cur_quad->corners[(i + 3) % 4]->pt.x) /
1561  2;
1562  float y3 = (cur_quad->corners[i]->pt.y +
1563  cur_quad->corners[(i + 3) % 4]->pt.y) /
1564  2;
1565  float x4 = (cur_quad->corners[(i + 1) % 4]->pt.x +
1566  cur_quad->corners[(i + 2) % 4]->pt.x) /
1567  2;
1568  float y4 = (cur_quad->corners[(i + 1) % 4]->pt.y +
1569  cur_quad->corners[(i + 2) % 4]->pt.y) /
1570  2;
1571 
1572  // MARTIN: Heuristic
1573  // For the corner "j" of quad "k" to be considered,
1574  // it needs to be on the same side of the two lines as
1575  // corner "i". This is given, if the cross product has
1576  // the same sign for both computations below:
1577  float a1 = x1 - x2;
1578  float b1 = y1 - y2;
1579  // the current corner
1580  float c11 = cur_quad->corners[i]->pt.x - x2;
1581  float d11 = cur_quad->corners[i]->pt.y - y2;
1582  // the candidate corner
1583  float c12 = new_quads[k]->corners[j]->pt.x - x2;
1584  float d12 = new_quads[k]->corners[j]->pt.y - y2;
1585  float sign11 = a1 * d11 - c11 * b1;
1586  float sign12 = a1 * d12 - c12 * b1;
1587 
1588  float a2 = x3 - x4;
1589  float b2 = y3 - y4;
1590  // the current corner
1591  float c21 = cur_quad->corners[i]->pt.x - x4;
1592  float d21 = cur_quad->corners[i]->pt.y - y4;
1593  // the candidate corner
1594  float c22 = new_quads[k]->corners[j]->pt.x - x4;
1595  float d22 = new_quads[k]->corners[j]->pt.y - y4;
1596  float sign21 = a2 * d21 - c21 * b2;
1597  float sign22 = a2 * d22 - c22 * b2;
1598 
1599  // Also make shure that two border quads of the same row
1600  // or
1601  // column don't link. Check from the current corner's
1602  // view,
1603  // whether the corner diagonal from the candidate corner
1604  // is also on the same side of the two lines as the
1605  // current
1606  // corner and the candidate corner.
1607  float c13 =
1608  new_quads[k]->corners[(j + 2) % 4]->pt.x - x2;
1609  float d13 =
1610  new_quads[k]->corners[(j + 2) % 4]->pt.y - y2;
1611  float c23 =
1612  new_quads[k]->corners[(j + 2) % 4]->pt.x - x4;
1613  float d23 =
1614  new_quads[k]->corners[(j + 2) % 4]->pt.y - y4;
1615  float sign13 = a1 * d13 - c13 * b1;
1616  float sign23 = a2 * d23 - c23 * b2;
1617 
1618  // Second: Then check everything from the viewpoint of
1619  // the candidate quad. Compute midpoints of "parallel"
1620  // quad sides 1
1621  float u1 = (new_quads[k]->corners[j]->pt.x +
1622  new_quads[k]->corners[(j + 1) % 4]->pt.x) /
1623  2;
1624  float v1 = (new_quads[k]->corners[j]->pt.y +
1625  new_quads[k]->corners[(j + 1) % 4]->pt.y) /
1626  2;
1627  float u2 = (new_quads[k]->corners[(j + 2) % 4]->pt.x +
1628  new_quads[k]->corners[(j + 3) % 4]->pt.x) /
1629  2;
1630  float v2 = (new_quads[k]->corners[(j + 2) % 4]->pt.y +
1631  new_quads[k]->corners[(j + 3) % 4]->pt.y) /
1632  2;
1633  // compute midpoints of "parallel" quad sides 2
1634  float u3 = (new_quads[k]->corners[j]->pt.x +
1635  new_quads[k]->corners[(j + 3) % 4]->pt.x) /
1636  2;
1637  float v3 = (new_quads[k]->corners[j]->pt.y +
1638  new_quads[k]->corners[(j + 3) % 4]->pt.y) /
1639  2;
1640  float u4 = (new_quads[k]->corners[(j + 1) % 4]->pt.x +
1641  new_quads[k]->corners[(j + 2) % 4]->pt.x) /
1642  2;
1643  float v4 = (new_quads[k]->corners[(j + 1) % 4]->pt.y +
1644  new_quads[k]->corners[(j + 2) % 4]->pt.y) /
1645  2;
1646 
1647  // MARTIN: Heuristic
1648  // For the corner "j" of quad "k" to be considered,
1649  // it needs to be on the same side of the two lines as
1650  // corner "i". This is given, if the cross product has
1651  // the same sign for both computations below:
1652  float a3 = u1 - u2;
1653  float b3 = v1 - v2;
1654  // the current corner
1655  float c31 = cur_quad->corners[i]->pt.x - u2;
1656  float d31 = cur_quad->corners[i]->pt.y - v2;
1657  // the candidate corner
1658  float c32 = new_quads[k]->corners[j]->pt.x - u2;
1659  float d32 = new_quads[k]->corners[j]->pt.y - v2;
1660  float sign31 = a3 * d31 - c31 * b3;
1661  float sign32 = a3 * d32 - c32 * b3;
1662 
1663  float a4 = u3 - u4;
1664  float b4 = v3 - v4;
1665  // the current corner
1666  float c41 = cur_quad->corners[i]->pt.x - u4;
1667  float d41 = cur_quad->corners[i]->pt.y - v4;
1668  // the candidate corner
1669  float c42 = new_quads[k]->corners[j]->pt.x - u4;
1670  float d42 = new_quads[k]->corners[j]->pt.y - v4;
1671  float sign41 = a4 * d41 - c41 * b4;
1672  float sign42 = a4 * d42 - c42 * b4;
1673 
1674  // Also make shure that two border quads of the same row
1675  // or
1676  // column don't link. Check from the candidate corner's
1677  // view,
1678  // whether the corner diagonal from the current corner
1679  // is also on the same side of the two lines as the
1680  // current
1681  // corner and the candidate corner.
1682  float c33 = cur_quad->corners[(i + 2) % 4]->pt.x - u2;
1683  float d33 = cur_quad->corners[(i + 2) % 4]->pt.y - v2;
1684  float c43 = cur_quad->corners[(i + 2) % 4]->pt.x - u4;
1685  float d43 = cur_quad->corners[(i + 2) % 4]->pt.y - v4;
1686  float sign33 = a3 * d33 - c33 * b3;
1687  float sign43 = a4 * d43 - c43 * b4;
1688 
1689  // This time we also need to make shure, that no quad
1690  // is linked to a quad of another dilation run which
1691  // may lie INSIDE it!!!
1692  // Third: Therefore check everything from the viewpoint
1693  // of the current quad compute midpoints of "parallel"
1694  // quad sides 1
1695  float x5 = cur_quad->corners[i]->pt.x;
1696  float y5 = cur_quad->corners[i]->pt.y;
1697  float x6 = cur_quad->corners[(i + 1) % 4]->pt.x;
1698  float y6 = cur_quad->corners[(i + 1) % 4]->pt.y;
1699  // compute midpoints of "parallel" quad sides 2
1700  float x7 = x5;
1701  float y7 = y5;
1702  float x8 = cur_quad->corners[(i + 3) % 4]->pt.x;
1703  float y8 = cur_quad->corners[(i + 3) % 4]->pt.y;
1704 
1705  // MARTIN: Heuristic
1706  // For the corner "j" of quad "k" to be considered,
1707  // it needs to be on the other side of the two lines
1708  // than
1709  // corner "i". This is given, if the cross product has
1710  // a different sign for both computations below:
1711  float a5 = x6 - x5;
1712  float b5 = y6 - y5;
1713  // the current corner
1714  float c51 = cur_quad->corners[(i + 2) % 4]->pt.x - x5;
1715  float d51 = cur_quad->corners[(i + 2) % 4]->pt.y - y5;
1716  // the candidate corner
1717  float c52 = new_quads[k]->corners[j]->pt.x - x5;
1718  float d52 = new_quads[k]->corners[j]->pt.y - y5;
1719  float sign51 = a5 * d51 - c51 * b5;
1720  float sign52 = a5 * d52 - c52 * b5;
1721 
1722  float a6 = x8 - x7;
1723  float b6 = y8 - y7;
1724  // the current corner
1725  float c61 = cur_quad->corners[(i + 2) % 4]->pt.x - x7;
1726  float d61 = cur_quad->corners[(i + 2) % 4]->pt.y - y7;
1727  // the candidate corner
1728  float c62 = new_quads[k]->corners[j]->pt.x - x7;
1729  float d62 = new_quads[k]->corners[j]->pt.y - y7;
1730  float sign61 = a6 * d61 - c61 * b6;
1731  float sign62 = a6 * d62 - c62 * b6;
1732 
1733  // Fourth: Then check everything from the viewpoint of
1734  // the candidate quad compute midpoints of "parallel"
1735  // quad sides 1
1736  float u5 = new_quads[k]->corners[j]->pt.x;
1737  float v5 = new_quads[k]->corners[j]->pt.y;
1738  float u6 = new_quads[k]->corners[(j + 1) % 4]->pt.x;
1739  float v6 = new_quads[k]->corners[(j + 1) % 4]->pt.y;
1740  // compute midpoints of "parallel" quad sides 2
1741  float u7 = u5;
1742  float v7 = v5;
1743  float u8 = new_quads[k]->corners[(j + 3) % 4]->pt.x;
1744  float v8 = new_quads[k]->corners[(j + 3) % 4]->pt.y;
1745 
1746  // MARTIN: Heuristic
1747  // For the corner "j" of quad "k" to be considered,
1748  // it needs to be on the other side of the two lines
1749  // than
1750  // corner "i". This is given, if the cross product has
1751  // a different sign for both computations below:
1752  float a7 = u6 - u5;
1753  float b7 = v6 - v5;
1754  // the current corner
1755  float c71 = cur_quad->corners[i]->pt.x - u5;
1756  float d71 = cur_quad->corners[i]->pt.y - v5;
1757  // the candidate corner
1758  float c72 =
1759  new_quads[k]->corners[(j + 2) % 4]->pt.x - u5;
1760  float d72 =
1761  new_quads[k]->corners[(j + 2) % 4]->pt.y - v5;
1762  float sign71 = a7 * d71 - c71 * b7;
1763  float sign72 = a7 * d72 - c72 * b7;
1764 
1765  float a8 = u8 - u7;
1766  float b8 = v8 - v7;
1767  // the current corner
1768  float c81 = cur_quad->corners[i]->pt.x - u7;
1769  float d81 = cur_quad->corners[i]->pt.y - v7;
1770  // the candidate corner
1771  float c82 =
1772  new_quads[k]->corners[(j + 2) % 4]->pt.x - u7;
1773  float d82 =
1774  new_quads[k]->corners[(j + 2) % 4]->pt.y - v7;
1775  float sign81 = a8 * d81 - c81 * b8;
1776  float sign82 = a8 * d82 - c82 * b8;
1777 
1778  // Check whether conditions are fulfilled
1779  if (((sign11 < 0 && sign12 < 0) ||
1780  (sign11 > 0 && sign12 > 0)) &&
1781  ((sign21 < 0 && sign22 < 0) ||
1782  (sign21 > 0 && sign22 > 0)) &&
1783  ((sign31 < 0 && sign32 < 0) ||
1784  (sign31 > 0 && sign32 > 0)) &&
1785  ((sign41 < 0 && sign42 < 0) ||
1786  (sign41 > 0 && sign42 > 0)) &&
1787  ((sign11 < 0 && sign13 < 0) ||
1788  (sign11 > 0 && sign13 > 0)) &&
1789  ((sign21 < 0 && sign23 < 0) ||
1790  (sign21 > 0 && sign23 > 0)) &&
1791  ((sign31 < 0 && sign33 < 0) ||
1792  (sign31 > 0 && sign33 > 0)) &&
1793  ((sign41 < 0 && sign43 < 0) ||
1794  (sign41 > 0 && sign43 > 0)) &&
1795  ((sign51 < 0 && sign52 > 0) ||
1796  (sign51 > 0 && sign52 < 0)) &&
1797  ((sign61 < 0 && sign62 > 0) ||
1798  (sign61 > 0 && sign62 < 0)) &&
1799  ((sign71 < 0 && sign72 > 0) ||
1800  (sign71 > 0 && sign72 < 0)) &&
1801  ((sign81 < 0 && sign82 > 0) ||
1802  (sign81 > 0 && sign82 < 0)))
1803  {
1804  closest_corner_idx = j;
1805  closest_quad = new_quads[k];
1806  min_dist = dist;
1807  }
1808  }
1809  }
1810  }
1811 
1812  // Have we found a matching corner point?
1813  if (closest_corner_idx >= 0 && min_dist < FLT_MAX)
1814  {
1815  CvCBCorner::Ptr& closest_corner =
1816  closest_quad->corners[closest_corner_idx];
1817  closest_corner->pt.x = (pt.x + closest_corner->pt.x) * 0.5f;
1818  closest_corner->pt.y = (pt.y + closest_corner->pt.y) * 0.5f;
1819 
1820  // We've found one more corner - remember it
1821  // ATTENTION: write the corner x and y coordinates separately,
1822  // else the crucial row/column entries will be overwritten !!!
1823  cur_quad->corners[i]->pt.x = closest_corner->pt.x;
1824  cur_quad->corners[i]->pt.y = closest_corner->pt.y;
1825  cur_quad->neighbors[i] = closest_quad;
1826  closest_quad->corners[closest_corner_idx]->pt.x =
1827  closest_corner->pt.x;
1828  closest_quad->corners[closest_corner_idx]->pt.y =
1829  closest_corner->pt.y;
1830 
1831  // Label closest quad as labeled. In this way we exclude it
1832  // being considered again during the next loop iteration
1833  closest_quad->labeled = true;
1834 
1835  // We have a new member of the final pattern, copy it over
1836  CvCBQuad::Ptr newQuad = CvCBQuad::Ptr(new CvCBQuad);
1837 
1838  newQuad->count = 1;
1839  newQuad->edge_len = closest_quad->edge_len;
1840  newQuad->group_idx =
1841  cur_quad->group_idx; // the same as the current quad
1842  newQuad->labeled = false; // do it right afterwards
1843 
1844  // We only know one neighbor for shure, initialize rest with
1845  // the nullptr pointer
1846  newQuad->neighbors[closest_corner_idx] = cur_quad;
1847  newQuad->neighbors[(closest_corner_idx + 1) % 4]
1848  .reset(); // = nullptr;
1849  newQuad->neighbors[(closest_corner_idx + 2) % 4]
1850  .reset(); // = nullptr;
1851  newQuad->neighbors[(closest_corner_idx + 3) % 4]
1852  .reset(); // = nullptr;
1853 
1854  for (int j = 0; j < 4; j++)
1855  {
1856  newQuad->corners[j] = CvCBCorner::Ptr(new CvCBCorner);
1857  newQuad->corners[j]->pt.x = closest_quad->corners[j]->pt.x;
1858  newQuad->corners[j]->pt.y = closest_quad->corners[j]->pt.y;
1859  }
1860 
1861  old_quads.push_back(newQuad);
1862  cur_quad->neighbors[i] = newQuad;
1863 
1864  // Start the function again
1865  return -1;
1866  }
1867  }
1868  }
1869 
1870  // Finished, don't start the function again
1871  return 1;
1872 }
1873 
1874 //===========================================================================
1875 // GENERATE QUADRANGLES
1876 //===========================================================================
1877 int icvGenerateQuads(
1878  vector<CvCBQuad::Ptr>& out_quads, vector<CvCBCorner::Ptr>& out_corners,
1879  const CImage& image, int flags, [[maybe_unused]] int dilation,
1880  bool firstRun)
1881 {
1882  // Initializations
1883  int quad_count = 0;
1884 
1885  // Create temporary storage for contours and the sequence of pointers to
1886  // found quadrangles
1887  cv::MemStorage temp_storage = cv::MemStorage(cvCreateMemStorage(0));
1888 
1889  CvSeq* src_contour = nullptr;
1890  CvSeq* root; // cv::Seq<> root; //
1891  CvContourEx* board = nullptr;
1892  CvContourScanner scanner;
1893 
1894  // Empiric sower bound for the size of allowable quadrangles.
1895  // MARTIN, modified: Added "*0.1" in order to find smaller quads.
1896  const int min_size =
1897  cvRound(image.getWidth() * image.getHeight() * .03 * 0.01 * 0.92 * 0.1);
1898 
1899  root = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq*), temp_storage);
1900 
1901  // Initialize contour retrieving routine
1902  cv::Mat im_mat = image.asCvMat<cv::Mat>(SHALLOW_COPY);
1903  IplImage im_ipl = cvIplImage(im_mat);
1904  scanner = cvStartFindContours(
1905  &im_ipl, temp_storage, sizeof(CvContourEx), CV_RETR_CCOMP,
1906  CV_CHAIN_APPROX_SIMPLE);
1907 
1908  // Get all the contours one by one
1909  while ((src_contour = cvFindNextContour(scanner)) != nullptr)
1910  {
1911  CvSeq* dst_contour = nullptr;
1912  CvRect rect = ((CvContour*)src_contour)->rect;
1913 
1914  // Reject contours with a too small perimeter and contours which are
1915  // completely surrounded by another contour
1916  // MARTIN: If this function is called during PART 1, then the parameter
1917  // "first run"
1918  // is set to "true". This guarantees, that only "nice" squares are
1919  // detected.
1920  // During PART 2, we allow the polygonial approcimation function below
1921  // to
1922  // approximate more freely, which can result in recognized "squares"
1923  // that are in
1924  // reality multiple blurred and sticked together squares.
1925  if (CV_IS_SEQ_HOLE(src_contour) && rect.width * rect.height >= min_size)
1926  {
1927  int min_approx_level = 2, max_approx_level;
1928  if (firstRun == true)
1929  max_approx_level = 3;
1930  else
1931  max_approx_level = MAX_CONTOUR_APPROX;
1932  int approx_level;
1933  for (approx_level = min_approx_level;
1934  approx_level <= max_approx_level; approx_level++)
1935  {
1936  dst_contour = cvApproxPoly(
1937  src_contour, sizeof(CvContour), temp_storage,
1938  CV_POLY_APPROX_DP, (float)approx_level);
1939 
1940  // We call this again on its own output, because sometimes
1941  // cvApproxPoly() does not simplify as much as it should.
1942  dst_contour = cvApproxPoly(
1943  dst_contour, sizeof(CvContour), temp_storage,
1944  CV_POLY_APPROX_DP, (float)approx_level);
1945 
1946  if (dst_contour->total == 4) break;
1947  }
1948 
1949  // Reject non-quadrangles
1950  if (dst_contour->total == 4 && cvCheckContourConvexity(dst_contour))
1951  {
1952  CvPoint pt[4];
1953  // double d1, d2; //, p = cvContourPerimeter(dst_contour);
1954  // double area = fabs(cvContourArea(dst_contour, CV_WHOLE_SEQ));
1955  // double dx, dy;
1956 
1957  for (int i = 0; i < 4; i++)
1958  pt[i] = *(CvPoint*)cvGetSeqElem(dst_contour, i);
1959 
1960  // dx = pt[0].x - pt[2].x;
1961  // dy = pt[0].y - pt[2].y;
1962  // d1 = sqrt(dx*dx + dy*dy);
1963  //
1964  // dx = pt[1].x - pt[3].x;
1965  // dy = pt[1].y - pt[3].y;
1966  // d2 = sqrt(dx*dx + dy*dy);
1967 
1968  // PHILIPG: Only accept those quadrangles which are more
1969  // square than rectangular and which are big enough
1970  // double d3, d4;
1971  // dx = pt[0].x - pt[1].x;
1972  // dy = pt[0].y - pt[1].y;
1973  // d3 = sqrt(dx*dx + dy*dy);
1974  // dx = pt[1].x - pt[2].x;
1975  // dy = pt[1].y - pt[2].y;
1976  // d4 = sqrt(dx*dx + dy*dy);
1977  if (true) //!(flags & CV_CALIB_CB_FILTER_QUADS) ||
1978  // d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_size
1979  // &&
1980  // d1 >= 0.15 * p && d2 >= 0.15 * p )
1981  {
1982  auto* parent = (CvContourEx*)(src_contour->v_prev);
1983  parent->counter++;
1984  if (!board || board->counter < parent->counter)
1985  board = parent;
1986  dst_contour->v_prev = (CvSeq*)parent;
1987  cvSeqPush(root, &dst_contour);
1988  }
1989  }
1990  }
1991  }
1992 
1993  // Finish contour retrieving
1994  cvEndFindContours(&scanner);
1995 
1996  // Allocate quad & corner buffers
1997  out_quads.clear();
1998  for (int q = 0; q < root->total; q++)
1999  out_quads.push_back(CvCBQuad::Ptr(new CvCBQuad));
2000 
2001  out_corners.clear();
2002  for (int q = 0; q < 4 * root->total; q++)
2003  out_corners.push_back(CvCBCorner::Ptr(new CvCBCorner));
2004 
2005  // Create array of quads structures
2006  for (int idx = 0; idx < root->total; idx++)
2007  {
2008  CvCBQuad::Ptr& q = out_quads[quad_count];
2009  src_contour = *(CvSeq**)cvGetSeqElem(root, idx);
2010  if ((flags & cv::CALIB_CB_FILTER_QUADS) &&
2011  src_contour->v_prev != (CvSeq*)board)
2012  continue;
2013 
2014  // Reset group ID
2015  // memset( q, 0, sizeof(*q) );
2016  q->group_idx = -1;
2017  assert(src_contour->total == 4);
2018  for (int i = 0; i < 4; i++)
2019  {
2020  CvPoint2D32f pt =
2021  cvPointTo32f(*(CvPoint*)cvGetSeqElem(src_contour, i));
2022  CvCBCorner::Ptr& corner = out_corners
2023  [quad_count * 4 + i]; // &(*out_corners)[quad_count*4
2024  // + i];
2025 
2026  // memset( corner, 0, sizeof(*corner) );
2027  corner->pt = pt;
2028  q->corners[i] = corner;
2029  }
2030  q->edge_len = FLT_MAX;
2031  for (int i = 0; i < 4; i++)
2032  {
2033  float dx = q->corners[i]->pt.x - q->corners[(i + 1) & 3]->pt.x;
2034  float dy = q->corners[i]->pt.y - q->corners[(i + 1) & 3]->pt.y;
2035  float d = dx * dx + dy * dy;
2036  if (q->edge_len > d) q->edge_len = d;
2037  }
2038  quad_count++;
2039  }
2040 
2041  if (cvGetErrStatus() < 0)
2042  {
2043  out_quads.clear();
2044  // if( out_corners ) cvFree( out_corners );
2045  out_corners.clear();
2046  quad_count = 0;
2047  }
2048 
2049  cvClearSeq(root);
2050 
2051  return quad_count;
2052 }
2053 
2054 // Return 1 on success in finding all the quads, 0 on didn't, -1 on error.
2055 int myQuads2Points(
2056  const std::vector<CvCBQuad::Ptr>& output_quads, const CvSize& pattern_size,
2057  std::vector<CvPoint2D32f>& out_corners)
2058 {
2059  // Initialize
2060  out_corners.clear();
2061 
2062  bool flagRow = false;
2063  bool flagColumn = false;
2064  int maxPattern_sizeRow = -1;
2065  int maxPattern_sizeColumn = -1;
2066 
2067  // Compute the minimum and maximum row / column ID
2068  // (it is unlikely that more than 8bit checkers are used per dimension)
2069  int min_row = 127;
2070  int max_row = -127;
2071  int min_column = 127;
2072  int max_column = -127;
2073 
2074  for (size_t i = 0; i < output_quads.size(); i++)
2075  {
2076  const CvCBQuad::Ptr& q = output_quads[i];
2077 
2078  for (int j = 0; j < 4; j++)
2079  {
2080  if ((q->corners[j])->row > max_row) max_row = (q->corners[j])->row;
2081  if ((q->corners[j])->row < min_row) min_row = (q->corners[j])->row;
2082  if ((q->corners[j])->column > max_column)
2083  max_column = (q->corners[j])->column;
2084  if ((q->corners[j])->column < min_column)
2085  min_column = (q->corners[j])->column;
2086  }
2087  }
2088 
2089  // If in a given direction the target pattern size is reached, we know
2090  // exactly how
2091  // the checkerboard is oriented.
2092  // Else we need to prepare enough "dummy" corners for the worst case.
2093  for (size_t i = 0; i < output_quads.size(); i++)
2094  {
2095  const CvCBQuad::Ptr& q = output_quads[i];
2096 
2097  for (int j = 0; j < 4; j++)
2098  {
2099  if ((q->corners[j])->column == max_column &&
2100  (q->corners[j])->row != min_row &&
2101  (q->corners[j])->row != max_row)
2102  {
2103  if ((q->corners[j]->needsNeighbor) == false)
2104  {
2105  // We know, that the target pattern size is reached
2106  // in column direction
2107  flagColumn = true;
2108  }
2109  }
2110  if ((q->corners[j])->row == max_row &&
2111  (q->corners[j])->column != min_column &&
2112  (q->corners[j])->column != max_column)
2113  {
2114  if ((q->corners[j]->needsNeighbor) == false)
2115  {
2116  // We know, that the target pattern size is reached
2117  // in row direction
2118  flagRow = true;
2119  }
2120  }
2121  }
2122  }
2123 
2124  if (flagColumn == true)
2125  {
2126  if (max_column - min_column == pattern_size.width + 1)
2127  {
2128  maxPattern_sizeColumn = pattern_size.width;
2129  maxPattern_sizeRow = pattern_size.height;
2130  }
2131  else
2132  {
2133  maxPattern_sizeColumn = pattern_size.height;
2134  maxPattern_sizeRow = pattern_size.width;
2135  }
2136  }
2137  else if (flagRow == true)
2138  {
2139  if (max_row - min_row == pattern_size.width + 1)
2140  {
2141  maxPattern_sizeRow = pattern_size.width;
2142  maxPattern_sizeColumn = pattern_size.height;
2143  }
2144  else
2145  {
2146  maxPattern_sizeRow = pattern_size.height;
2147  maxPattern_sizeColumn = pattern_size.width;
2148  }
2149  }
2150  else
2151  {
2152  // If target pattern size is not reached in at least one of the two
2153  // directions, then we do not know where the remaining corners are
2154  // located. Account for this.
2155  maxPattern_sizeColumn = max(pattern_size.width, pattern_size.height);
2156  maxPattern_sizeRow = max(pattern_size.width, pattern_size.height);
2157  }
2158 
2159  // JL: Check sizes:
2160  if (maxPattern_sizeRow * maxPattern_sizeColumn !=
2161  pattern_size.width * pattern_size.height)
2162  return 0; // Bad...
2163  // and also, swap rows/columns so we always have consistently the points in
2164  // the order: first all columns in a row, then the next row, etc...
2165  bool do_swap_col_row = maxPattern_sizeRow != pattern_size.height;
2166 
2167  if (do_swap_col_row)
2168  {
2169  std::swap(min_row, min_column);
2170  std::swap(maxPattern_sizeRow, maxPattern_sizeColumn);
2171  }
2172 
2173  // Write the corners in increasing order to "out_corners"
2174 
2175  for (int i = min_row + 1; i < maxPattern_sizeRow + min_row + 1; i++)
2176  {
2177  for (int j = min_column + 1; j < maxPattern_sizeColumn + min_column + 1;
2178  j++)
2179  {
2180  // Reset the iterator
2181  int iter = 1;
2182 
2183  for (size_t k = 0; k < output_quads.size(); k++)
2184  {
2185  for (int l = 0; l < 4; l++)
2186  {
2187  int r = output_quads[k]->corners[l]->row;
2188  int c = output_quads[k]->corners[l]->column;
2189  if (do_swap_col_row) std::swap(r, c);
2190 
2191  if (r == i && c == j)
2192  {
2193  // Only write corners to the output file, which are
2194  // connected
2195  // i.e. only if iter == 2
2196  if (iter == 2)
2197  {
2198  // The respective row and column have been found,
2199  // save point:
2200  out_corners.push_back(
2201  output_quads[k]->corners[l]->pt);
2202  }
2203 
2204  // If the iterator is larger than two, this means that
2205  // more than
2206  // two corners have the same row / column entries. Then
2207  // some
2208  // linking errors must have occured and we should not
2209  // use the found
2210  // pattern
2211  if (iter > 2) return -1;
2212 
2213  iter++;
2214  }
2215  }
2216  }
2217 
2218  // If the respective row / column is non - existent or is a border
2219  // corner
2220  if (iter == 1 || iter == 2)
2221  {
2222  // cornersX << -1;
2223  // cornersX << " ";
2224  // cornersY << -1;
2225  // cornersY << " ";
2226  }
2227  }
2228  }
2229 
2230  // All corners found?
2231  return (out_corners.size() ==
2232  size_t(pattern_size.width) * size_t(pattern_size.height))
2233  ? 1
2234  : 0;
2235 }
2236 
2237 // Make unique all the (smart pointers-pointed) objects in the list and
2238 // neighbors lists.
2239 void quadListMakeUnique(std::vector<CvCBQuad::Ptr>& quads)
2240 {
2241  std::map<CvCBQuad*, size_t> pointer2index;
2242  for (size_t i = 0; i < quads.size(); i++) pointer2index[quads[i].get()] = i;
2243 
2244  vector<std::array<size_t, 4>> neig_indices(quads.size());
2245  for (size_t i = 0; i < quads.size(); i++)
2246  for (size_t j = 0; j < 4; j++)
2247  neig_indices[i][j] = pointer2index[quads[i]->neighbors[j].get()];
2248 
2249  std::vector<CvCBQuad::Ptr> new_quads = quads;
2250  std::for_each(new_quads.begin(), new_quads.end(), [](CvCBQuad::Ptr& ptr) {
2251  ptr = CvCBQuad::Ptr(new CvCBQuad(*ptr));
2252  });
2253  for (size_t i = 0; i < new_quads.size(); i++)
2254  for (size_t j = 0; j < 4; j++)
2255  new_quads[i]->neighbors[j] = new_quads[neig_indices[i][j]];
2256 }
2257 
2258 //===========================================================================
2259 // END OF FILE (Of "OCamCalib Toolbox" code)
2260 //===========================================================================
2261 
2262 #endif // MRPT_HAS_OPENCV
mrFindQuadNeighbors2
void mrFindQuadNeighbors2(std::vector< CvCBQuad::Ptr > &quads, int dilation)
Definition: checkerboard_ocamcalib_detector.cpp:1229
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:768
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
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:1490
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:2055
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
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:2239
icvFindConnectedQuads
void icvFindConnectedQuads(std::vector< CvCBQuad::Ptr > &quad, std::vector< CvCBQuad::Ptr > &out_group, const int group_idx, [[maybe_unused]] const int dilation)
Definition: checkerboard_ocamcalib_detector.cpp:715
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
icvGenerateQuads
int icvGenerateQuads(vector< CvCBQuad::Ptr > &out_quads, vector< CvCBCorner::Ptr > &out_corners, const CImage &image, int flags, [[maybe_unused]] int dilation, bool firstRun)
Definition: checkerboard_ocamcalib_detector.cpp:1877


Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mié abr 1 00:50:30 CEST 2020