20 #include <mrpt/3rdparty/do_opencv_includes.h>
36 const CImage& img_, CvSize pattern_size,
37 std::vector<std::vector<CvPoint2D32f>>& out_corners)
47 const size_t expected_quads_count =
48 ((pattern_size.width + 1) * (pattern_size.height + 1) + 1) / 2;
56 vector<CvCBQuad::Ptr> quads;
57 vector<CvCBCorner::Ptr> corners;
58 list<vector<CvCBQuad::Ptr>>
61 if (pattern_size.width < 2 || pattern_size.height < 2)
63 std::cerr <<
"Pattern should have at least 2x2 size" << endl;
66 if (pattern_size.width > 127 || pattern_size.height > 127)
68 std::cerr <<
"Pattern should not have a size larger than 127 x 127"
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);
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);
98 cv::adaptiveThreshold(
101 CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, block_size, 0);
114 bool last_dilation =
false;
116 for (
int dilations = 0; !last_dilation; dilations++)
124 thresh_img, dilations, kernel_cross, kernel_rect, kernel_diag1,
125 kernel_diag2, kernel_horz, kernel_vert);
134 CV_RGB(255, 255, 255), 3, 8);
139 quads, corners, thresh_img, flags, dilations,
true);
140 if (quad_count <= 0)
continue;
150 std::vector<CVectorFixedDouble<2>> quad_centers;
151 quad_centers.resize(quads.size());
152 for (
size_t i = 0; i < quads.size(); i++)
164 static const size_t MAX_NUM_CLUSTERS = 4;
165 for (
size_t nClusters = 1; nClusters < MAX_NUM_CLUSTERS; nClusters++)
167 vector<size_t> num_quads_by_cluster(nClusters);
169 vector<int> assignments;
173 for (
size_t i = 0; i < nClusters; i++)
174 num_quads_by_cluster[i] =
175 std::count(assignments.begin(), assignments.end(), i);
179 for (
size_t i = 0; i < nClusters; i++)
181 if (num_quads_by_cluster[i] <
182 size_t(pattern_size.height) *
size_t(pattern_size.width))
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]);
206 for (
int group_idx = 0;; group_idx++)
208 vector<CvCBQuad::Ptr> quad_group;
211 ith_quads, quad_group, group_idx, dilations);
212 if (quad_group.empty())
break;
215 size_t count = quad_group.size();
217 if (count == expected_quads_count)
226 good_quad_groups.push_back(quad_group);
244 for (
auto it = good_quad_groups.begin(); it != good_quad_groups.end(); ++it)
248 for (
size_t i = 0; i < it->size(); i++)
252 (*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 (*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);
262 double min_dist = std::numeric_limits<double>::max();
263 for (
size_t b = 0; b < out_boards_centers.size(); b++)
268 if (out_corners.empty() || min_dist > 80)
270 vector<CvPoint2D32f> pts;
275 out_corners.push_back(pts);
277 out_boards_centers.push_back(boardCenter);
283 cvReleaseStructuringElement(&kernel_cross);
284 cvReleaseStructuringElement(&kernel_rect);
285 cvReleaseStructuringElement(&kernel_diag1);
286 cvReleaseStructuringElement(&kernel_diag2);
287 cvReleaseStructuringElement(&kernel_horz);
288 cvReleaseStructuringElement(&kernel_vert);
290 return !out_corners.empty();
293 #endif // MRPT_HAS_OPENCV