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


Page generated by Doxygen 1.9.1 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at mar 26 may 2026 13:12:03 CEST