1//M*//////////////////////////////////////////////////////////////////////////////////////
2//
3// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4//
5// By downloading, copying, installing or using the software you agree to this license.
6// If you do not agree to this license, do not download, install,
7// copy or use the software.
8//
9//
10// Intel License Agreement
11// For Open Source Computer Vision Library
12//
13// Copyright (C) 2000, Intel Corporation, all rights reserved.
14// Third party copyrights are property of their respective owners.
15//
16// Redistribution and use in source and binary forms, with or without modification,
17// are permitted provided that the following conditions are met:
18//
19// * Redistribution's of source code must retain the above copyright notice,
20// this list of conditions and the following disclaimer.
21//
22// * Redistribution's in binary form must reproduce the above copyright notice,
23// this list of conditions and the following disclaimer in the documentation
24// and/or other materials provided with the distribution.
25//
26// * The name of Intel Corporation may not be used to endorse or promote products
27// derived from this software without specific prior written permission.
28//
29// This software is provided by the copyright holders and contributors "as is" and
30// any express or implied warranties, including, but not limited to, the implied
31// warranties of merchantability and fitness for a particular purpose are disclaimed.
32// In no event shall the Intel Corporation or contributors be liable for any direct,
33// indirect, incidental, special, exemplary, or consequential damages
34// (including, but not limited to, procurement of substitute goods or services;
35// loss of use, data, or profits; or business interruption) however caused
36// and on any theory of liability, whether in contract, strict liability,
37// or tort (including negligence or otherwise) arising in any way out of
38// the use of this software, even if advised of the possibility of such damage.
39//
40//M*/
41
42/************************************************************************************\
43 This is improved variant of chessboard corner detection algorithm that
44 uses a graph of connected quads. It is based on the code contributed
45 by Vladimir Vezhnevets and Philip Gruebele.
46 Here is the copyright notice from the original Vladimir's code:
47 ===============================================================
48
49 The algorithms developed and implemented by Vezhnevets Vladimir
50 aka Dead Moroz (vvp@graphics.cs.msu.ru)
51 See http://graphics.cs.msu.su/en/research/calibration/opencv.html
52 for detailed information.
53
54 Reliability additions and modifications made by Philip Gruebele.
55 <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>
56
57 Some further improvements for detection of partially occluded boards at non-ideal
58 lighting conditions have been made by Alex Bovyrin and Kurt Kolonige
59
60\************************************************************************************/
61
62/************************************************************************************\
63 This version adds a new and improved variant of chessboard corner detection
64 that works better in poor lighting condition. It is based on work from
65 Oliver Schreer and Stefano Masneri. This method works faster than the previous
66 one and reverts back to the older method in case no chessboard detection is
67 possible. Overall performance improves also because now the method avoids
68 performing the same computation multiple times when not necessary.
69
70\************************************************************************************/
71
72#include "precomp.hpp"
73#include "circlesgrid.hpp"
74#include "opencv2/flann.hpp"
75
76#include <stack>
77
78//#define ENABLE_TRIM_COL_ROW
79
80// Requires CMake flag: DEBUG_opencv_calib3d=ON
81//#define DEBUG_CHESSBOARD
82#define DEBUG_CHESSBOARD_TIMEOUT 0 // 0 - wait for 'q'
83
84#include <opencv2/core/utils/logger.defines.hpp>
85//#undef CV_LOG_STRIP_LEVEL
86//#define CV_LOG_STRIP_LEVEL CV_LOG_LEVEL_VERBOSE + 1
87#include <opencv2/core/utils/logger.hpp>
88
89#ifdef DEBUG_CHESSBOARD
90#include "opencv2/highgui.hpp"
91#include "opencv2/imgproc.hpp"
92#define DPRINTF(...) CV_LOG_INFO(NULL, cv::format("calib3d: " __VA_ARGS__))
93#else
94#define DPRINTF(...)
95#endif
96
97namespace cv {
98
99//=====================================================================================
100// Implementation for the enhanced calibration object detection
101//=====================================================================================
102
103#define MAX_CONTOUR_APPROX 7
104
105struct QuadCountour {
106 Point pt[4];
107 int parent_contour;
108
109 QuadCountour(const Point pt_[4], int parent_contour_) :
110 parent_contour(parent_contour_)
111 {
112 pt[0] = pt_[0]; pt[1] = pt_[1]; pt[2] = pt_[2]; pt[3] = pt_[3];
113 }
114};
115
116
117/** This structure stores information about the chessboard corner.*/
118struct ChessBoardCorner
119{
120 cv::Point2f pt; // Coordinates of the corner
121 int row; // Board row index
122 int count; // Number of neighbor corners
123 struct ChessBoardCorner* neighbors[4]; // Neighbor corners
124
125 ChessBoardCorner(const cv::Point2f& pt_ = cv::Point2f()) :
126 pt(pt_), row(0), count(0)
127 {
128 neighbors[0] = neighbors[1] = neighbors[2] = neighbors[3] = NULL;
129 }
130
131 float sumDist(int& n_) const
132 {
133 float sum = 0;
134 int n = 0;
135 for (int i = 0; i < 4; ++i)
136 {
137 if (neighbors[i])
138 {
139 sum += sqrt(x: normL2Sqr<float>(pt: neighbors[i]->pt - pt));
140 n++;
141 }
142 }
143 n_ = n;
144 return sum;
145 }
146};
147
148
149/** This structure stores information about the chessboard quadrangle.*/
150struct ChessBoardQuad
151{
152 int count; // Number of quad neighbors
153 int group_idx; // quad group ID
154 int row, col; // row and column of this quad
155 bool ordered; // true if corners/neighbors are ordered counter-clockwise
156 float edge_sqr_len; // quad edge squared length, in pix^2
157 // neighbors and corners are synced, i.e., neighbor 0 shares corner 0
158 ChessBoardCorner *corners[4]; // Coordinates of quad corners
159 struct ChessBoardQuad *neighbors[4]; // Pointers of quad neighbors. M.b. sparse.
160 // Each neighbors element corresponds to quad corner, but not just sequential index.
161
162 ChessBoardQuad(int group_idx_ = -1) :
163 count(0),
164 group_idx(group_idx_),
165 row(0), col(0),
166 ordered(0),
167 edge_sqr_len(0)
168 {
169 corners[0] = corners[1] = corners[2] = corners[3] = NULL;
170 neighbors[0] = neighbors[1] = neighbors[2] = neighbors[3] = NULL;
171 }
172};
173
174
175
176#ifdef DEBUG_CHESSBOARD
177static void SHOW(const std::string & name, Mat & img)
178{
179 imshow(name, img);
180#if DEBUG_CHESSBOARD_TIMEOUT
181 waitKey(DEBUG_CHESSBOARD_TIMEOUT);
182#else
183 while ((uchar)waitKey(0) != 'q') {}
184#endif
185}
186static void SHOW_QUADS(const std::string & name, const Mat & img_, ChessBoardQuad * quads, int quads_count)
187{
188 Mat img = img_.clone();
189 if (img.channels() == 1)
190 cvtColor(img, img, COLOR_GRAY2BGR);
191 for (int i = 0; i < quads_count; ++i)
192 {
193 ChessBoardQuad & quad = quads[i];
194 for (int j = 0; j < 4; ++j)
195 {
196 line(img, quad.corners[j]->pt, quad.corners[(j + 1) & 3]->pt, Scalar(0, 240, 0), 1, LINE_AA);
197 }
198 }
199 imshow(name, img);
200#if DEBUG_CHESSBOARD_TIMEOUT
201 waitKey(DEBUG_CHESSBOARD_TIMEOUT);
202#else
203 while ((uchar)waitKey(0) != 'q') {}
204#endif
205}
206#else
207#define SHOW(...)
208#define SHOW_QUADS(...)
209#endif
210
211
212
213class ChessBoardDetector
214{
215public:
216 cv::Mat binarized_image;
217 Size pattern_size;
218
219 cv::AutoBuffer<ChessBoardQuad> all_quads;
220 cv::AutoBuffer<ChessBoardCorner> all_corners;
221
222 int all_quads_count;
223
224 struct NeighborsFinder {
225 const float thresh_sqr_scale = 2.f;
226 ChessBoardDetector& detector;
227 std::vector<int> neighbors_indices;
228 std::vector<float> neighbors_dists;
229 std::vector<Point2f> all_quads_pts;
230 flann::GenericIndex<flann::L2_Simple<float>> all_quads_pts_index;
231
232 NeighborsFinder(ChessBoardDetector& detector);
233
234 bool findCornerNeighbor(
235 const int quad_idx,
236 const int corner_idx,
237 const cv::Point2f& corner_pt,
238 float& min_sqr_dist,
239 const float sqr_radius,
240 int& closest_quad_idx,
241 int& closest_corner_idx,
242 cv::Point2f& closest_corner_pt);
243 };
244
245 ChessBoardDetector(const Size& pattern_size_) :
246 pattern_size(pattern_size_),
247 all_quads_count(0)
248 {
249 }
250
251 void reset()
252 {
253 all_quads.deallocate();
254 all_corners.deallocate();
255 all_quads_count = 0;
256 }
257
258 void generateQuads(const cv::Mat& image_, int flags, int dilations);
259
260 bool processQuads(std::vector<cv::Point2f>& out_corners, int &prev_sqr_size);
261
262 void findQuadNeighbors();
263
264 void findConnectedQuads(std::vector<ChessBoardQuad*>& out_group, int group_idx);
265
266 int checkQuadGroup(const std::vector<ChessBoardQuad*>& quad_group, std::vector<ChessBoardCorner*>& out_corners);
267
268 int cleanFoundConnectedQuads(std::vector<ChessBoardQuad*>& quad_group);
269
270 int orderFoundConnectedQuads(std::vector<ChessBoardQuad*>& quads);
271
272 void orderQuad(ChessBoardQuad& quad, ChessBoardCorner& corner, int common);
273
274#ifdef ENABLE_TRIM_COL_ROW
275 void trimCol(std::vector<ChessBoardQuad*>& quads, int col, int dir);
276 void trimRow(std::vector<ChessBoardQuad*>& quads, int row, int dir);
277#endif
278
279 int addOuterQuad(ChessBoardQuad& quad, std::vector<ChessBoardQuad*>& quads);
280
281 void removeQuadFromGroup(std::vector<ChessBoardQuad*>& quads, ChessBoardQuad& q0);
282
283 bool checkBoardMonotony(const std::vector<cv::Point2f>& corners);
284};
285
286/***************************************************************************************************/
287//COMPUTE INTENSITY HISTOGRAM OF INPUT IMAGE
288template<typename ArrayContainer>
289static void icvGetIntensityHistogram256(const Mat& img, ArrayContainer& piHist)
290{
291 for (int i = 0; i < 256; i++)
292 piHist[i] = 0;
293 // sum up all pixel in row direction and divide by number of columns
294 for (int j = 0; j < img.rows; ++j)
295 {
296 const uchar* row = img.ptr<uchar>(y: j);
297 for (int i = 0; i < img.cols; i++)
298 {
299 piHist[row[i]]++;
300 }
301 }
302}
303/***************************************************************************************************/
304//SMOOTH HISTOGRAM USING WINDOW OF SIZE 2*iWidth+1
305template<int iWidth_, typename ArrayContainer>
306static void icvSmoothHistogram256(const ArrayContainer& piHist, ArrayContainer& piHistSmooth, int iWidth = 0)
307{
308 CV_DbgAssert(iWidth_ == 0 || (iWidth == iWidth_ || iWidth == 0));
309 iWidth = (iWidth_ != 0) ? iWidth_ : iWidth;
310 CV_Assert(iWidth > 0);
311 CV_DbgAssert(piHist.size() == 256);
312 CV_DbgAssert(piHistSmooth.size() == 256);
313 for (int i = 0; i < 256; ++i)
314 {
315 int iIdx_min = std::max(a: 0, b: i - iWidth);
316 int iIdx_max = std::min(a: 255, b: i + iWidth);
317 int iSmooth = 0;
318 for (int iIdx = iIdx_min; iIdx <= iIdx_max; ++iIdx)
319 {
320 CV_DbgAssert(iIdx >= 0 && iIdx < 256);
321 iSmooth += piHist[iIdx];
322 }
323 piHistSmooth[i] = iSmooth/(iIdx_max-iIdx_min+1);
324 }
325}
326/***************************************************************************************************/
327//COMPUTE FAST HISTOGRAM GRADIENT
328template<typename ArrayContainer>
329static void icvGradientOfHistogram256(const ArrayContainer& piHist, ArrayContainer& piHistGrad)
330{
331 CV_DbgAssert(piHist.size() == 256);
332 CV_DbgAssert(piHistGrad.size() == 256);
333 piHistGrad[0] = 0;
334 int prev_grad = 0;
335 for (int i = 1; i < 255; ++i)
336 {
337 int grad = piHist[i-1] - piHist[i+1];
338 if (std::abs(x: grad) < 100)
339 {
340 if (prev_grad == 0)
341 grad = -100;
342 else
343 grad = prev_grad;
344 }
345 piHistGrad[i] = grad;
346 prev_grad = grad;
347 }
348 piHistGrad[255] = 0;
349}
350/***************************************************************************************************/
351//PERFORM SMART IMAGE THRESHOLDING BASED ON ANALYSIS OF INTENSITY HISTOGRAM
352static void icvBinarizationHistogramBased(Mat & img)
353{
354 CV_Assert(img.channels() == 1 && img.depth() == CV_8U);
355 int iCols = img.cols;
356 int iRows = img.rows;
357 int iMaxPix = iCols*iRows;
358 int iMaxPix1 = iMaxPix/100;
359 const int iNumBins = 256;
360 const int iMaxPos = 20;
361 cv::AutoBuffer<int, 256> piHistIntensity(iNumBins);
362 cv::AutoBuffer<int, 256> piHistSmooth(iNumBins);
363 cv::AutoBuffer<int, 256> piHistGrad(iNumBins);
364 cv::AutoBuffer<int> piMaxPos(iMaxPos);
365
366 icvGetIntensityHistogram256(img, piHist&: piHistIntensity);
367
368#if 0
369 // get accumulated sum starting from bright
370 cv::AutoBuffer<int, 256> piAccumSum(iNumBins);
371 piAccumSum[iNumBins-1] = piHistIntensity[iNumBins-1];
372 for (int i = iNumBins - 2; i >= 0; --i)
373 {
374 piAccumSum[i] = piHistIntensity[i] + piAccumSum[i+1];
375 }
376#endif
377
378 // first smooth the distribution
379 //const int iWidth = 1;
380 icvSmoothHistogram256<1>(piHist: piHistIntensity, piHistSmooth);
381
382 // compute gradient
383 icvGradientOfHistogram256(piHist: piHistSmooth, piHistGrad);
384
385 // check for zeros
386 unsigned iCntMaxima = 0;
387 for (int i = iNumBins-2; (i > 2) && (iCntMaxima < iMaxPos); --i)
388 {
389 if ((piHistGrad[i-1] < 0) && (piHistGrad[i] > 0))
390 {
391 int iSumAroundMax = piHistSmooth[i-1] + piHistSmooth[i] + piHistSmooth[i+1];
392 if (!(iSumAroundMax < iMaxPix1 && i < 64))
393 {
394 piMaxPos[iCntMaxima++] = i;
395 }
396 }
397 }
398
399 DPRINTF("HIST: MAXIMA COUNT: %d (%d, %d, %d, ...)", iCntMaxima,
400 iCntMaxima > 0 ? piMaxPos[0] : -1,
401 iCntMaxima > 1 ? piMaxPos[1] : -1,
402 iCntMaxima > 2 ? piMaxPos[2] : -1);
403
404 int iThresh = 0;
405
406 CV_Assert((size_t)iCntMaxima <= piMaxPos.size());
407
408 DPRINTF("HIST: MAXIMA COUNT: %d (%d, %d, %d, ...)", iCntMaxima,
409 iCntMaxima > 0 ? piMaxPos[0] : -1,
410 iCntMaxima > 1 ? piMaxPos[1] : -1,
411 iCntMaxima > 2 ? piMaxPos[2] : -1);
412
413 if (iCntMaxima == 0)
414 {
415 // no any maxima inside (only 0 and 255 which are not counted above)
416 // Does image black-write already?
417 const int iMaxPix2 = iMaxPix / 2;
418 for (int sum = 0, i = 0; i < 256; ++i) // select mean intensity
419 {
420 sum += piHistIntensity[i];
421 if (sum > iMaxPix2)
422 {
423 iThresh = i;
424 break;
425 }
426 }
427 }
428 else if (iCntMaxima == 1)
429 {
430 iThresh = piMaxPos[0]/2;
431 }
432 else if (iCntMaxima == 2)
433 {
434 iThresh = (piMaxPos[0] + piMaxPos[1])/2;
435 }
436 else // iCntMaxima >= 3
437 {
438 // CHECKING THRESHOLD FOR WHITE
439 int iIdxAccSum = 0, iAccum = 0;
440 for (int i = iNumBins - 1; i > 0; --i)
441 {
442 iAccum += piHistIntensity[i];
443 // iMaxPix/18 is about 5,5%, minimum required number of pixels required for white part of chessboard
444 if ( iAccum > (iMaxPix/18) )
445 {
446 iIdxAccSum = i;
447 break;
448 }
449 }
450
451 unsigned iIdxBGMax = 0;
452 int iBrightMax = piMaxPos[0];
453 // printf("iBrightMax = %d\n", iBrightMax);
454 for (unsigned n = 0; n < iCntMaxima - 1; ++n)
455 {
456 iIdxBGMax = n + 1;
457 if ( piMaxPos[n] < iIdxAccSum )
458 {
459 break;
460 }
461 iBrightMax = piMaxPos[n];
462 }
463
464 // CHECKING THRESHOLD FOR BLACK
465 int iMaxVal = piHistIntensity[piMaxPos[iIdxBGMax]];
466
467 //IF TOO CLOSE TO 255, jump to next maximum
468 if (piMaxPos[iIdxBGMax] >= 250 && iIdxBGMax + 1 < iCntMaxima)
469 {
470 iIdxBGMax++;
471 iMaxVal = piHistIntensity[piMaxPos[iIdxBGMax]];
472 }
473
474 for (unsigned n = iIdxBGMax + 1; n < iCntMaxima; n++)
475 {
476 if (piHistIntensity[piMaxPos[n]] >= iMaxVal)
477 {
478 iMaxVal = piHistIntensity[piMaxPos[n]];
479 iIdxBGMax = n;
480 }
481 }
482
483 //SETTING THRESHOLD FOR BINARIZATION
484 int iDist2 = (iBrightMax - piMaxPos[iIdxBGMax])/2;
485 iThresh = iBrightMax - iDist2;
486 DPRINTF("THRESHOLD SELECTED = %d, BRIGHTMAX = %d, DARKMAX = %d", iThresh, iBrightMax, piMaxPos[iIdxBGMax]);
487 }
488
489 if (iThresh > 0)
490 {
491 img = (img >= iThresh);
492 }
493}
494
495static std::vector<Point2f> getCornersFromQuads(ChessBoardQuad* p_all_quads, const int all_quads_count)
496{
497 std::vector<Point2f> all_quads_pts;
498 all_quads_pts.reserve(n: all_quads_count * 4);
499 for (int idx = 0; idx < all_quads_count; idx++)
500 {
501 const ChessBoardQuad& cur_quad = (const ChessBoardQuad&)p_all_quads[idx];
502 for (int i = 0; i < 4; i++)
503 all_quads_pts.push_back(x: cur_quad.corners[i]->pt);
504 }
505 return all_quads_pts;
506}
507
508ChessBoardDetector::NeighborsFinder::NeighborsFinder(ChessBoardDetector& _detector) :
509 detector(_detector),
510 all_quads_pts(getCornersFromQuads(p_all_quads: detector.all_quads.data(), all_quads_count: detector.all_quads_count)),
511 all_quads_pts_index(Mat(all_quads_pts).reshape(cn: 1, rows: detector.all_quads_count * 4), cvflann::KDTreeSingleIndexParams())
512{
513 const int all_corners_count = detector.all_quads_count * 4;
514 neighbors_indices.resize(new_size: all_corners_count);
515 neighbors_dists.resize(new_size: all_corners_count);
516}
517
518static double pointSideFromLine(const Point2f& line_direction_vector, const Point2f& vector)
519{
520 return line_direction_vector.cross(pt: vector);
521}
522
523static bool arePointsOnSameSideFromLine(const Point2f& line_pt1, const Point2f& line_pt2, const Point2f& pt1, const Point2f& pt2)
524{
525 const Point2f line_direction_vector = line_pt2 - line_pt1;
526 const Point2f vector1 = pt1 - line_pt1;
527 const Point2f vector2 = pt2 - line_pt1;
528 return pointSideFromLine(line_direction_vector, vector: vector1) * pointSideFromLine(line_direction_vector, vector: vector2) > 0.;
529}
530
531bool ChessBoardDetector::NeighborsFinder::findCornerNeighbor(
532 const int quad_idx,
533 const int corner_idx,
534 const cv::Point2f& corner_pt,
535 float& min_sqr_dist,
536 const float sqr_radius,
537 int& closest_quad_idx,
538 int& closest_corner_idx,
539 cv::Point2f& closest_corner_pt)
540{
541 ChessBoardQuad* p_all_quads = detector.all_quads.data();
542
543 const ChessBoardQuad& cur_quad = (const ChessBoardQuad&)p_all_quads[quad_idx];
544 int closest_neighbor_idx = -1;
545 ChessBoardQuad *closest_quad = 0;
546
547 // find the closest corner in all other quadrangles
548 const std::vector<float> query = { corner_pt.x, corner_pt.y };
549 const cvflann::SearchParams search_params(-1);
550 const int neighbors_count = all_quads_pts_index.radiusSearch(query, indices&: neighbors_indices, dists&: neighbors_dists, radius: sqr_radius, searchParams: search_params);
551
552 for (int neighbor_idx_idx = 0; neighbor_idx_idx < neighbors_count; neighbor_idx_idx++)
553 {
554 const int neighbor_idx = neighbors_indices[neighbor_idx_idx];
555 const int k = neighbor_idx >> 2;
556 if (k == quad_idx)
557 continue;
558
559 ChessBoardQuad& q_k = p_all_quads[k];
560 const int j = neighbor_idx & 3;
561 if (q_k.neighbors[j])
562 continue;
563
564 const Point2f neighbor_pt = all_quads_pts[neighbor_idx];
565 const float sqr_dist = normL2Sqr<float>(pt: corner_pt - neighbor_pt);
566 if (sqr_dist <= cur_quad.edge_sqr_len * thresh_sqr_scale &&
567 sqr_dist <= q_k.edge_sqr_len * thresh_sqr_scale)
568 {
569 // check edge lengths, make sure they're compatible
570 // edges that are different by more than 1:4 are rejected.
571 // edge_sqr_len is edge squared length, so we compare them
572 // with squared constant 16 = 4^2
573 if (q_k.edge_sqr_len > 16 * cur_quad.edge_sqr_len ||
574 cur_quad.edge_sqr_len > 16 * q_k.edge_sqr_len)
575 {
576 DPRINTF("Incompatible edge lengths");
577 continue;
578 }
579
580 const Point2f mid_pt1 = (cur_quad.corners[corner_idx]->pt + cur_quad.corners[(corner_idx + 1) & 3]->pt) / 2.f;
581 const Point2f mid_pt2 = (cur_quad.corners[(corner_idx + 2) & 3]->pt + cur_quad.corners[(corner_idx + 3) & 3]->pt) / 2.f;
582 if (!arePointsOnSameSideFromLine(line_pt1: mid_pt1, line_pt2: mid_pt2, pt1: corner_pt, pt2: neighbor_pt))
583 continue;
584
585 const Point2f mid_pt3 = (cur_quad.corners[(corner_idx + 1) & 3]->pt + cur_quad.corners[(corner_idx + 2) & 3]->pt) / 2.f;
586 const Point2f mid_pt4 = (cur_quad.corners[(corner_idx + 3) & 3]->pt + cur_quad.corners[corner_idx]->pt) / 2.f;
587 if (!arePointsOnSameSideFromLine(line_pt1: mid_pt3, line_pt2: mid_pt4, pt1: corner_pt, pt2: neighbor_pt))
588 continue;
589
590 const Point2f neighbor_pt_diagonal = q_k.corners[(j + 2) & 3]->pt;
591 if (!arePointsOnSameSideFromLine(line_pt1: mid_pt1, line_pt2: mid_pt2, pt1: corner_pt, pt2: neighbor_pt_diagonal))
592 continue;
593
594 if (!arePointsOnSameSideFromLine(line_pt1: mid_pt3, line_pt2: mid_pt4, pt1: corner_pt, pt2: neighbor_pt_diagonal))
595 continue;
596
597 closest_neighbor_idx = neighbor_idx;
598 closest_quad_idx = k;
599 closest_corner_idx = j;
600 closest_quad = &q_k;
601 min_sqr_dist = sqr_dist;
602 break;
603 }
604 }
605
606 // we found a matching corner point?
607 if (closest_neighbor_idx >= 0 && closest_quad_idx >= 0 && closest_corner_idx >= 0 && min_sqr_dist < FLT_MAX)
608 {
609 CV_Assert(closest_quad);
610
611 if (cur_quad.count >= 4 || closest_quad->count >= 4)
612 return false;
613
614 // If another point from our current quad is closer to the found corner
615 // than the current one, then we don't count this one after all.
616 // This is necessary to support small squares where otherwise the wrong
617 // corner will get matched to closest_quad;
618 closest_corner_pt = all_quads_pts[closest_neighbor_idx];
619
620 int j = 0;
621 for (; j < 4; j++)
622 {
623 if (cur_quad.neighbors[j] == closest_quad)
624 break;
625
626 if (normL2Sqr<float>(pt: closest_corner_pt - all_quads_pts[(quad_idx << 2) + j]) < min_sqr_dist)
627 break;
628 }
629 if (j < 4)
630 return false;
631
632 // Check that each corner is a neighbor of different quads
633 for(j = 0; j < 4; j++ )
634 {
635 if (closest_quad->neighbors[j] == &cur_quad)
636 break;
637 }
638 if (j < 4)
639 return false;
640
641 return true;
642 }
643
644 return false;
645}
646
647bool findChessboardCorners(InputArray image_, Size pattern_size,
648 OutputArray corners_, int flags)
649{
650 CV_INSTRUMENT_REGION();
651
652 DPRINTF("==== findChessboardCorners(img=%dx%d, pattern=%dx%d, flags=%d)",
653 image_.cols(), image_.rows(), pattern_size.width, pattern_size.height, flags);
654
655 bool found = false;
656
657 const bool is_plain = (flags & CALIB_CB_PLAIN) != 0;
658
659 int type = image_.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type);
660 Mat img = image_.getMat();
661
662 CV_CheckType(type, depth == CV_8U && (cn == 1 || cn == 3 || cn == 4),
663 "Only 8-bit grayscale or color images are supported");
664
665 if (pattern_size.width <= 2 || pattern_size.height <= 2)
666 CV_Error(Error::StsOutOfRange, "Both width and height of the pattern should have bigger than 2");
667
668 if (!corners_.needed())
669 CV_Error(Error::StsNullPtr, "Null pointer to corners");
670
671 std::vector<cv::Point2f> out_corners;
672
673 if (is_plain)
674 CV_CheckType(type, depth == CV_8U && cn == 1, "Only 8-bit grayscale images are supported whith CALIB_CB_PLAIN flag enable");
675
676 if (img.channels() != 1)
677 {
678 cvtColor(src: img, dst: img, code: COLOR_BGR2GRAY);
679 }
680
681 int prev_sqr_size = 0;
682
683 Mat thresh_img_new = img.clone();
684 if(!is_plain)
685 icvBinarizationHistogramBased(img&: thresh_img_new); // process image in-place
686 SHOW("New binarization", thresh_img_new);
687
688 if (flags & CALIB_CB_FAST_CHECK && !is_plain)
689 {
690 //perform new method for checking chessboard using a binary image.
691 //image is binarised using a threshold dependent on the image histogram
692 if (checkChessboardBinary(img: thresh_img_new, size: pattern_size) <= 0) //fall back to the old method
693 {
694 if (!checkChessboard(img, size: pattern_size))
695 {
696 corners_.release();
697 return false;
698 }
699 }
700 }
701
702 ChessBoardDetector detector(pattern_size);
703
704 const int min_dilations = 0;
705 const int max_dilations = is_plain ? 0 : 7;
706
707 // Try our standard "0" and "1" dilations, but if the pattern is not found, iterate the whole procedure with higher dilations.
708 // This is necessary because some squares simply do not separate properly without and with a single dilations. However,
709 // we want to use the minimum number of dilations possible since dilations cause the squares to become smaller,
710 // making it difficult to detect smaller squares.
711 for (int dilations = min_dilations; dilations <= max_dilations; dilations++)
712 {
713 //USE BINARY IMAGE COMPUTED USING icvBinarizationHistogramBased METHOD
714 if(!is_plain && dilations > 0)
715 dilate( src: thresh_img_new, dst: thresh_img_new, kernel: Mat(), anchor: Point(-1, -1), iterations: 1 );
716
717 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
718 // Otherwise FindContours will miss those clipped rectangle contours.
719 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
720 rectangle( img: thresh_img_new, pt1: Point(0,0), pt2: Point(thresh_img_new.cols-1, thresh_img_new.rows-1), color: Scalar(255,255,255), thickness: 3, lineType: LINE_8);
721
722 detector.reset();
723 detector.generateQuads(image_: thresh_img_new, flags, dilations);
724 DPRINTF("Quad count: %d/%d", detector.all_quads_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
725 SHOW_QUADS("New quads", thresh_img_new, &detector.all_quads[0], detector.all_quads_count);
726 if (detector.processQuads(out_corners, prev_sqr_size))
727 {
728 found = true;
729 break;
730 }
731 }
732
733 DPRINTF("Chessboard detection result 0: %d", (int)found);
734
735 // revert to old, slower, method if detection failed
736 if (!found && !is_plain)
737 {
738 if (flags & CALIB_CB_NORMALIZE_IMAGE)
739 {
740 img = img.clone();
741 equalizeHist(src: img, dst: img);
742 }
743
744 Mat thresh_img;
745 prev_sqr_size = 0;
746
747 DPRINTF("Fallback to old algorithm");
748 const bool useAdaptive = flags & CALIB_CB_ADAPTIVE_THRESH;
749 if (!useAdaptive)
750 {
751 // empiric threshold level
752 // thresholding performed here and not inside the cycle to save processing time
753 double mean = cv::mean(src: img).val[0];
754 int thresh_level = std::max(a: cvRound(value: mean - 10), b: 10);
755 threshold(src: img, dst: thresh_img, thresh: thresh_level, maxval: 255, type: THRESH_BINARY);
756 }
757 //if flag CALIB_CB_ADAPTIVE_THRESH is not set it doesn't make sense to iterate over k
758 int max_k = useAdaptive ? 6 : 1;
759 Mat prev_thresh_img;
760 for (int k = 0; k < max_k && !found; k++)
761 {
762 int prev_block_size = -1;
763 for (int dilations = min_dilations; dilations <= max_dilations; dilations++)
764 {
765 // convert the input grayscale image to binary (black-n-white)
766 if (useAdaptive)
767 {
768 int block_size = cvRound(value: prev_sqr_size == 0
769 ? std::min(a: img.cols, b: img.rows) * (k % 2 == 0 ? 0.2 : 0.1)
770 : prev_sqr_size * 2);
771 block_size = block_size | 1;
772 // convert to binary
773 if (block_size != prev_block_size)
774 {
775 adaptiveThreshold( src: img, dst: thresh_img, maxValue: 255, adaptiveMethod: ADAPTIVE_THRESH_MEAN_C, thresholdType: THRESH_BINARY, blockSize: block_size, C: (k/2)*5 );
776 dilate( src: thresh_img, dst: thresh_img, kernel: Mat(), anchor: Point(-1, -1), iterations: dilations );
777 thresh_img.copyTo(m: prev_thresh_img);
778 }
779 else if (dilations > 0)
780 {
781 dilate( src: prev_thresh_img, dst: prev_thresh_img, kernel: Mat(), anchor: Point(-1, -1), iterations: 1 );
782 prev_thresh_img.copyTo(m: thresh_img);
783 }
784 prev_block_size = block_size;
785 }
786 else
787 {
788 if (dilations > 0)
789 dilate( src: thresh_img, dst: thresh_img, kernel: Mat(), anchor: Point(-1, -1), iterations: 1 );
790 }
791 SHOW("Old binarization", thresh_img);
792
793 // So we can find rectangles that go to the edge, we draw a white line around the image edge.
794 // Otherwise FindContours will miss those clipped rectangle contours.
795 // The border color will be the image mean, because otherwise we risk screwing up filters like cvSmooth()...
796 rectangle( img: thresh_img, pt1: Point(0,0), pt2: Point(thresh_img.cols-1, thresh_img.rows-1), color: Scalar(255,255,255), thickness: 3, lineType: LINE_8);
797
798 detector.reset();
799 detector.generateQuads(image_: thresh_img, flags, dilations);
800 DPRINTF("Quad count: %d/%d", detector.all_quads_count, (pattern_size.width/2+1)*(pattern_size.height/2+1));
801 SHOW_QUADS("Old quads", thresh_img, &detector.all_quads[0], detector.all_quads_count);
802 if (detector.processQuads(out_corners, prev_sqr_size))
803 {
804 found = 1;
805 break;
806 }
807 }
808 }
809 }
810
811 DPRINTF("Chessboard detection result 1: %d", (int)found);
812
813 if (found)
814 found = detector.checkBoardMonotony(corners: out_corners);
815
816 DPRINTF("Chessboard detection result 2: %d", (int)found);
817
818 // check that none of the found corners is too close to the image boundary
819 if (found)
820 {
821 const int BORDER = 8;
822 for (int k = 0; k < pattern_size.width*pattern_size.height; ++k)
823 {
824 if( out_corners[k].x <= BORDER || out_corners[k].x > img.cols - BORDER ||
825 out_corners[k].y <= BORDER || out_corners[k].y > img.rows - BORDER )
826 {
827 found = false;
828 break;
829 }
830 }
831 }
832
833 DPRINTF("Chessboard detection result 3: %d", (int)found);
834
835 if (found)
836 {
837 if ((pattern_size.height & 1) == 0 && (pattern_size.width & 1) == 0 )
838 {
839 int last_row = (pattern_size.height-1)*pattern_size.width;
840 double dy0 = out_corners[last_row].y - out_corners[0].y;
841 if (dy0 < 0)
842 {
843 int n = pattern_size.width*pattern_size.height;
844 for(int i = 0; i < n/2; i++ )
845 {
846 std::swap(a&: out_corners[i], b&: out_corners[n-i-1]);
847 }
848 }
849 }
850 cv::cornerSubPix(image: img, corners: out_corners, winSize: Size(2, 2), zeroZone: Size(-1,-1),
851 criteria: cv::TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 15, 0.1));
852 }
853
854 Mat(out_corners).copyTo(m: corners_);
855 return found;
856}
857
858//
859// Checks that each board row and column is pretty much monotonous curve:
860// It analyzes each row and each column of the chessboard as following:
861// for each corner c lying between end points in the same row/column it checks that
862// the point projection to the line segment (a,b) is lying between projections
863// of the neighbor corners in the same row/column.
864//
865// This function has been created as temporary workaround for the bug in current implementation
866// of cvFindChessboardCorners that produces absolutely unordered sets of corners.
867//
868bool ChessBoardDetector::checkBoardMonotony(const std::vector<cv::Point2f>& corners)
869{
870 for (int k = 0; k < 2; ++k)
871 {
872 int max_i = (k == 0 ? pattern_size.height : pattern_size.width);
873 int max_j = (k == 0 ? pattern_size.width: pattern_size.height) - 1;
874 for (int i = 0; i < max_i; ++i)
875 {
876 cv::Point2f a = k == 0 ? corners[i*pattern_size.width] : corners[i];
877 cv::Point2f b = k == 0 ? corners[(i+1)*pattern_size.width-1]
878 : corners[(pattern_size.height-1)*pattern_size.width + i];
879 float dx0 = b.x - a.x, dy0 = b.y - a.y;
880 if (fabs(x: dx0) + fabs(x: dy0) < FLT_EPSILON)
881 return false;
882 float prevt = 0;
883 for (int j = 1; j < max_j; ++j)
884 {
885 cv::Point2f c = k == 0 ? corners[i*pattern_size.width + j]
886 : corners[j*pattern_size.width + i];
887 float t = ((c.x - a.x)*dx0 + (c.y - a.y)*dy0)/(dx0*dx0 + dy0*dy0);
888 if (t < prevt || t > 1)
889 return false;
890 prevt = t;
891 }
892 }
893 }
894 return true;
895}
896
897//
898// order a group of connected quads
899// order of corners:
900// 0 is top left
901// clockwise from there
902// note: "top left" is nominal, depends on initial ordering of starting quad
903// but all other quads are ordered consistently
904//
905// can change the number of quads in the group
906// can add quads, so we need to have quad/corner arrays passed in
907//
908int ChessBoardDetector::orderFoundConnectedQuads(std::vector<ChessBoardQuad*>& quads)
909{
910 const int max_quad_buf_size = (int)all_quads.size();
911 int quad_count = (int)quads.size();
912
913 std::stack<ChessBoardQuad*> stack;
914
915 // first find an interior quad
916 ChessBoardQuad *start = NULL;
917 for (int i = 0; i < quad_count; i++)
918 {
919 if (quads[i]->count == 4)
920 {
921 start = quads[i];
922 break;
923 }
924 }
925
926 if (start == NULL)
927 return 0; // no 4-connected quad
928
929 // start with first one, assign rows/cols
930 int row_min = 0, col_min = 0, row_max=0, col_max = 0;
931
932 std::map<int, int> col_hist;
933 std::map<int, int> row_hist;
934
935 stack.push(x: start);
936 start->row = 0;
937 start->col = 0;
938 start->ordered = true;
939
940 // Recursively order the quads so that all position numbers (e.g.,
941 // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
942
943 while (!stack.empty())
944 {
945 ChessBoardQuad* q = stack.top(); stack.pop(); CV_Assert(q);
946
947 int col = q->col;
948 int row = q->row;
949 col_hist[col]++;
950 row_hist[row]++;
951
952 // check min/max
953 if (row > row_max) row_max = row;
954 if (row < row_min) row_min = row;
955 if (col > col_max) col_max = col;
956 if (col < col_min) col_min = col;
957
958 for (int i = 0; i < 4; i++)
959 {
960 CV_DbgAssert(q);
961 ChessBoardQuad *neighbor = q->neighbors[i];
962 switch(i) // adjust col, row for this quad
963 { // start at top left, go clockwise
964 case 0:
965 row--; col--; break;
966 case 1:
967 col += 2; break;
968 case 2:
969 row += 2; break;
970 case 3:
971 col -= 2; break;
972 }
973
974 // just do inside quads
975 if (neighbor && neighbor->ordered == false && neighbor->count == 4)
976 {
977 DPRINTF("col: %d row: %d", col, row);
978 CV_Assert(q->corners[i]);
979 orderQuad(quad&: *neighbor, corner&: *(q->corners[i]), common: (i+2)&3); // set in order
980 neighbor->ordered = true;
981 neighbor->row = row;
982 neighbor->col = col;
983 stack.push(x: neighbor);
984 }
985 }
986 }
987
988#ifdef DEBUG_CHESSBOARD
989 for (int i = col_min; i <= col_max; i++)
990 DPRINTF("HIST[%d] = %d", i, col_hist[i]);
991#endif
992
993 // analyze inner quad structure
994 int w = pattern_size.width - 1;
995 int h = pattern_size.height - 1;
996 int drow = row_max - row_min + 1;
997 int dcol = col_max - col_min + 1;
998
999 // normalize pattern and found quad indices
1000 if ((w > h && dcol < drow) ||
1001 (w < h && drow < dcol))
1002 {
1003 h = pattern_size.width - 1;
1004 w = pattern_size.height - 1;
1005 }
1006
1007 DPRINTF("Size: %dx%d Pattern: %dx%d", dcol, drow, w, h);
1008
1009 // check if there are enough inner quads
1010 if (dcol < w || drow < h) // found enough inner quads?
1011 {
1012 DPRINTF("Too few inner quad rows/cols");
1013 return 0; // no, return
1014 }
1015#ifdef ENABLE_TRIM_COL_ROW
1016 // too many columns, not very common
1017 if (dcol == w+1) // too many, trim
1018 {
1019 DPRINTF("Trimming cols");
1020 if (col_hist[col_max] > col_hist[col_min])
1021 {
1022 DPRINTF("Trimming left col");
1023 trimCol(quads, col_min, -1);
1024 }
1025 else
1026 {
1027 DPRINTF("Trimming right col");
1028 trimCol(quads, col_max, +1);
1029 }
1030 }
1031
1032 // too many rows, not very common
1033 if (drow == h+1) // too many, trim
1034 {
1035 DPRINTF("Trimming rows");
1036 if (row_hist[row_max] > row_hist[row_min])
1037 {
1038 DPRINTF("Trimming top row");
1039 trimRow(quads, row_min, -1);
1040 }
1041 else
1042 {
1043 DPRINTF("Trimming bottom row");
1044 trimRow(quads, row_max, +1);
1045 }
1046 }
1047
1048 quad_count = (int)quads.size(); // update after icvTrimCol/icvTrimRow
1049#endif
1050
1051 // check edges of inner quads
1052 // if there is an outer quad missing, fill it in
1053 // first order all inner quads
1054 int found = 0;
1055 for (int i=0; i < quad_count; ++i)
1056 {
1057 ChessBoardQuad& q = *quads[i];
1058 if (q.count != 4)
1059 continue;
1060
1061 { // ok, look at neighbors
1062 int col = q.col;
1063 int row = q.row;
1064 for (int j = 0; j < 4; j++)
1065 {
1066 switch(j) // adjust col, row for this quad
1067 { // start at top left, go clockwise
1068 case 0:
1069 row--; col--; break;
1070 case 1:
1071 col += 2; break;
1072 case 2:
1073 row += 2; break;
1074 case 3:
1075 col -= 2; break;
1076 }
1077 ChessBoardQuad *neighbor = q.neighbors[j];
1078 if (neighbor && !neighbor->ordered && // is it an inner quad?
1079 col <= col_max && col >= col_min &&
1080 row <= row_max && row >= row_min)
1081 {
1082 // if so, set in order
1083 DPRINTF("Adding inner: col: %d row: %d", col, row);
1084 found++;
1085 CV_Assert(q.corners[j]);
1086 orderQuad(quad&: *neighbor, corner&: *q.corners[j], common: (j+2)&3);
1087 neighbor->ordered = true;
1088 neighbor->row = row;
1089 neighbor->col = col;
1090 }
1091 }
1092 }
1093 }
1094
1095 // if we have found inner quads, add corresponding outer quads,
1096 // which are missing
1097 if (found > 0)
1098 {
1099 DPRINTF("Found %d inner quads not connected to outer quads, repairing", found);
1100 for (int i = 0; i < quad_count && all_quads_count < max_quad_buf_size; i++)
1101 {
1102 ChessBoardQuad& q = *quads[i];
1103 if (q.count < 4 && q.ordered)
1104 {
1105 int added = addOuterQuad(quad&: q, quads);
1106 quad_count += added;
1107 }
1108 }
1109
1110 if (all_quads_count >= max_quad_buf_size)
1111 return 0;
1112 }
1113
1114
1115 // final trimming of outer quads
1116 if (dcol == w && drow == h) // found correct inner quads
1117 {
1118 DPRINTF("Inner bounds ok, check outer quads");
1119 for (int i = quad_count - 1; i >= 0; i--) // eliminate any quad not connected to an ordered quad
1120 {
1121 ChessBoardQuad& q = *quads[i];
1122 if (q.ordered == false)
1123 {
1124 bool outer = false;
1125 for (int j=0; j<4; j++) // any neighbors that are ordered?
1126 {
1127 if (q.neighbors[j] && q.neighbors[j]->ordered)
1128 outer = true;
1129 }
1130 if (!outer) // not an outer quad, eliminate
1131 {
1132 DPRINTF("Removing quad %d", i);
1133 removeQuadFromGroup(quads, q0&: q);
1134 }
1135 }
1136
1137 }
1138 return (int)quads.size();
1139 }
1140
1141 return 0;
1142}
1143
1144
1145// add an outer quad
1146// looks for the neighbor of <quad> that isn't present,
1147// tries to add it in.
1148// <quad> is ordered
1149int ChessBoardDetector::addOuterQuad(ChessBoardQuad& quad, std::vector<ChessBoardQuad*>& quads)
1150{
1151 int added = 0;
1152 int max_quad_buf_size = (int)all_quads.size();
1153
1154 for (int i = 0; i < 4 && all_quads_count < max_quad_buf_size; i++) // find no-neighbor corners
1155 {
1156 if (!quad.neighbors[i]) // ok, create and add neighbor
1157 {
1158 int j = (i+2)&3;
1159 DPRINTF("Adding quad as neighbor 2");
1160 int q_index = all_quads_count++;
1161 ChessBoardQuad& q = all_quads[q_index];
1162 q = ChessBoardQuad(0);
1163 added++;
1164 quads.push_back(x: &q);
1165
1166 // set neighbor and group id
1167 quad.neighbors[i] = &q;
1168 quad.count += 1;
1169 q.neighbors[j] = &quad;
1170 q.group_idx = quad.group_idx;
1171 q.count = 1; // number of neighbors
1172 q.ordered = false;
1173 q.edge_sqr_len = quad.edge_sqr_len;
1174
1175 // make corners of new quad
1176 // same as neighbor quad, but offset
1177 const cv::Point2f pt_offset = quad.corners[i]->pt - quad.corners[j]->pt;
1178 for (int k = 0; k < 4; k++)
1179 {
1180 ChessBoardCorner& corner = (ChessBoardCorner&)all_corners[q_index * 4 + k];
1181 const cv::Point2f& pt = quad.corners[k]->pt;
1182 corner = ChessBoardCorner(pt);
1183 q.corners[k] = &corner;
1184 corner.pt += pt_offset;
1185 }
1186 // have to set exact corner
1187 q.corners[j] = quad.corners[i];
1188
1189 // set row and col for next step check
1190 switch (i)
1191 {
1192 case 0:
1193 q.col = quad.col - 1; q.row = quad.row - 1; break;
1194 case 1:
1195 q.col = quad.col + 1; q.row = quad.row - 1; break;
1196 case 2:
1197 q.col = quad.col + 1; q.row = quad.row - 1; break;
1198 case 3:
1199 q.col = quad.col - 1; q.row = quad.row + 1; break;
1200 }
1201
1202 // now find other neighbor and add it, if possible
1203 for (int k = 1; k <= 3; k += 2)
1204 {
1205 int next_i = (i + k) % 4;
1206 int prev_i = (i + k + 2) % 4;
1207 ChessBoardQuad* quad_prev = quad.neighbors[prev_i];
1208 if (quad_prev &&
1209 quad_prev->ordered &&
1210 quad_prev->neighbors[i] &&
1211 quad_prev->neighbors[i]->ordered &&
1212 std::abs(x: quad_prev->neighbors[i]->col - q.col) == 1 &&
1213 std::abs(x: quad_prev->neighbors[i]->row - q.row) == 1)
1214 {
1215 ChessBoardQuad* qn = quad_prev->neighbors[i];
1216 q.count = 2;
1217 q.neighbors[prev_i] = qn;
1218 qn->neighbors[next_i] = &q;
1219 qn->count += 1;
1220 // have to set exact corner
1221 q.corners[prev_i] = qn->corners[next_i];
1222 }
1223 }
1224 }
1225 }
1226 return added;
1227}
1228
1229
1230// trimming routines
1231#ifdef ENABLE_TRIM_COL_ROW
1232void ChessBoardDetector::trimCol(std::vector<ChessBoardQuad*>& quads, int col, int dir)
1233{
1234 std::vector<ChessBoardQuad*> quads_(quads);
1235 // find the right quad(s)
1236 for (size_t i = 0; i < quads_.size(); ++i)
1237 {
1238 ChessBoardQuad& q = *quads_[i];
1239#ifdef DEBUG_CHESSBOARD
1240 if (q.ordered)
1241 DPRINTF("i: %d index: %d cur: %d", (int)i, col, q.col);
1242#endif
1243 if (q.ordered && q.col == col)
1244 {
1245 if (dir == 1)
1246 {
1247 if (q.neighbors[1])
1248 {
1249 removeQuadFromGroup(quads, *q.neighbors[1]);
1250 }
1251 if (q.neighbors[2])
1252 {
1253 removeQuadFromGroup(quads, *q.neighbors[2]);
1254 }
1255 }
1256 else
1257 {
1258 if (q.neighbors[0])
1259 {
1260 removeQuadFromGroup(quads, *q.neighbors[0]);
1261 }
1262 if (q.neighbors[3])
1263 {
1264 removeQuadFromGroup(quads, *q.neighbors[3]);
1265 }
1266 }
1267 }
1268 }
1269}
1270
1271void ChessBoardDetector::trimRow(std::vector<ChessBoardQuad*>& quads, int row, int dir)
1272{
1273 std::vector<ChessBoardQuad*> quads_(quads);
1274 // find the right quad(s)
1275 for (size_t i = 0; i < quads_.size(); ++i)
1276 {
1277 ChessBoardQuad& q = *quads_[i];
1278#ifdef DEBUG_CHESSBOARD
1279 if (q.ordered)
1280 DPRINTF("i: %d index: %d cur: %d", (int)i, row, q.row);
1281#endif
1282 if (q.ordered && q.row == row)
1283 {
1284 if (dir == 1) // remove from bottom
1285 {
1286 if (q.neighbors[2])
1287 {
1288 removeQuadFromGroup(quads, *q.neighbors[2]);
1289 }
1290 if (q.neighbors[3])
1291 {
1292 removeQuadFromGroup(quads, *q.neighbors[3]);
1293 }
1294 }
1295 else // remove from top
1296 {
1297 if (q.neighbors[0])
1298 {
1299 removeQuadFromGroup(quads, *q.neighbors[0]);
1300 }
1301 if (q.neighbors[1])
1302 {
1303 removeQuadFromGroup(quads, *q.neighbors[1]);
1304 }
1305 }
1306
1307 }
1308 }
1309}
1310#endif
1311
1312//
1313// remove quad from quad group
1314//
1315void ChessBoardDetector::removeQuadFromGroup(std::vector<ChessBoardQuad*>& quads, ChessBoardQuad& q0)
1316{
1317 const int count = (int)quads.size();
1318
1319 int self_idx = -1;
1320
1321 // remove any references to this quad as a neighbor
1322 for (int i = 0; i < count; ++i)
1323 {
1324 ChessBoardQuad* q = quads[i];
1325 if (q == &q0)
1326 self_idx = i;
1327 for (int j = 0; j < 4; j++)
1328 {
1329 if (q->neighbors[j] == &q0)
1330 {
1331 q->neighbors[j] = NULL;
1332 q->count--;
1333 for (int k = 0; k < 4; ++k)
1334 {
1335 if (q0.neighbors[k] == q)
1336 {
1337 q0.neighbors[k] = 0;
1338 q0.count--;
1339#ifndef _DEBUG
1340 break;
1341#endif
1342 }
1343 }
1344 break;
1345 }
1346 }
1347 }
1348 CV_Assert(self_idx >= 0); // item itself should be found
1349
1350 // remove the quad
1351 if (self_idx != count-1)
1352 quads[self_idx] = quads[count-1];
1353 quads.resize(new_size: count - 1);
1354}
1355
1356//
1357// put quad into correct order, where <corner> has value <common>
1358//
1359void ChessBoardDetector::orderQuad(ChessBoardQuad& quad, ChessBoardCorner& corner, int common)
1360{
1361 CV_DbgAssert(common >= 0 && common <= 3);
1362
1363 // find the corner
1364 int tc = 0;;
1365 for (; tc < 4; ++tc)
1366 if (quad.corners[tc]->pt == corner.pt)
1367 break;
1368
1369 // set corner order
1370 // shift
1371 while (tc != common)
1372 {
1373 // shift by one
1374 ChessBoardCorner *tempc = quad.corners[3];
1375 ChessBoardQuad *tempq = quad.neighbors[3];
1376 for (int i = 3; i > 0; --i)
1377 {
1378 quad.corners[i] = quad.corners[i-1];
1379 quad.neighbors[i] = quad.neighbors[i-1];
1380 }
1381 quad.corners[0] = tempc;
1382 quad.neighbors[0] = tempq;
1383 tc = (tc + 1) & 3;
1384 }
1385}
1386
1387
1388// if we found too many connect quads, remove those which probably do not belong.
1389int ChessBoardDetector::cleanFoundConnectedQuads(std::vector<ChessBoardQuad*>& quad_group)
1390{
1391 // number of quads this pattern should contain
1392 int count = ((pattern_size.width + 1)*(pattern_size.height + 1) + 1)/2;
1393
1394 // if we have more quadrangles than we should,
1395 // try to eliminate duplicates or ones which don't belong to the pattern rectangle...
1396 int quad_count = (int)quad_group.size();
1397 if (quad_count <= count)
1398 return quad_count;
1399 CV_DbgAssert(quad_count > 0);
1400
1401 // create an array of quadrangle centers
1402 cv::AutoBuffer<cv::Point2f> centers(quad_count);
1403
1404 cv::Point2f center;
1405 for (int i = 0; i < quad_count; ++i)
1406 {
1407 ChessBoardQuad* q = quad_group[i];
1408
1409 const cv::Point2f ci = (
1410 q->corners[0]->pt +
1411 q->corners[1]->pt +
1412 q->corners[2]->pt +
1413 q->corners[3]->pt
1414 ) * 0.25f;
1415
1416 centers[i] = ci;
1417 center += ci;
1418 }
1419 center *= (1.0f / quad_count);
1420
1421 // If we still have more quadrangles than we should,
1422 // we try to eliminate bad ones based on minimizing the bounding box.
1423 // We iteratively remove the point which reduces the size of
1424 // the bounding box of the blobs the most
1425 // (since we want the rectangle to be as small as possible)
1426 // remove the quadrangle that causes the biggest reduction
1427 // in pattern size until we have the correct number
1428 while (quad_count > count)
1429 {
1430 double min_box_area = DBL_MAX;
1431 int min_box_area_index = -1;
1432
1433 // For each point, calculate box area without that point
1434 for (int skip = 0; skip < quad_count; ++skip)
1435 {
1436 // get bounding rectangle
1437 cv::Point2f temp = centers[skip]; // temporarily make index 'skip' the same as
1438 centers[skip] = center; // pattern center (so it is not counted for convex hull)
1439 std::vector<Point2f> hull;
1440 Mat points(1, quad_count, CV_32FC2, &centers[0]);
1441 cv::convexHull(points, hull, clockwise: true);
1442 centers[skip] = temp;
1443 double hull_area = contourArea(contour: hull, oriented: false);
1444
1445 // remember smallest box area
1446 if (hull_area < min_box_area)
1447 {
1448 min_box_area = hull_area;
1449 min_box_area_index = skip;
1450 }
1451 }
1452
1453 ChessBoardQuad *q0 = quad_group[min_box_area_index];
1454
1455 // remove any references to this quad as a neighbor
1456 for (int i = 0; i < quad_count; ++i)
1457 {
1458 ChessBoardQuad *q = quad_group[i];
1459 CV_DbgAssert(q);
1460 for (int j = 0; j < 4; ++j)
1461 {
1462 if (q->neighbors[j] == q0)
1463 {
1464 q->neighbors[j] = 0;
1465 q->count--;
1466 for (int k = 0; k < 4; ++k)
1467 {
1468 if (q0->neighbors[k] == q)
1469 {
1470 q0->neighbors[k] = 0;
1471 q0->count--;
1472 break;
1473 }
1474 }
1475 break;
1476 }
1477 }
1478 }
1479
1480 // remove the quad
1481 quad_count--;
1482 quad_group[min_box_area_index] = quad_group[quad_count];
1483 centers[min_box_area_index] = centers[quad_count];
1484 }
1485 quad_group.resize(new_size: quad_count);
1486
1487 return quad_count;
1488}
1489
1490
1491
1492void ChessBoardDetector::findConnectedQuads(std::vector<ChessBoardQuad*>& out_group, int group_idx)
1493{
1494 out_group.clear();
1495
1496 std::stack<ChessBoardQuad*> stack;
1497
1498 int i = 0;
1499 for (; i < all_quads_count; i++)
1500 {
1501 ChessBoardQuad* q = (ChessBoardQuad*)&all_quads[i];
1502
1503 // Scan the array for a first unlabeled quad
1504 if (q->count <= 0 || q->group_idx >= 0) continue;
1505
1506 // Recursively find a group of connected quads starting from the seed all_quads[i]
1507 stack.push(x: q);
1508 out_group.push_back(x: q);
1509 q->group_idx = group_idx;
1510 q->ordered = false;
1511
1512 while (!stack.empty())
1513 {
1514 q = stack.top(); CV_Assert(q);
1515 stack.pop();
1516 for (int k = 0; k < 4; k++ )
1517 {
1518 CV_DbgAssert(q);
1519 ChessBoardQuad *neighbor = q->neighbors[k];
1520 if (neighbor && neighbor->count > 0 && neighbor->group_idx < 0 )
1521 {
1522 stack.push(x: neighbor);
1523 out_group.push_back(x: neighbor);
1524 neighbor->group_idx = group_idx;
1525 neighbor->ordered = false;
1526 }
1527 }
1528 }
1529 break;
1530 }
1531}
1532
1533
1534int ChessBoardDetector::checkQuadGroup(const std::vector<ChessBoardQuad*>& quad_group, std::vector<ChessBoardCorner*>& out_corners)
1535{
1536 const int ROW1 = 1000000;
1537 const int ROW2 = 2000000;
1538 const int ROW_ = 3000000;
1539
1540 int quad_count = (int)quad_group.size();
1541
1542 std::vector<ChessBoardCorner*> corners(quad_count*4);
1543 int corner_count = 0;
1544 int result = 0;
1545
1546 int width = 0, height = 0;
1547 int hist[5] = {0,0,0,0,0};
1548 //ChessBoardCorner* first = 0, *first2 = 0, *right, *cur, *below, *c;
1549
1550 // build dual graph, which vertices are internal quad corners
1551 // and two vertices are connected iff they lie on the same quad edge
1552 for (int i = 0; i < quad_count; ++i)
1553 {
1554 ChessBoardQuad* q = quad_group[i];
1555 /*CvScalar color = q->count == 0 ? cvScalar(0,255,255) :
1556 q->count == 1 ? cvScalar(0,0,255) :
1557 q->count == 2 ? cvScalar(0,255,0) :
1558 q->count == 3 ? cvScalar(255,255,0) :
1559 cvScalar(255,0,0);*/
1560
1561 for (int j = 0; j < 4; ++j)
1562 {
1563 if (q->neighbors[j])
1564 {
1565 int next_j = (j + 1) & 3;
1566 ChessBoardCorner *a = q->corners[j], *b = q->corners[next_j];
1567 // mark internal corners that belong to:
1568 // - a quad with a single neighbor - with ROW1,
1569 // - a quad with two neighbors - with ROW2
1570 // make the rest of internal corners with ROW_
1571 int row_flag = q->count == 1 ? ROW1 : q->count == 2 ? ROW2 : ROW_;
1572
1573 if (a->row == 0)
1574 {
1575 corners[corner_count++] = a;
1576 a->row = row_flag;
1577 }
1578 else if (a->row > row_flag)
1579 {
1580 a->row = row_flag;
1581 }
1582
1583 if (q->neighbors[next_j])
1584 {
1585 if (a->count >= 4 || b->count >= 4)
1586 goto finalize;
1587 for (int k = 0; k < 4; ++k)
1588 {
1589 if (a->neighbors[k] == b)
1590 goto finalize;
1591 if (b->neighbors[k] == a)
1592 goto finalize;
1593 }
1594 a->neighbors[a->count++] = b;
1595 b->neighbors[b->count++] = a;
1596 }
1597 }
1598 }
1599 }
1600
1601 if (corner_count != pattern_size.width*pattern_size.height)
1602 goto finalize;
1603
1604{
1605 ChessBoardCorner* first = NULL, *first2 = NULL;
1606 for (int i = 0; i < corner_count; ++i)
1607 {
1608 int n = corners[i]->count;
1609 CV_DbgAssert(0 <= n && n <= 4);
1610 hist[n]++;
1611 if (!first && n == 2)
1612 {
1613 if (corners[i]->row == ROW1)
1614 first = corners[i];
1615 else if (!first2 && corners[i]->row == ROW2)
1616 first2 = corners[i];
1617 }
1618 }
1619
1620 // start with a corner that belongs to a quad with a single neighbor.
1621 // if we do not have such, start with a corner of a quad with two neighbors.
1622 if( !first )
1623 first = first2;
1624
1625 if( !first || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1626 hist[3] != (pattern_size.width + pattern_size.height)*2 - 8 )
1627 goto finalize;
1628
1629 ChessBoardCorner* cur = first;
1630 ChessBoardCorner* right = NULL;
1631 ChessBoardCorner* below = NULL;
1632 out_corners.clear();
1633 out_corners.push_back(x: cur);
1634
1635 for (int k = 0; k < 4; ++k)
1636 {
1637 ChessBoardCorner* c = cur->neighbors[k];
1638 if (c)
1639 {
1640 if (!right)
1641 right = c;
1642 else if (!below)
1643 below = c;
1644 }
1645 }
1646
1647 if( !right || (right->count != 2 && right->count != 3) ||
1648 !below || (below->count != 2 && below->count != 3) )
1649 goto finalize;
1650
1651 cur->row = 0;
1652
1653 first = below; // remember the first corner in the next row
1654
1655 // find and store the first row (or column)
1656 while( 1 )
1657 {
1658 right->row = 0;
1659 out_corners.push_back(x: right);
1660 if( right->count == 2 )
1661 break;
1662 if( right->count != 3 || (int)out_corners.size() >= std::max(a: pattern_size.width,b: pattern_size.height) )
1663 goto finalize;
1664 cur = right;
1665 for (int k = 0; k < 4; ++k)
1666 {
1667 ChessBoardCorner* c = cur->neighbors[k];
1668 if (c && c->row > 0)
1669 {
1670 int kk = 0;
1671 for (; kk < 4; ++kk)
1672 {
1673 if (c->neighbors[kk] == below)
1674 break;
1675 }
1676 if (kk < 4)
1677 below = c;
1678 else
1679 right = c;
1680 }
1681 }
1682 }
1683
1684 width = (int)out_corners.size();
1685 if (width == pattern_size.width)
1686 height = pattern_size.height;
1687 else if (width == pattern_size.height)
1688 height = pattern_size.width;
1689 else
1690 goto finalize;
1691
1692 // find and store all the other rows
1693 for (int i = 1; ; ++i)
1694 {
1695 if( !first )
1696 break;
1697 cur = first;
1698 first = 0;
1699 int j = 0;
1700 for (; ; ++j)
1701 {
1702 cur->row = i;
1703 out_corners.push_back(x: cur);
1704 if (cur->count == 2 + (i < height-1) && j > 0)
1705 break;
1706
1707 right = 0;
1708
1709 // find a neighbor that has not been processed yet
1710 // and that has a neighbor from the previous row
1711 for (int k = 0; k < 4; ++k)
1712 {
1713 ChessBoardCorner* c = cur->neighbors[k];
1714 if (c && c->row > i)
1715 {
1716 int kk = 0;
1717 for (; kk < 4; ++kk)
1718 {
1719 if (c->neighbors[kk] && c->neighbors[kk]->row == i-1)
1720 break;
1721 }
1722 if(kk < 4)
1723 {
1724 right = c;
1725 if (j > 0)
1726 break;
1727 }
1728 else if (j == 0)
1729 first = c;
1730 }
1731 }
1732 if (!right)
1733 goto finalize;
1734 cur = right;
1735 }
1736
1737 if (j != width - 1)
1738 goto finalize;
1739 }
1740
1741 if ((int)out_corners.size() != corner_count)
1742 goto finalize;
1743
1744 // check if we need to transpose the board
1745 if (width != pattern_size.width)
1746 {
1747 std::swap(a&: width, b&: height);
1748
1749 std::vector<ChessBoardCorner*> tmp(out_corners);
1750 for (int i = 0; i < height; ++i)
1751 for (int j = 0; j < width; ++j)
1752 out_corners[i*width + j] = tmp[j*height + i];
1753 }
1754
1755 // check if we need to revert the order in each row
1756 {
1757 cv::Point2f p0 = out_corners[0]->pt,
1758 p1 = out_corners[pattern_size.width-1]->pt,
1759 p2 = out_corners[pattern_size.width]->pt;
1760 if( (p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x) < 0 )
1761 {
1762 if (width % 2 == 0)
1763 {
1764 for (int i = 0; i < height; ++i)
1765 for (int j = 0; j < width/2; ++j)
1766 std::swap(a&: out_corners[i*width+j], b&: out_corners[i*width+width-j-1]);
1767 }
1768 else
1769 {
1770 for (int j = 0; j < width; ++j)
1771 for (int i = 0; i < height/2; ++i)
1772 std::swap(a&: out_corners[i*width+j], b&: out_corners[(height - i - 1)*width+j]);
1773 }
1774 }
1775 }
1776
1777 result = corner_count;
1778}
1779
1780finalize:
1781 if (result <= 0)
1782 {
1783 corner_count = std::min(a: corner_count, b: pattern_size.area());
1784 out_corners.resize(new_size: corner_count);
1785 for (int i = 0; i < corner_count; i++)
1786 out_corners[i] = corners[i];
1787
1788 result = -corner_count;
1789
1790 if (result == -pattern_size.area())
1791 result = -result;
1792 }
1793
1794 return result;
1795}
1796
1797
1798
1799void ChessBoardDetector::findQuadNeighbors()
1800{
1801 NeighborsFinder neighborsFinder(*this);
1802 for (int idx = 0; idx < all_quads_count; idx++)
1803 {
1804 ChessBoardQuad& cur_quad = (ChessBoardQuad&)all_quads[idx];
1805
1806 // choose the points of the current quadrangle that are close to
1807 // some points of the other quadrangles
1808 // (it can happen for split corners (due to dilation) of the
1809 // checker board). Search only in other quadrangles!
1810
1811 // for each corner of this quadrangle
1812 for (int i = 0; i < 4; i++)
1813 {
1814 if (cur_quad.neighbors[i])
1815 continue;
1816
1817 const cv::Point2f pt = neighborsFinder.all_quads_pts[(idx << 2) + i];
1818
1819 float min_sqr_dist = FLT_MAX;
1820
1821 int closest_quad_idx = -1;
1822 int closest_corner_idx = -1;
1823
1824 float sqr_radius = cur_quad.edge_sqr_len * neighborsFinder.thresh_sqr_scale + 1;
1825
1826 cv::Point2f closest_corner_pt;
1827
1828 bool found = neighborsFinder.findCornerNeighbor(
1829 quad_idx: idx,
1830 corner_idx: i,
1831 corner_pt: pt,
1832 min_sqr_dist,
1833 sqr_radius,
1834 closest_quad_idx,
1835 closest_corner_idx,
1836 closest_corner_pt);
1837
1838 if (!found)
1839 continue;
1840
1841 sqr_radius = min_sqr_dist + 1;
1842 min_sqr_dist = FLT_MAX;
1843
1844 int closest_closest_quad_idx = -1;
1845 int closest_closest_corner_idx = -1;
1846
1847 cv::Point2f closest_closest_corner_pt;
1848
1849 found = neighborsFinder.findCornerNeighbor(
1850 quad_idx: closest_quad_idx,
1851 corner_idx: closest_corner_idx,
1852 corner_pt: closest_corner_pt,
1853 min_sqr_dist,
1854 sqr_radius,
1855 closest_quad_idx&: closest_closest_quad_idx,
1856 closest_corner_idx&: closest_closest_corner_idx,
1857 closest_corner_pt&: closest_closest_corner_pt);
1858
1859 if (!found)
1860 continue;
1861
1862 if (closest_closest_quad_idx != idx ||
1863 closest_closest_corner_idx != i ||
1864 closest_closest_corner_pt != pt)
1865 continue;
1866
1867 ChessBoardQuad* closest_quad = &all_quads[closest_quad_idx];
1868 ChessBoardCorner& closest_corner = *closest_quad->corners[closest_corner_idx];
1869 closest_corner.pt = (pt + closest_corner_pt) * 0.5f;
1870
1871 // We've found one more corner - remember it
1872 cur_quad.count++;
1873 cur_quad.neighbors[i] = closest_quad;
1874 cur_quad.corners[i] = &closest_corner;
1875
1876 closest_quad->count++;
1877 closest_quad->neighbors[closest_corner_idx] = &cur_quad;
1878 }
1879 }
1880}
1881
1882
1883// returns corners in clockwise order
1884// corners don't necessarily start at same position on quad (e.g.,
1885// top left corner)
1886void ChessBoardDetector::generateQuads(const cv::Mat& image_, int flags, int dilations)
1887{
1888 binarized_image = image_; // save for debug purposes
1889
1890 int quad_count = 0;
1891
1892 all_quads.deallocate();
1893 all_corners.deallocate();
1894
1895 // empiric bound for minimal allowed area for squares
1896 const int min_area = 25; //cvRound( image->cols * image->rows * .03 * 0.01 * 0.92 );
1897
1898 bool filterQuads = (flags & CALIB_CB_FILTER_QUADS) != 0;
1899
1900 std::vector<std::vector<Point> > contours;
1901 std::vector<Vec4i> hierarchy;
1902
1903 cv::findContours(image: image_, contours, hierarchy, mode: RETR_CCOMP, method: CHAIN_APPROX_SIMPLE);
1904
1905 if (contours.empty())
1906 {
1907 CV_LOG_DEBUG(NULL, "calib3d(chessboard): cv::findContours() returns no contours");
1908 return;
1909 }
1910
1911 std::vector<int> contour_child_counter(contours.size(), 0);
1912 int boardIdx = -1;
1913
1914 std::vector<QuadCountour> contour_quads;
1915
1916 for (int idx = (int)(contours.size() - 1); idx >= 0; --idx)
1917 {
1918 int parentIdx = hierarchy[idx][3];
1919 if (hierarchy[idx][2] != -1 || parentIdx == -1) // holes only (no child contours and with parent)
1920 continue;
1921 const std::vector<Point>& contour = contours[idx];
1922
1923 Rect contour_rect = boundingRect(array: contour);
1924 if (contour_rect.area() < min_area)
1925 continue;
1926
1927 std::vector<Point> approx_contour = contour;
1928
1929 const int min_approx_level = 1, max_approx_level = MAX_CONTOUR_APPROX;
1930 for (int approx_level = min_approx_level; approx_contour.size() > 4 && approx_level <= max_approx_level; approx_level++ )
1931 {
1932 approxPolyDP(curve: approx_contour, approxCurve: approx_contour, epsilon: (float)approx_level, closed: true);
1933 }
1934
1935 // reject non-quadrangles
1936 if (approx_contour.size() != 4)
1937 continue;
1938 if (!cv::isContourConvex(contour: approx_contour))
1939 continue;
1940
1941 cv::Point pt[4];
1942 for (int i = 0; i < 4; ++i)
1943 pt[i] = approx_contour[i];
1944 CV_LOG_VERBOSE(NULL, 9, "... contours(" << contour_quads.size() << " added):" << pt[0] << " " << pt[1] << " " << pt[2] << " " << pt[3]);
1945
1946 if (filterQuads)
1947 {
1948 double p = cv::arcLength(curve: approx_contour, closed: true);
1949 double area = cv::contourArea(contour: approx_contour, oriented: false);
1950
1951 double d1 = sqrt(x: normL2Sqr<double>(pt: pt[0] - pt[2]));
1952 double d2 = sqrt(x: normL2Sqr<double>(pt: pt[1] - pt[3]));
1953
1954 // philipg. Only accept those quadrangles which are more square
1955 // than rectangular and which are big enough
1956 double d3 = sqrt(x: normL2Sqr<double>(pt: pt[0] - pt[1]));
1957 double d4 = sqrt(x: normL2Sqr<double>(pt: pt[1] - pt[2]));
1958 if (!(d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > min_area &&
1959 d1 >= 0.15 * p && d2 >= 0.15 * p))
1960 continue;
1961 }
1962
1963 contour_child_counter[parentIdx]++;
1964 if (boardIdx != parentIdx && (boardIdx < 0 || contour_child_counter[boardIdx] < contour_child_counter[parentIdx]))
1965 boardIdx = parentIdx;
1966
1967 contour_quads.emplace_back(args&: pt, args&: parentIdx);
1968 }
1969
1970 size_t total = contour_quads.size();
1971 size_t max_quad_buf_size = std::max(a: (size_t)2, b: total * 3);
1972 all_quads.allocate(size: max_quad_buf_size);
1973 all_corners.allocate(size: max_quad_buf_size * 4);
1974
1975 // Create array of quads structures
1976 for (size_t idx = 0; idx < total; ++idx)
1977 {
1978 QuadCountour& qc = contour_quads[idx];
1979 if (filterQuads && qc.parent_contour != boardIdx)
1980 continue;
1981
1982 int quad_idx = quad_count++;
1983 ChessBoardQuad& q = all_quads[quad_idx];
1984
1985 // reset group ID
1986 q = ChessBoardQuad();
1987 for (int i = 0; i < 4; ++i)
1988 {
1989 Point2f pt(qc.pt[i]);
1990 ChessBoardCorner& corner = all_corners[quad_idx * 4 + i];
1991
1992 corner = ChessBoardCorner(pt);
1993 q.corners[i] = &corner;
1994 }
1995 q.edge_sqr_len = FLT_MAX;
1996 for (int i = 0; i < 4; ++i)
1997 {
1998 float sqr_d = normL2Sqr<float>(pt: q.corners[i]->pt - q.corners[(i+1)&3]->pt);
1999 q.edge_sqr_len = std::min(a: q.edge_sqr_len, b: sqr_d);
2000 }
2001
2002 const int edge_len_compensation = 2 * dilations;
2003 q.edge_sqr_len += 2 * sqrt(x: q.edge_sqr_len) * edge_len_compensation + edge_len_compensation * edge_len_compensation;
2004 }
2005
2006 all_quads_count = quad_count;
2007
2008 CV_LOG_VERBOSE(NULL, 3, "Total quad contours: " << total);
2009 CV_LOG_VERBOSE(NULL, 3, "max_quad_buf_size=" << max_quad_buf_size);
2010 CV_LOG_VERBOSE(NULL, 3, "filtered quad_count=" << quad_count);
2011}
2012
2013bool ChessBoardDetector::processQuads(std::vector<cv::Point2f>& out_corners, int &prev_sqr_size)
2014{
2015 out_corners.resize(new_size: 0);
2016 if (all_quads_count <= 0)
2017 return false;
2018
2019 size_t max_quad_buf_size = all_quads.size();
2020
2021 // Find quad's neighbors
2022 findQuadNeighbors();
2023
2024 // allocate extra for adding in orderFoundQuads
2025 std::vector<ChessBoardQuad*> quad_group;
2026 std::vector<ChessBoardCorner*> corner_group; corner_group.reserve(n: max_quad_buf_size * 4);
2027
2028 for (int group_idx = 0; ; group_idx++)
2029 {
2030 findConnectedQuads(out_group&: quad_group, group_idx);
2031 if (quad_group.empty())
2032 break;
2033
2034 int count = (int)quad_group.size();
2035
2036 // order the quad corners globally
2037 // maybe delete or add some
2038 DPRINTF("Starting ordering of inner quads (%d)", count);
2039 count = orderFoundConnectedQuads(quads&: quad_group);
2040 DPRINTF("Finished ordering of inner quads (%d)", count);
2041
2042 if (count == 0)
2043 continue; // haven't found inner quads
2044
2045 // If count is more than it should be, this will remove those quads
2046 // which cause maximum deviation from a nice square pattern.
2047 count = cleanFoundConnectedQuads(quad_group);
2048 DPRINTF("Connected group: %d, count: %d", group_idx, count);
2049
2050 count = checkQuadGroup(quad_group, out_corners&: corner_group);
2051 DPRINTF("Connected group: %d, count: %d", group_idx, count);
2052
2053 int n = count > 0 ? pattern_size.width * pattern_size.height : -count;
2054 n = std::min(a: n, b: pattern_size.width * pattern_size.height);
2055 float sum_dist = 0;
2056 int total = 0;
2057
2058 for(int i = 0; i < n; i++ )
2059 {
2060 int ni = 0;
2061 float sum = corner_group[i]->sumDist(n_&: ni);
2062 sum_dist += sum;
2063 total += ni;
2064 }
2065 prev_sqr_size = cvRound(value: sum_dist/std::max(a: total, b: 1));
2066
2067 if (count > 0 || (-count > (int)out_corners.size()))
2068 {
2069 // copy corners to output array
2070 out_corners.clear();
2071 out_corners.reserve(n: n);
2072 for (int i = 0; i < n; ++i)
2073 out_corners.push_back(x: corner_group[i]->pt);
2074
2075 if (count == pattern_size.width*pattern_size.height
2076 && checkBoardMonotony(corners: out_corners))
2077 {
2078 return true;
2079 }
2080 }
2081 }
2082
2083 return false;
2084}
2085
2086
2087
2088void drawChessboardCorners( InputOutputArray image, Size patternSize,
2089 InputArray _corners,
2090 bool patternWasFound )
2091{
2092 CV_INSTRUMENT_REGION();
2093
2094 int type = image.type();
2095 int cn = CV_MAT_CN(type);
2096 CV_CheckType(type, cn == 1 || cn == 3 || cn == 4,
2097 "Number of channels must be 1, 3 or 4" );
2098
2099 int depth = CV_MAT_DEPTH(type);
2100 CV_CheckType(type, depth == CV_8U || depth == CV_16U || depth == CV_32F,
2101 "Only 8-bit, 16-bit or floating-point 32-bit images are supported");
2102
2103 if (_corners.empty())
2104 return;
2105 Mat corners = _corners.getMat();
2106 const Point2f* corners_data = corners.ptr<Point2f>(y: 0);
2107 CV_DbgAssert(corners_data);
2108 int nelems = corners.checkVector(elemChannels: 2, CV_32F, requireContinuous: true);
2109 CV_Assert(nelems >= 0);
2110
2111 const int shift = 0;
2112 const int radius = 4;
2113 const int r = radius*(1 << shift);
2114
2115 double scale = 1;
2116 switch (depth)
2117 {
2118 case CV_8U:
2119 scale = 1;
2120 break;
2121 case CV_16U:
2122 scale = 256;
2123 break;
2124 case CV_32F:
2125 scale = 1./255;
2126 break;
2127 }
2128
2129 int line_type = (type == CV_8UC1 || type == CV_8UC3) ? LINE_AA : LINE_8;
2130
2131 if (!patternWasFound)
2132 {
2133 Scalar color(0,0,255,0);
2134 if (cn == 1)
2135 color = Scalar::all(v0: 200);
2136 color *= scale;
2137
2138 for (int i = 0; i < nelems; i++ )
2139 {
2140 cv::Point2i pt(
2141 cvRound(value: corners_data[i].x*(1 << shift)),
2142 cvRound(value: corners_data[i].y*(1 << shift))
2143 );
2144 line(img: image, pt1: Point(pt.x - r, pt.y - r), pt2: Point( pt.x + r, pt.y + r), color, thickness: 1, lineType: line_type, shift);
2145 line(img: image, pt1: Point(pt.x - r, pt.y + r), pt2: Point( pt.x + r, pt.y - r), color, thickness: 1, lineType: line_type, shift);
2146 circle(img: image, center: pt, radius: r+(1<<shift), color, thickness: 1, lineType: line_type, shift);
2147 }
2148 }
2149 else
2150 {
2151 const int line_max = 7;
2152 static const int line_colors[line_max][4] =
2153 {
2154 {0,0,255,0},
2155 {0,128,255,0},
2156 {0,200,200,0},
2157 {0,255,0,0},
2158 {200,200,0,0},
2159 {255,0,0,0},
2160 {255,0,255,0}
2161 };
2162
2163 cv::Point2i prev_pt;
2164 for (int y = 0, i = 0; y < patternSize.height; y++)
2165 {
2166 const int* line_color = &line_colors[y % line_max][0];
2167 Scalar color(line_color[0], line_color[1], line_color[2], line_color[3]);
2168 if (cn == 1)
2169 color = Scalar::all(v0: 200);
2170 color *= scale;
2171
2172 for (int x = 0; x < patternSize.width; x++, i++)
2173 {
2174 cv::Point2i pt(
2175 cvRound(value: corners_data[i].x*(1 << shift)),
2176 cvRound(value: corners_data[i].y*(1 << shift))
2177 );
2178
2179 if (i != 0)
2180 line(img: image, pt1: prev_pt, pt2: pt, color, thickness: 1, lineType: line_type, shift);
2181
2182 line(img: image, pt1: Point(pt.x - r, pt.y - r), pt2: Point( pt.x + r, pt.y + r), color, thickness: 1, lineType: line_type, shift);
2183 line(img: image, pt1: Point(pt.x - r, pt.y + r), pt2: Point( pt.x + r, pt.y - r), color, thickness: 1, lineType: line_type, shift);
2184 circle(img: image, center: pt, radius: r+(1<<shift), color, thickness: 1, lineType: line_type, shift);
2185 prev_pt = pt;
2186 }
2187 }
2188 }
2189}
2190
2191bool findCirclesGrid( InputArray _image, Size patternSize,
2192 OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector,
2193 const CirclesGridFinderParameters& parameters_)
2194{
2195 CV_INSTRUMENT_REGION();
2196
2197 CirclesGridFinderParameters parameters = parameters_; // parameters.gridType is amended below
2198
2199 bool isAsymmetricGrid = (flags & CALIB_CB_ASYMMETRIC_GRID) ? true : false;
2200 bool isSymmetricGrid = (flags & CALIB_CB_SYMMETRIC_GRID ) ? true : false;
2201 CV_Assert(isAsymmetricGrid ^ isSymmetricGrid);
2202
2203 std::vector<Point2f> centers;
2204
2205 std::vector<Point2f> points;
2206 if (blobDetector)
2207 {
2208 std::vector<KeyPoint> keypoints;
2209 blobDetector->detect(image: _image, keypoints);
2210 for (size_t i = 0; i < keypoints.size(); i++)
2211 {
2212 points.push_back(x: keypoints[i].pt);
2213 }
2214 }
2215 else
2216 {
2217 CV_CheckTypeEQ(_image.type(), CV_32FC2, "blobDetector must be provided or image must contains Point2f array (std::vector<Point2f>) with candidates");
2218 _image.copyTo(arr: points);
2219 }
2220
2221 if(flags & CALIB_CB_ASYMMETRIC_GRID)
2222 parameters.gridType = CirclesGridFinderParameters::ASYMMETRIC_GRID;
2223 if(flags & CALIB_CB_SYMMETRIC_GRID)
2224 parameters.gridType = CirclesGridFinderParameters::SYMMETRIC_GRID;
2225
2226 if(flags & CALIB_CB_CLUSTERING)
2227 {
2228 CirclesGridClusterFinder circlesGridClusterFinder(parameters);
2229 circlesGridClusterFinder.findGrid(points, patternSize, centers);
2230 Mat(centers).copyTo(m: _centers);
2231 return !centers.empty();
2232 }
2233
2234 bool isValid = false;
2235 const int attempts = 2;
2236 const size_t minHomographyPoints = 4;
2237 Mat H;
2238 for (int i = 0; i < attempts; i++)
2239 {
2240 centers.clear();
2241 CirclesGridFinder boxFinder(patternSize, points, parameters);
2242 try
2243 {
2244 bool isFound = boxFinder.findHoles();
2245 if (isFound)
2246 {
2247 switch(parameters.gridType)
2248 {
2249 case CirclesGridFinderParameters::SYMMETRIC_GRID:
2250 boxFinder.getHoles(holes&: centers);
2251 break;
2252 case CirclesGridFinderParameters::ASYMMETRIC_GRID:
2253 boxFinder.getAsymmetricHoles(holes&: centers);
2254 break;
2255 default:
2256 CV_Error(Error::StsBadArg, "Unknown pattern type");
2257 }
2258
2259 isValid = true;
2260 break; // done, return result
2261 }
2262 }
2263 catch (const cv::Exception& e)
2264 {
2265 CV_UNUSED(e);
2266 CV_LOG_DEBUG(NULL, "findCirclesGrid2: attempt=" << i << ": " << e.what());
2267 // nothing, next attempt
2268 }
2269
2270 boxFinder.getHoles(holes&: centers);
2271 if (i != attempts - 1)
2272 {
2273 if (centers.size() < minHomographyPoints)
2274 break;
2275 H = CirclesGridFinder::rectifyGrid(detectedGridSize: boxFinder.getDetectedGridSize(), centers, keypoint: points, warpedKeypoints&: points);
2276 }
2277 }
2278
2279 if (!centers.empty() && !H.empty()) // undone rectification
2280 {
2281 Mat orgPointsMat;
2282 transform(src: centers, dst: orgPointsMat, m: H.inv());
2283 convertPointsFromHomogeneous(src: orgPointsMat, dst: centers);
2284 }
2285 Mat(centers).copyTo(m: _centers);
2286 return isValid;
2287}
2288
2289bool findCirclesGrid(InputArray _image, Size patternSize,
2290 OutputArray _centers, int flags, const Ptr<FeatureDetector> &blobDetector)
2291{
2292 return cv::findCirclesGrid(_image, patternSize, _centers, flags, blobDetector, parameters_: CirclesGridFinderParameters());
2293}
2294
2295} // namespace
2296/* End of file. */
2297

Provided by KDAB

Privacy Policy
Learn to use CMake with our Intro Training
Find out more

source code of opencv/modules/calib3d/src/calibinit.cpp