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