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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019