MRPT  2.0.1
checkerboard_multiple_detector.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/img/CImage.h>
13 #include <mrpt/math/CVectorFixed.h>
14 #include <mrpt/math/geometry.h>
15 #include <mrpt/math/kmeans.h>
16 #include <list>
17 #include <vector>
18 
19 // Universal include for all versions of OpenCV
20 #include <mrpt/3rdparty/do_opencv_includes.h>
22 
23 #if VIS
25 #endif
26 
27 using namespace mrpt;
28 using namespace mrpt::img;
29 using namespace mrpt::math;
30 using namespace std;
31 
32 #if MRPT_HAS_OPENCV
33 
34 // Return: true: found OK
36  const CImage& img_, CvSize pattern_size,
37  std::vector<std::vector<CvPoint2D32f>>& out_corners)
38 {
39  // Assure it's a grayscale image:
40  const CImage img(img_, FAST_REF_OR_CONVERT_TO_GRAY);
41 
42  CImage thresh_img(img.getWidth(), img.getHeight(), CH_GRAY);
43  CImage thresh_img_save(img.getWidth(), img.getHeight(), CH_GRAY);
44 
45  out_corners.clear(); // for now, empty the output.
46 
47  const size_t expected_quads_count =
48  ((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<CvCBQuad::Ptr> quads;
57  vector<CvCBCorner::Ptr> corners;
58  list<vector<CvCBQuad::Ptr>>
59  good_quad_groups; // Storage of potential good quad groups found
60 
61  if (pattern_size.width < 2 || pattern_size.height < 2)
62  {
63  std::cerr << "Pattern should have at least 2x2 size" << endl;
64  return false;
65  }
66  if (pattern_size.width > 127 || pattern_size.height > 127)
67  {
68  std::cerr << "Pattern should not have a size larger than 127 x 127"
69  << endl;
70  return false;
71  }
72 
73  // JL: Move these constructors out of the loops:
74  IplConvKernel* kernel_cross =
75  cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_CROSS, nullptr);
76  IplConvKernel* kernel_rect =
77  cvCreateStructuringElementEx(3, 3, 1, 1, CV_SHAPE_RECT, nullptr);
78 
79  static int kernel_diag1_vals[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
80  IplConvKernel* kernel_diag1 = cvCreateStructuringElementEx(
81  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_diag1_vals);
82  static int kernel_diag2_vals[9] = {0, 0, 1, 0, 1, 0, 1, 0, 0};
83  IplConvKernel* kernel_diag2 = cvCreateStructuringElementEx(
84  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_diag2_vals);
85  static int kernel_horz_vals[9] = {0, 0, 0, 1, 1, 1, 0, 0, 0};
86  IplConvKernel* kernel_horz = cvCreateStructuringElementEx(
87  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_horz_vals);
88  static int kernel_vert_vals[9] = {0, 1, 0, 0, 1, 0, 0, 1, 0};
89  IplConvKernel* kernel_vert = cvCreateStructuringElementEx(
90  3, 3, 1, 1, CV_SHAPE_CUSTOM, kernel_vert_vals);
91 
92  // For image binarization (thresholding)
93  // we use an adaptive threshold with a gaussian mask
94  // ATTENTION: Gaussian thresholding takes MUCH more time than Mean
95  // thresholding!
96  int block_size = cvRound(MIN(img.getWidth(), img.getHeight()) * 0.2) | 1;
97 
98  cv::adaptiveThreshold(
99  img.asCvMat<cv::Mat>(SHALLOW_COPY),
100  thresh_img.asCvMat<cv::Mat>(SHALLOW_COPY), 255,
101  CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, block_size, 0);
102 
103  thresh_img_save = thresh_img.makeDeepCopy();
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;
112  // dilations++ )
113 
114  bool last_dilation = false;
115 
116  for (int dilations = 0; !last_dilation; dilations++)
117  {
118  // Calling "cvCopy" again is much faster than rerunning
119  // "cvAdaptiveThreshold"
120  thresh_img = thresh_img_save.makeDeepCopy();
121 
122  // Dilate squares:
123  last_dilation = do_special_dilation(
124  thresh_img, dilations, kernel_cross, kernel_rect, kernel_diag1,
125  kernel_diag2, kernel_horz, kernel_vert);
126 
127  // In order to find rectangles that go to the edge, we draw a white
128  // line around the image edge. Otherwise FindContours will miss those
129  // clipped rectangle contours. The border color will be the image mean,
130  // because otherwise we risk screwing up filters like cvSmooth()
131  cv::rectangle(
132  thresh_img.asCvMatRef(), cv::Point(0, 0),
133  cv::Point(thresh_img.getWidth() - 1, thresh_img.getHeight() - 1),
134  CV_RGB(255, 255, 255), 3, 8);
135 
136  // Generate quadrangles in the following function
137  // "quad_count" is the number of cound quadrangles
138  int quad_count = icvGenerateQuads(
139  quads, corners, thresh_img, flags, dilations, true);
140  if (quad_count <= 0) continue;
141 
142  // The following function finds and assigns neighbor quads to every
143  // quadrangle in the immediate vicinity fulfilling certain
144  // prerequisites
145  mrFindQuadNeighbors2(quads, dilations);
146 
147  // JL: To achieve multiple-checkerboard, take all the raw detected quads
148  // and
149  // separate them in groups with k-means.
150  std::vector<CVectorFixedDouble<2>> quad_centers;
151  quad_centers.resize(quads.size());
152  for (size_t i = 0; i < quads.size(); i++)
153  {
154  const CvCBQuad* q = quads[i].get();
155  quad_centers[i][0] =
156  0.25 * (q->corners[0]->pt.x + q->corners[1]->pt.x +
157  q->corners[2]->pt.x + q->corners[3]->pt.x);
158  quad_centers[i][1] =
159  0.25 * (q->corners[0]->pt.y + q->corners[1]->pt.y +
160  q->corners[2]->pt.y + q->corners[3]->pt.y);
161  }
162 
163  // Try the k-means with a number of variable # of clusters:
164  static const size_t MAX_NUM_CLUSTERS = 4;
165  for (size_t nClusters = 1; nClusters < MAX_NUM_CLUSTERS; nClusters++)
166  {
167  vector<size_t> num_quads_by_cluster(nClusters);
168 
169  vector<int> assignments;
170  mrpt::math::kmeanspp(nClusters, quad_centers, assignments);
171 
172  // Count # of quads in each cluster:
173  for (size_t i = 0; i < nClusters; i++)
174  num_quads_by_cluster[i] =
175  std::count(assignments.begin(), assignments.end(), i);
176 
177  // Take a look at the promising clusters:
178  // -----------------------------------------
179  for (size_t i = 0; i < nClusters; i++)
180  {
181  if (num_quads_by_cluster[i] <
182  size_t(pattern_size.height) * size_t(pattern_size.width))
183  continue; // Can't be good...
184 
185  // Create a subset of the quads with those in the i'th cluster:
186  vector<CvCBQuad::Ptr> ith_quads;
187  for (size_t q = 0; q < quads.size(); q++)
188  if (size_t(assignments[q]) == i)
189  ith_quads.push_back(quads[q]);
190 
191  // Make sense out of smart pointers...
192  quadListMakeUnique(ith_quads);
193 
194  // The connected quads will be organized in groups. The
195  // following loop
196  // increases a "group_idx" identifier.
197  // The function "icvFindConnectedQuads assigns all connected
198  // quads
199  // a unique group ID.
200  // If more quadrangles were assigned to a given group (i.e.
201  // connected)
202  // than are expected by the input variable "pattern_size", the
203  // function "icvCleanFoundConnectedQuads" erases the surplus
204  // quadrangles by minimizing the convex hull of the remaining
205  // pattern.
206  for (int group_idx = 0;; group_idx++)
207  {
208  vector<CvCBQuad::Ptr> quad_group;
209 
211  ith_quads, quad_group, group_idx, dilations);
212  if (quad_group.empty()) break;
213 
214  icvCleanFoundConnectedQuads(quad_group, pattern_size);
215  size_t count = quad_group.size();
216 
217  if (count == expected_quads_count)
218  {
219  // The following function labels all corners of every
220  // quad
221  // with a row and column entry.
222  mrLabelQuadGroup(quad_group, pattern_size, true);
223 
224  // Add this set of quads as a good result to be returned
225  // to the user:
226  good_quad_groups.push_back(quad_group);
227  // And "make_unique()" it:
228  // quadListMakeUnique( good_quad_groups.back() );
229  }
230 
231  } // end for each "group_idx"
232 
233  } // end of the loop "try with each promising cluster"
234 
235  } // end loop, for each "nClusters" size.
236 
237  } // end for dilation
238 
239  // Convert the set of good detected quad sets in "good_quad_groups"
240  // to the expected output data struct, doing a final check to
241  // remove duplicates:
242  vector<TPoint2D>
243  out_boards_centers; // the center (average) of each output board.
244  for (auto it = good_quad_groups.begin(); it != good_quad_groups.end(); ++it)
245  {
246  // Compute the center of this board:
247  TPoint2D boardCenter(0, 0);
248  for (size_t i = 0; i < it->size(); i++)
249  { // JL: Avoid the normalizations of all the averages, since it really
250  // doesn't matter for our purpose.
251  boardCenter += TPoint2D(
252  /*0.25* */ (*it)[i]->corners[0]->pt.x +
253  (*it)[i]->corners[1]->pt.x + (*it)[i]->corners[2]->pt.x +
254  (*it)[i]->corners[3]->pt.x,
255  /*0.25* */ (*it)[i]->corners[0]->pt.y +
256  (*it)[i]->corners[1]->pt.y + (*it)[i]->corners[2]->pt.y +
257  (*it)[i]->corners[3]->pt.y);
258  }
259 
260  // If it's too close to an already existing board, it's surely a
261  // duplicate:
262  double min_dist = std::numeric_limits<double>::max();
263  for (size_t b = 0; b < out_boards_centers.size(); b++)
264  keep_min(
265  min_dist,
266  mrpt::math::distance(boardCenter, out_boards_centers[b]));
267 
268  if (out_corners.empty() || min_dist > 80)
269  {
270  vector<CvPoint2D32f> pts;
271  if (1 ==
272  myQuads2Points(*it, pattern_size, pts)) // and populate it now.
273  {
274  // Ok with it: add to the output list:
275  out_corners.push_back(pts);
276  // Add center to list of known centers:
277  out_boards_centers.push_back(boardCenter);
278  }
279  }
280  }
281 
282  // Free mem:
283  cvReleaseStructuringElement(&kernel_cross);
284  cvReleaseStructuringElement(&kernel_rect);
285  cvReleaseStructuringElement(&kernel_diag1);
286  cvReleaseStructuringElement(&kernel_diag2);
287  cvReleaseStructuringElement(&kernel_horz);
288  cvReleaseStructuringElement(&kernel_vert);
289 
290  return !out_corners.empty();
291 }
292 
293 #endif // MRPT_HAS_OPENCV
void mrFindQuadNeighbors2(std::vector< CvCBQuad::Ptr > &quads, int dilation)
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:75
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...
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
void mrLabelQuadGroup(std::vector< CvCBQuad::Ptr > &quad_group, const CvSize &pattern_size, bool firstRun)
void icvCleanFoundConnectedQuads(std::vector< CvCBQuad::Ptr > &quad_group, const CvSize &pattern_size)
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:849
STL namespace.
void asCvMat(cv::Mat &out_img, copy_type_t copy_type) const
Makes a shallow or deep copy of this image into the provided cv::Mat.
Definition: CImage.cpp:217
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
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)
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...
void clear()
Resets the image to the state after a default ctor.
Definition: CImage.cpp:1565
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool find_chessboard_corners_multiple(const CImage &img_, CvSize pattern_size, std::vector< std::vector< CvPoint2D32f >> &out_corners)
CvCBCorner::Ptr corners[4]
void quadListMakeUnique(std::vector< CvCBQuad::Ptr > &quads)
void icvFindConnectedQuads(std::vector< CvCBQuad::Ptr > &quad, std::vector< CvCBQuad::Ptr > &out_group, const int group_idx, [[maybe_unused]] const int dilation)
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
int icvGenerateQuads(vector< CvCBQuad::Ptr > &out_quads, vector< CvCBCorner::Ptr > &out_corners, const CImage &image, int flags, [[maybe_unused]] int dilation, bool firstRun)



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020