Main MRPT website > C++ reference for MRPT 1.5.7
checkerboard_multiple_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 <mrpt/math/kmeans.h>
13 #include <mrpt/math/geometry.h>
14 #include <list>
15 
16 // Universal include for all versions of OpenCV
17 #include <mrpt/otherlibs/do_opencv_includes.h>
19 
20 #if VIS
22 #endif
23 
24 
25 using namespace mrpt;
26 using namespace mrpt::utils;
27 using namespace mrpt::math;
28 using namespace mrpt::utils;
29 using namespace std;
30 
31 
32 #if MRPT_HAS_OPENCV
33 
34 // Return: true: found OK
36  const mrpt::utils::CImage & img_,
37  CvSize pattern_size,
38  std::vector< std::vector<CvPoint2D32f> > &out_corners)
39 {
40  // Assure it's a grayscale image:
42 
43  mrpt::utils::CImage thresh_img(img.getWidth(),img.getHeight(), CH_GRAY );
44  mrpt::utils::CImage thresh_img_save(img.getWidth(),img.getHeight(), CH_GRAY );
45 
46  out_corners.clear(); // for now, empty the output.
47 
48  const size_t expected_quads_count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
49 
50  // PART 0: INITIALIZATION
51  //-----------------------------------------------------------------------
52  // Initialize variables
53  int flags = 1; // not part of the function call anymore!
54  //int found = 0;
55 
56  vector<CvCBQuadPtr> quads;
57  vector<CvCBCornerPtr> corners;
58  list< vector<CvCBQuadPtr> > good_quad_groups; // Storage of potential good quad groups found
59 
60  if( pattern_size.width < 2 || pattern_size.height < 2 )
61  {
62  std::cerr << "Pattern should have at least 2x2 size" << endl;
63  return false;
64  }
65  if( pattern_size.width > 127 || pattern_size.height > 127 )
66  {
67  std::cerr << "Pattern should not have a size larger than 127 x 127" << endl;
68  return false;
69  }
70 
71  // JL: Move these constructors out of the loops:
72  IplConvKernel *kernel_cross = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_CROSS,NULL);
73  IplConvKernel *kernel_rect = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_RECT,NULL);
74 
75  static int kernel_diag1_vals[9] = {
76  1,0,0,
77  0,1,0,
78  0,0,1 };
79  IplConvKernel *kernel_diag1 = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_CUSTOM,kernel_diag1_vals);
80  static int kernel_diag2_vals[9] = {
81  0,0,1,
82  0,1,0,
83  1,0,0 };
84  IplConvKernel *kernel_diag2 = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_CUSTOM,kernel_diag2_vals);
85  static int kernel_horz_vals[9] = {
86  0,0,0,
87  1,1,1,
88  0,0,0 };
89  IplConvKernel *kernel_horz = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_CUSTOM,kernel_horz_vals);
90  static int kernel_vert_vals[9] = {
91  0,1,0,
92  0,1,0,
93  0,1,0 };
94  IplConvKernel *kernel_vert = cvCreateStructuringElementEx(3,3,1,1,CV_SHAPE_CUSTOM,kernel_vert_vals);
95 
96  // For image binarization (thresholding)
97  // we use an adaptive threshold with a gaussian mask
98  // ATTENTION: Gaussian thresholding takes MUCH more time than Mean thresholding!
99  int block_size = cvRound(MIN(img.getWidth(),img.getHeight())*0.2)|1;
100 
101  cvAdaptiveThreshold( img.getAs<IplImage>(), thresh_img.getAs<IplImage>(), 255, CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, block_size, 0 );
102 
103  cvCopy( thresh_img.getAs<IplImage>(), thresh_img_save.getAs<IplImage>());
104 
105  // PART 1: FIND LARGEST PATTERN
106  //-----------------------------------------------------------------------
107  // Checker patterns are tried to be found by dilating the background and
108  // then applying a canny edge finder on the closed contours (checkers).
109  // Try one dilation run, but if the pattern is not found, repeat until
110  // max_dilations is reached.
111  //for( int dilations = min_dilations; dilations <= max_dilations; dilations++ )
112 
113  bool last_dilation = false;
114 
115  for( int dilations = 0; !last_dilation; dilations++ )
116  {
117  // Calling "cvCopy" again is much faster than rerunning "cvAdaptiveThreshold"
118  cvCopy( thresh_img_save.getAs<IplImage>(), thresh_img.getAs<IplImage>() );
119 
120  // Dilate squares:
121  last_dilation = do_special_dilation(thresh_img, dilations,kernel_cross,kernel_rect,kernel_diag1,kernel_diag2, kernel_horz,kernel_vert);
122 
123  // In order to find rectangles that go to the edge, we draw a white
124  // line around the image edge. Otherwise FindContours will miss those
125  // clipped rectangle contours. The border color will be the image mean,
126  // because otherwise we risk screwing up filters like cvSmooth()
127  cvRectangle( thresh_img.getAs<IplImage>(), cvPoint(0,0),
128  cvPoint(thresh_img.getWidth()-1,thresh_img.getHeight()-1),
129  CV_RGB(255,255,255), 3, 8);
130 
131  // Generate quadrangles in the following function
132  // "quad_count" is the number of cound quadrangles
133  int quad_count = icvGenerateQuads( quads, corners, thresh_img, flags, dilations, true );
134  if( quad_count <= 0 )
135  continue;
136 
137  // The following function finds and assigns neighbor quads to every
138  // quadrangle in the immediate vicinity fulfilling certain
139  // prerequisites
140  mrFindQuadNeighbors2( quads, dilations);
141 
142  // JL: To achieve multiple-checkerboard, take all the raw detected quads and
143  // separate them in groups with k-means.
144  vector<CArrayDouble<2>, Eigen::aligned_allocator<CArrayDouble<2> > > quad_centers;
145  quad_centers.resize(quads.size());
146  for (size_t i=0;i<quads.size();i++)
147  {
148  const CvCBQuad* q= quads[i].get();
149  quad_centers[i][0] = 0.25 * (q->corners[0]->pt.x + q->corners[1]->pt.x + q->corners[2]->pt.x + q->corners[3]->pt.x);
150  quad_centers[i][1] = 0.25 * (q->corners[0]->pt.y + q->corners[1]->pt.y + q->corners[2]->pt.y + q->corners[3]->pt.y);
151  }
152 
153  // Try the k-means with a number of variable # of clusters:
154  static const size_t MAX_NUM_CLUSTERS = 4;
155  for (size_t nClusters=1;nClusters<MAX_NUM_CLUSTERS;nClusters++)
156  {
157  vector<size_t> num_quads_by_cluster(nClusters);
158 
159  vector<int> assignments;
161  vector<CArrayDouble<2>,Eigen::aligned_allocator<CArrayDouble<2> > >,
162  vector<CArrayDouble<2>,Eigen::aligned_allocator<CArrayDouble<2> > > >
163  (nClusters,quad_centers,assignments);
164 
165  // Count # of quads in each cluster:
166  for (size_t i=0;i<nClusters;i++)
167  num_quads_by_cluster[i] = std::count(assignments.begin(),assignments.end(), i);
168 
169 #if VIS
170  {
171  static mrpt::gui::CDisplayWindow win;
172  win.setWindowTitle( format("All quads (%u) | %u clusters",
173  (unsigned)quad_centers.size(),
174  (unsigned)nClusters ));
176  img.colorImage(im);
177  for (size_t i=0;i<quad_centers.size();i++)
178  {
179  static const TColor colors[4] = { TColor(255,0,0),TColor(0,0,255),TColor(255,0,255),TColor(0,255,0) };
180  im.cross(quad_centers[i][0],quad_centers[i][1], colors[assignments[i]%4],'+', 10);
181  }
182  win.showImage(im);
183  win.waitForKey();
184  }
185 #endif // VIS
186 
187 
188  // Take a look at the promising clusters:
189  // -----------------------------------------
190  for (size_t i=0;i<nClusters;i++)
191  {
192  if (num_quads_by_cluster[i]<size_t(pattern_size.height*pattern_size.width))
193  continue; // Can't be good...
194 
195  // Create a subset of the quads with those in the i'th cluster:
196  vector<CvCBQuadPtr> ith_quads;
197  for (size_t q=0;q<quads.size();q++)
198  if (size_t(assignments[q])==i)
199  ith_quads.push_back(quads[q]);
200 
201  // Make sense out of smart pointers...
202  quadListMakeUnique(ith_quads);
203 
204  // The connected quads will be organized in groups. The following loop
205  // increases a "group_idx" identifier.
206  // The function "icvFindConnectedQuads assigns all connected quads
207  // a unique group ID.
208  // If more quadrangles were assigned to a given group (i.e. connected)
209  // than are expected by the input variable "pattern_size", the
210  // function "icvCleanFoundConnectedQuads" erases the surplus
211  // quadrangles by minimizing the convex hull of the remaining pattern.
212  for( int group_idx = 0; ; group_idx++ )
213  {
214  vector<CvCBQuadPtr> quad_group;
215 
216  icvFindConnectedQuads( ith_quads , quad_group, group_idx, dilations );
217  if( quad_group.empty() )
218  break;
219 
220  icvCleanFoundConnectedQuads( quad_group, pattern_size );
221  size_t count = quad_group.size();
222 
223  if( count == expected_quads_count )
224  {
225 #if VIS
226  {
227  static mrpt::gui::CDisplayWindow win;
228  win.setWindowTitle( format("Candidate group #%i (%i)",(int)group_idx, (int)quad_group.size()));
230  img.colorImage(im);
231  for (size_t i=0;i<quad_group.size();i++)
232  {
233  static const TColor colors[4] = { TColor(255,0,0),TColor(0,0,255),TColor(255,0,255),TColor(0,255,0) };
234  const double x=0.25*(quad_group[i]->corners[0]->pt.x+quad_group[i]->corners[1]->pt.x+quad_group[i]->corners[2]->pt.x+quad_group[i]->corners[3]->pt.x);
235  const double y=0.25*(quad_group[i]->corners[0]->pt.y+quad_group[i]->corners[1]->pt.y+quad_group[i]->corners[2]->pt.y+quad_group[i]->corners[3]->pt.y);
236  im.cross(x,y, colors[group_idx%4],'+', 10);
237  }
238  win.showImage(im);
239  win.waitForKey();
240  }
241 #endif // VIS
242 
243  // The following function labels all corners of every quad
244  // with a row and column entry.
245  mrLabelQuadGroup( quad_group, pattern_size, true );
246 
247  // Add this set of quads as a good result to be returned to the user:
248  good_quad_groups.push_back(quad_group);
249  // And "make_unique()" it:
250  //quadListMakeUnique( good_quad_groups.back() );
251  }
252 
253  } // end for each "group_idx"
254 
255  } // end of the loop "try with each promising cluster"
256 
257  } // end loop, for each "nClusters" size.
258 
259  } // end for dilation
260 
261 
262  // Convert the set of good detected quad sets in "good_quad_groups"
263  // to the expected output data struct, doing a final check to
264  // remove duplicates:
265  vector<TPoint2D> out_boards_centers; // the center (average) of each output board.
266  for (list<vector<CvCBQuadPtr> >::const_iterator it=good_quad_groups.begin();it!=good_quad_groups.end();++it)
267  {
268  // Compute the center of this board:
269  TPoint2D boardCenter(0,0);
270  for (size_t i=0;i<it->size();i++)
271  { // JL: Avoid the normalizations of all the averages, since it really doesn't matter for our purpose.
272  boardCenter += TPoint2D(
273  /*0.25* */ (*it)[i]->corners[0]->pt.x+(*it)[i]->corners[1]->pt.x+(*it)[i]->corners[2]->pt.x+(*it)[i]->corners[3]->pt.x,
274  /*0.25* */ (*it)[i]->corners[0]->pt.y+(*it)[i]->corners[1]->pt.y+(*it)[i]->corners[2]->pt.y+(*it)[i]->corners[3]->pt.y );
275  }
276 
277  // If it's too close to an already existing board, it's surely a duplicate:
278  double min_dist = std::numeric_limits<double>::max();
279  for (size_t b=0;b<out_boards_centers.size();b++)
280  keep_min(min_dist, mrpt::math::distance(boardCenter, out_boards_centers[b]) );
281 
282  if (out_corners.empty() || min_dist > 80 )
283  {
284  vector<CvPoint2D32f> pts;
285  if (1==myQuads2Points( *it, pattern_size,pts )) // and populate it now.
286  {
287  // Ok with it: add to the output list:
288  out_corners.push_back(pts);
289  // Add center to list of known centers:
290  out_boards_centers.push_back(boardCenter);
291  }
292  }
293  }
294 
295 
296  // Free mem:
297  cvReleaseStructuringElement(&kernel_cross);
298  cvReleaseStructuringElement(&kernel_rect);
299  cvReleaseStructuringElement(&kernel_diag1);
300  cvReleaseStructuringElement(&kernel_diag2);
301  cvReleaseStructuringElement(&kernel_horz);
302  cvReleaseStructuringElement(&kernel_vert);
303 
304  return !out_corners.empty();
305 }
306 
307 
308 #endif // MRPT_HAS_OPENCV
int waitForKey(bool ignoreControlKeys=true, mrptKeyModifier *out_pushModifier=NULL)
Waits for any key to be pushed on the image or the console, and returns the key code.
GLuint GLuint GLsizei count
Definition: glext.h:3512
bool find_chessboard_corners_multiple(const mrpt::utils::CImage &img_, CvSize pattern_size, std::vector< std::vector< CvPoint2D32f > > &out_corners)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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)
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
void mrFindQuadNeighbors2(std::vector< CvCBQuadPtr > &quads, int dilation)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
void icvCleanFoundConnectedQuads(std::vector< CvCBQuadPtr > &quad_group, const CvSize &pattern_size)
#define MIN(a, b)
Definition: jpegint.h:266
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
int myQuads2Points(const std::vector< CvCBQuadPtr > &output_quads, const CvSize &pattern_size, std::vector< CvPoint2D32f > &out_corners)
void icvFindConnectedQuads(std::vector< CvCBQuadPtr > &quad, std::vector< CvCBQuadPtr > &out_group, const int group_idx, const int dilation)
double kmeanspp(const size_t k, const LIST_OF_VECTORS1 &points, std::vector< int > &assignments, LIST_OF_VECTORS2 *out_centers=NULL, const size_t attempts=3)
k-means++ algorithm to cluster a list of N points of arbitrary dimensionality into exactly K clusters...
void mrLabelQuadGroup(std::vector< CvCBQuadPtr > &quad_group, const CvSize &pattern_size, bool firstRun)
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
GLint GLvoid * img
Definition: glext.h:3645
void cross(int x0, int y0, const mrpt::utils::TColor color, char type, unsigned int size=5, unsigned int width=1)
Draw a cross.
Definition: CCanvas.cpp:329
This class creates a window as a graphical user interface (GUI) for displaying images to the user...
A RGB color - 8bit.
Definition: TColor.h:26
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLubyte GLubyte b
Definition: glext.h:5575
void showImage(const mrpt::utils::CImage &img)
Show a given color or grayscale image on the window.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
Definition: bits.h:171
void setWindowTitle(const std::string &str) MRPT_OVERRIDE
Changes the window title text.
#define CH_GRAY
Definition: CImage.h:41
void quadListMakeUnique(std::vector< CvCBQuadPtr > &quads)
GLenum GLint GLint y
Definition: glext.h:3516
void colorImage(CImage &ret) const
Returns a RGB version of the grayscale image, or itself if it is already a RGB image.
Definition: CImage.cpp:2392
GLenum GLint x
Definition: glext.h:3516
int icvGenerateQuads(vector< CvCBQuadPtr > &out_quads, vector< CvCBCornerPtr > &out_corners, const mrpt::utils::CImage &image, int flags, int dilation, bool firstRun)
Lightweight 2D point.
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019