1// This file is part of OpenCV project.
2// It is subject to the license terms in the LICENSE file found in the top-level directory
3// of this distribution and at http://opencv.org/license.html
4
5#include "../precomp.hpp"
6
7#include <opencv2/calib3d.hpp>
8#include <opencv2/core/utils/logger.hpp>
9#include "opencv2/objdetect/charuco_detector.hpp"
10#include "aruco_utils.hpp"
11
12namespace cv {
13namespace aruco {
14
15using namespace std;
16
17struct CharucoDetector::CharucoDetectorImpl {
18 CharucoBoard board;
19 CharucoParameters charucoParameters;
20 ArucoDetector arucoDetector;
21
22 CharucoDetectorImpl(const CharucoBoard& _board, const CharucoParameters _charucoParameters,
23 const ArucoDetector& _arucoDetector): board(_board), charucoParameters(_charucoParameters),
24 arucoDetector(_arucoDetector)
25 {}
26
27 bool checkBoard(InputArrayOfArrays markerCorners, InputArray markerIds, InputArray charucoCorners, InputArray charucoIds) {
28 vector<Mat> mCorners;
29 markerCorners.getMatVector(mv&: mCorners);
30 const Mat mIds = markerIds.getMat();
31 const Mat chCorners = charucoCorners.getMat();
32 const Mat chIds = charucoIds.getMat();
33 const vector<int>& boardIds = board.getIds();
34
35 const vector<vector<int> > nearestMarkerIdx = board.getNearestMarkerIdx();
36 vector<Point2f> distance(board.getNearestMarkerIdx().size(), Point2f(0.f, std::numeric_limits<float>::max()));
37 // distance[i].x: max distance from the i-th charuco corner to charuco corner-forming markers.
38 // The two charuco corner-forming markers of i-th charuco corner are defined in getNearestMarkerIdx()[i]
39 // distance[i].y: min distance from the charuco corner to other markers.
40 for (size_t i = 0ull; i < chIds.total(); i++) {
41 int chId = chIds.ptr<int>(y: 0)[i];
42 Point2f charucoCorner(chCorners.ptr<Point2f>(y: 0)[i]);
43 for (size_t j = 0ull; j < mIds.total(); j++) {
44 int idMaker = mIds.ptr<int>(y: 0)[j];
45 // skip the check if the marker is not in the current board.
46 if (find(first: boardIds.begin(), last: boardIds.end(), val: idMaker) == boardIds.end())
47 continue;
48 Point2f centerMarker((mCorners[j].ptr<Point2f>(y: 0)[0] + mCorners[j].ptr<Point2f>(y: 0)[1] +
49 mCorners[j].ptr<Point2f>(y: 0)[2] + mCorners[j].ptr<Point2f>(y: 0)[3]) / 4.f);
50 float dist = sqrt(x: normL2Sqr<float>(pt: centerMarker - charucoCorner));
51 // nearestMarkerIdx contains for each charuco corner, nearest marker index in ids array
52 const int nearestMarkerId1 = boardIds[nearestMarkerIdx[chId][0]];
53 const int nearestMarkerId2 = boardIds[nearestMarkerIdx[chId][1]];
54 if (nearestMarkerId1 == idMaker || nearestMarkerId2 == idMaker) {
55 int nearestCornerId = nearestMarkerId1 == idMaker ? board.getNearestMarkerCorners()[chId][0] : board.getNearestMarkerCorners()[chId][1];
56 Point2f nearestCorner = mCorners[j].ptr<Point2f>(y: 0)[nearestCornerId];
57 // distToNearest: distance from the charuco corner to charuco corner-forming markers
58 float distToNearest = sqrt(x: normL2Sqr<float>(pt: nearestCorner - charucoCorner));
59 distance[chId].x = max(a: distance[chId].x, b: distToNearest);
60 // check that nearestCorner is nearest point
61 {
62 Point2f mid1 = (mCorners[j].ptr<Point2f>(y: 0)[(nearestCornerId + 1) % 4]+nearestCorner)*0.5f;
63 Point2f mid2 = (mCorners[j].ptr<Point2f>(y: 0)[(nearestCornerId + 3) % 4]+nearestCorner)*0.5f;
64 float tmpDist = min(a: sqrt(x: normL2Sqr<float>(pt: mid1 - charucoCorner)), b: sqrt(x: normL2Sqr<float>(pt: mid2 - charucoCorner)));
65 if (tmpDist < distToNearest)
66 return false;
67 }
68 }
69 // check distance from the charuco corner to other markers
70 else
71 distance[chId].y = min(a: distance[chId].y, b: dist);
72 }
73 // if distance from the charuco corner to charuco corner-forming markers more then distance from the charuco corner to other markers,
74 // then a false board is found.
75 if (distance[chId].x > 0.f && distance[chId].y < std::numeric_limits<float>::max() && distance[chId].x > distance[chId].y)
76 return false;
77 }
78 return true;
79 }
80
81 /** Calculate the maximum window sizes for corner refinement for each charuco corner based on the distance
82 * to their closest markers */
83 vector<Size> getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, InputArray markerIds,
84 InputArray charucoCorners) {
85 size_t nCharucoCorners = charucoCorners.getMat().total();
86
87 CV_Assert(board.getNearestMarkerIdx().size() == nCharucoCorners);
88
89 vector<Size> winSizes(nCharucoCorners, Size(-1, -1));
90 for(size_t i = 0ull; i < nCharucoCorners; i++) {
91 if(charucoCorners.getMat().at<Point2f>(i0: (int)i) == Point2f(-1.f, -1.f)) continue;
92 if(board.getNearestMarkerIdx()[i].empty()) continue;
93 double minDist = -1;
94 int counter = 0;
95 // calculate the distance to each of the closest corner of each closest marker
96 for(size_t j = 0; j < board.getNearestMarkerIdx()[i].size(); j++) {
97 // find marker
98 int markerId = board.getIds()[board.getNearestMarkerIdx()[i][j]];
99 int markerIdx = -1;
100 for(size_t k = 0; k < markerIds.getMat().total(); k++) {
101 if(markerIds.getMat().at<int>(i0: (int)k) == markerId) {
102 markerIdx = (int)k;
103 break;
104 }
105 }
106 if(markerIdx == -1) continue;
107 Point2f markerCorner =
108 markerCorners.getMat(i: markerIdx).at<Point2f>(i0: board.getNearestMarkerCorners()[i][j]);
109 Point2f charucoCorner = charucoCorners.getMat().at<Point2f>(i0: (int)i);
110 double dist = norm(pt: markerCorner - charucoCorner);
111 if(minDist == -1) minDist = dist; // if first distance, just assign it
112 minDist = min(a: dist, b: minDist);
113 counter++;
114 }
115 // if this is the first closest marker, dont do anything
116 if(counter == 0)
117 continue;
118 else {
119 // else, calculate the maximum window size
120 int winSizeInt = int(minDist - 2); // remove 2 pixels for safety
121 if(winSizeInt < 1) winSizeInt = 1; // minimum size is 1
122 if(winSizeInt > 10) winSizeInt = 10; // maximum size is 10
123 winSizes[i] = Size(winSizeInt, winSizeInt);
124 }
125 }
126 return winSizes;
127 }
128
129 /** @brief From all projected chessboard corners, select those inside the image and apply subpixel refinement */
130 void selectAndRefineChessboardCorners(InputArray allCorners, InputArray image, OutputArray selectedCorners,
131 OutputArray selectedIds, const vector<Size> &winSizes) {
132 const int minDistToBorder = 2; // minimum distance of the corner to the image border
133 // remaining corners, ids and window refinement sizes after removing corners outside the image
134 vector<Point2f> filteredChessboardImgPoints;
135 vector<Size> filteredWinSizes;
136 vector<int> filteredIds;
137 // filter corners outside the image
138 Rect innerRect(minDistToBorder, minDistToBorder, image.getMat().cols - 2 * minDistToBorder,
139 image.getMat().rows - 2 * minDistToBorder);
140 for(unsigned int i = 0; i < allCorners.getMat().total(); i++) {
141 if(innerRect.contains(pt: allCorners.getMat().at<Point2f>(i0: i))) {
142 filteredChessboardImgPoints.push_back(x: allCorners.getMat().at<Point2f>(i0: i));
143 filteredIds.push_back(x: i);
144 filteredWinSizes.push_back(x: winSizes[i]);
145 }
146 }
147 // if none valid, return 0
148 if(filteredChessboardImgPoints.empty()) return;
149 // corner refinement, first convert input image to grey
150 Mat grey;
151 if(image.type() == CV_8UC3)
152 cvtColor(src: image, dst: grey, code: COLOR_BGR2GRAY);
153 else
154 grey = image.getMat();
155 //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize
156 parallel_for_(range: Range(0, (int)filteredChessboardImgPoints.size()), functor: [&](const Range& range) {
157 const int begin = range.start;
158 const int end = range.end;
159 for (int i = begin; i < end; i++) {
160 vector<Point2f> in;
161 in.push_back(x: filteredChessboardImgPoints[i] - Point2f(0.5, 0.5)); // adjust sub-pixel coordinates for cornerSubPix
162 Size winSize = filteredWinSizes[i];
163 if (winSize.height == -1 || winSize.width == -1)
164 winSize = Size(arucoDetector.getDetectorParameters().cornerRefinementWinSize,
165 arucoDetector.getDetectorParameters().cornerRefinementWinSize);
166 cornerSubPix(image: grey, corners: in, winSize, zeroZone: Size(),
167 criteria: TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
168 arucoDetector.getDetectorParameters().cornerRefinementMaxIterations,
169 arucoDetector.getDetectorParameters().cornerRefinementMinAccuracy));
170 filteredChessboardImgPoints[i] = in[0] + Point2f(0.5, 0.5);
171 }
172 });
173 // parse output
174 Mat(filteredChessboardImgPoints).copyTo(m: selectedCorners);
175 Mat(filteredIds).copyTo(m: selectedIds);
176 }
177
178 /** Interpolate charuco corners using approximated pose estimation */
179 void interpolateCornersCharucoApproxCalib(InputArrayOfArrays markerCorners, InputArray markerIds,
180 InputArray image, OutputArray charucoCorners, OutputArray charucoIds) {
181 CV_Assert(image.getMat().channels() == 1 || image.getMat().channels() == 3);
182 CV_Assert(markerCorners.total() == markerIds.getMat().total());
183
184 // approximated pose estimation using marker corners
185 Mat approximatedRvec, approximatedTvec;
186 Mat objPoints, imgPoints; // object and image points for the solvePnP function
187 Board simpleBoard(board.getObjPoints(), board.getDictionary(), board.getIds());
188 simpleBoard.matchImagePoints(detectedCorners: markerCorners, detectedIds: markerIds, objPoints, imgPoints);
189 if (objPoints.total() < 4ull) // need, at least, 4 corners
190 return;
191
192 solvePnP(objectPoints: objPoints, imagePoints: imgPoints, cameraMatrix: charucoParameters.cameraMatrix, distCoeffs: charucoParameters.distCoeffs, rvec: approximatedRvec, tvec: approximatedTvec);
193
194 // project chessboard corners
195 vector<Point2f> allChessboardImgPoints;
196 projectPoints(objectPoints: board.getChessboardCorners(), rvec: approximatedRvec, tvec: approximatedTvec, cameraMatrix: charucoParameters.cameraMatrix,
197 distCoeffs: charucoParameters.distCoeffs, imagePoints: allChessboardImgPoints);
198 // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
199 // to the closes marker corner to avoid erroneous displacements to marker corners
200 vector<Size> subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, charucoCorners: allChessboardImgPoints);
201 // filter corners outside the image and subpixel-refine charuco corners
202 selectAndRefineChessboardCorners(allCorners: allChessboardImgPoints, image, selectedCorners: charucoCorners, selectedIds: charucoIds, winSizes: subPixWinSizes);
203 }
204
205 /** Interpolate charuco corners using local homography */
206 void interpolateCornersCharucoLocalHom(InputArrayOfArrays markerCorners, InputArray markerIds, InputArray image,
207 OutputArray charucoCorners, OutputArray charucoIds) {
208 CV_Assert(image.getMat().channels() == 1 || image.getMat().channels() == 3);
209 CV_Assert(markerCorners.total() == markerIds.getMat().total());
210 size_t nMarkers = markerIds.getMat().total();
211 // calculate local homographies for each marker
212 vector<Mat> transformations(nMarkers);
213 vector<bool> validTransform(nMarkers, false);
214 const auto& ids = board.getIds();
215 for(size_t i = 0ull; i < nMarkers; i++) {
216 vector<Point2f> markerObjPoints2D;
217 int markerId = markerIds.getMat().at<int>(i0: (int)i);
218 auto it = find(first: ids.begin(), last: ids.end(), val: markerId);
219 if(it == ids.end()) continue;
220 auto boardIdx = it - ids.begin();
221 markerObjPoints2D.resize(new_size: 4ull);
222 for(size_t j = 0ull; j < 4ull; j++)
223 markerObjPoints2D[j] =
224 Point2f(board.getObjPoints()[boardIdx][j].x, board.getObjPoints()[boardIdx][j].y);
225 transformations[i] = getPerspectiveTransform(src: markerObjPoints2D, dst: markerCorners.getMat(i: (int)i));
226 // set transform as valid if transformation is non-singular
227 double det = determinant(mtx: transformations[i]);
228 validTransform[i] = std::abs(x: det) > 1e-6;
229 }
230 size_t nCharucoCorners = (size_t)board.getChessboardCorners().size();
231 vector<Point2f> allChessboardImgPoints(nCharucoCorners, Point2f(-1, -1));
232 // for each charuco corner, calculate its interpolation position based on the closest markers
233 // homographies
234 for(size_t i = 0ull; i < nCharucoCorners; i++) {
235 Point2f objPoint2D = Point2f(board.getChessboardCorners()[i].x, board.getChessboardCorners()[i].y);
236 vector<Point2f> interpolatedPositions;
237 for(size_t j = 0ull; j < board.getNearestMarkerIdx()[i].size(); j++) {
238 int markerId = board.getIds()[board.getNearestMarkerIdx()[i][j]];
239 int markerIdx = -1;
240 for(size_t k = 0ull; k < markerIds.getMat().total(); k++) {
241 if(markerIds.getMat().at<int>(i0: (int)k) == markerId) {
242 markerIdx = (int)k;
243 break;
244 }
245 }
246 if (markerIdx != -1 &&
247 validTransform[markerIdx])
248 {
249 vector<Point2f> in, out;
250 in.push_back(x: objPoint2D);
251 perspectiveTransform(src: in, dst: out, m: transformations[markerIdx]);
252 interpolatedPositions.push_back(x: out[0]);
253 }
254 }
255 // none of the closest markers detected
256 if(interpolatedPositions.empty()) continue;
257 // more than one closest marker detected, take middle point
258 if(interpolatedPositions.size() > 1ull) {
259 allChessboardImgPoints[i] = (interpolatedPositions[0] + interpolatedPositions[1]) / 2.;
260 }
261 // a single closest marker detected
262 else allChessboardImgPoints[i] = interpolatedPositions[0];
263 }
264 // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
265 // to the closes marker corner to avoid erroneous displacements to marker corners
266 vector<Size> subPixWinSizes = getMaximumSubPixWindowSizes(markerCorners, markerIds, charucoCorners: allChessboardImgPoints);
267 // filter corners outside the image and subpixel-refine charuco corners
268 selectAndRefineChessboardCorners(allCorners: allChessboardImgPoints, image, selectedCorners: charucoCorners, selectedIds: charucoIds, winSizes: subPixWinSizes);
269 }
270
271 /** Remove charuco corners if any of their minMarkers closest markers has not been detected */
272 int filterCornersWithoutMinMarkers(InputArray _allCharucoCorners, InputArray allCharucoIds, InputArray allArucoIds,
273 OutputArray _filteredCharucoCorners, OutputArray _filteredCharucoIds) {
274 CV_Assert(charucoParameters.minMarkers >= 0 && charucoParameters.minMarkers <= 2);
275 vector<Point2f> filteredCharucoCorners;
276 vector<int> filteredCharucoIds;
277 // for each charuco corner
278 for(unsigned int i = 0; i < allCharucoIds.getMat().total(); i++) {
279 int currentCharucoId = allCharucoIds.getMat().at<int>(i0: i);
280 int totalMarkers = 0; // nomber of closest marker detected
281 // look for closest markers
282 for(unsigned int m = 0; m < board.getNearestMarkerIdx()[currentCharucoId].size(); m++) {
283 int markerId = board.getIds()[board.getNearestMarkerIdx()[currentCharucoId][m]];
284 bool found = false;
285 for(unsigned int k = 0; k < allArucoIds.getMat().total(); k++) {
286 if(allArucoIds.getMat().at<int>(i0: k) == markerId) {
287 found = true;
288 break;
289 }
290 }
291 if(found) totalMarkers++;
292 }
293 // if enough markers detected, add the charuco corner to the final list
294 if(totalMarkers >= charucoParameters.minMarkers) {
295 filteredCharucoIds.push_back(x: currentCharucoId);
296 filteredCharucoCorners.push_back(x: _allCharucoCorners.getMat().at<Point2f>(i0: i));
297 }
298 }
299 // parse output
300 Mat(filteredCharucoCorners).copyTo(m: _filteredCharucoCorners);
301 Mat(filteredCharucoIds).copyTo(m: _filteredCharucoIds);
302 return (int)_filteredCharucoIds.total();
303 }
304
305 void detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds,
306 InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) {
307 CV_Assert((markerCorners.empty() && markerIds.empty() && !image.empty()) || (markerCorners.total() == markerIds.total()));
308 vector<vector<Point2f>> tmpMarkerCorners;
309 vector<int> tmpMarkerIds;
310 InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners;
311 InputOutputArray _markerIds = markerIds.needed() ? markerIds : tmpMarkerIds;
312
313 if (markerCorners.empty() && markerIds.empty()) {
314 vector<vector<Point2f> > rejectedMarkers;
315 arucoDetector.detectMarkers(image, corners: _markerCorners, ids: _markerIds, rejectedImgPoints: rejectedMarkers);
316 if (charucoParameters.tryRefineMarkers)
317 arucoDetector.refineDetectedMarkers(image, board, detectedCorners: _markerCorners, detectedIds: _markerIds, rejectedCorners: rejectedMarkers);
318 if (_markerCorners.empty() && _markerIds.empty())
319 return;
320 }
321 // if camera parameters are avaible, use approximated calibration
322 if(!charucoParameters.cameraMatrix.empty())
323 interpolateCornersCharucoApproxCalib(markerCorners: _markerCorners, markerIds: _markerIds, image, charucoCorners, charucoIds);
324 // else use local homography
325 else
326 interpolateCornersCharucoLocalHom(markerCorners: _markerCorners, markerIds: _markerIds, image, charucoCorners, charucoIds);
327 // to return a charuco corner, its closest aruco markers should have been detected
328 filterCornersWithoutMinMarkers(allCharucoCorners: charucoCorners, allCharucoIds: charucoIds, allArucoIds: _markerIds, filteredCharucoCorners: charucoCorners, filteredCharucoIds: charucoIds);
329 }
330
331 void detectBoardWithCheck(InputArray image, OutputArray charucoCorners, OutputArray charucoIds,
332 InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) {
333 vector<vector<Point2f>> tmpMarkerCorners;
334 vector<int> tmpMarkerIds;
335 InputOutputArrayOfArrays _markerCorners = markerCorners.needed() ? markerCorners : tmpMarkerCorners;
336 InputOutputArray _markerIds = markerIds.needed() ? markerIds : tmpMarkerIds;
337 detectBoard(image, charucoCorners, charucoIds, markerCorners: _markerCorners, markerIds: _markerIds);
338 if (charucoParameters.checkMarkers && checkBoard(markerCorners: _markerCorners, markerIds: _markerIds, charucoCorners, charucoIds) == false) {
339 CV_LOG_DEBUG(NULL, "ChArUco board is built incorrectly");
340 charucoCorners.release();
341 charucoIds.release();
342 }
343 }
344};
345
346CharucoDetector::CharucoDetector(const CharucoBoard &board, const CharucoParameters &charucoParams,
347 const DetectorParameters &detectorParams, const RefineParameters& refineParams) {
348 this->charucoDetectorImpl = makePtr<CharucoDetectorImpl>(a1: board, a1: charucoParams, a1: ArucoDetector(board.getDictionary(), detectorParams, refineParams));
349}
350
351const CharucoBoard& CharucoDetector::getBoard() const {
352 return charucoDetectorImpl->board;
353}
354
355void CharucoDetector::setBoard(const CharucoBoard& board) {
356 this->charucoDetectorImpl->board = board;
357 charucoDetectorImpl->arucoDetector.setDictionary(board.getDictionary());
358}
359
360const CharucoParameters &CharucoDetector::getCharucoParameters() const {
361 return charucoDetectorImpl->charucoParameters;
362}
363
364void CharucoDetector::setCharucoParameters(CharucoParameters &charucoParameters) {
365 charucoDetectorImpl->charucoParameters = charucoParameters;
366}
367
368const DetectorParameters& CharucoDetector::getDetectorParameters() const {
369 return charucoDetectorImpl->arucoDetector.getDetectorParameters();
370}
371
372void CharucoDetector::setDetectorParameters(const DetectorParameters& detectorParameters) {
373 charucoDetectorImpl->arucoDetector.setDetectorParameters(detectorParameters);
374}
375
376const RefineParameters& CharucoDetector::getRefineParameters() const {
377 return charucoDetectorImpl->arucoDetector.getRefineParameters();
378}
379
380void CharucoDetector::setRefineParameters(const RefineParameters& refineParameters) {
381 charucoDetectorImpl->arucoDetector.setRefineParameters(refineParameters);
382}
383
384void CharucoDetector::detectBoard(InputArray image, OutputArray charucoCorners, OutputArray charucoIds,
385 InputOutputArrayOfArrays markerCorners, InputOutputArray markerIds) const {
386 charucoDetectorImpl->detectBoardWithCheck(image, charucoCorners, charucoIds, markerCorners, markerIds);
387}
388
389void CharucoDetector::detectDiamonds(InputArray image, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
390 InputOutputArrayOfArrays inMarkerCorners, InputOutputArray inMarkerIds) const {
391 CV_Assert(getBoard().getChessboardSize() == Size(3, 3));
392 CV_Assert((inMarkerCorners.empty() && inMarkerIds.empty() && !image.empty()) || (inMarkerCorners.total() == inMarkerIds.total()));
393
394 vector<vector<Point2f>> tmpMarkerCorners;
395 vector<int> tmpMarkerIds;
396 InputOutputArrayOfArrays _markerCorners = inMarkerCorners.needed() ? inMarkerCorners : tmpMarkerCorners;
397 InputOutputArray _markerIds = inMarkerIds.needed() ? inMarkerIds : tmpMarkerIds;
398 if (_markerCorners.empty() && _markerIds.empty()) {
399 charucoDetectorImpl->arucoDetector.detectMarkers(image, corners: _markerCorners, ids: _markerIds);
400 }
401
402 const float minRepDistanceRate = 1.302455f;
403 vector<vector<Point2f>> diamondCorners;
404 vector<Vec4i> diamondIds;
405
406 // stores if the detected markers have been assigned or not to a diamond
407 vector<bool> assigned(_markerIds.total(), false);
408 if(_markerIds.total() < 4ull) return; // a diamond need at least 4 markers
409
410 // convert input image to grey
411 Mat grey;
412 if(image.type() == CV_8UC3)
413 cvtColor(src: image, dst: grey, code: COLOR_BGR2GRAY);
414 else
415 grey = image.getMat();
416 auto board = getBoard();
417
418 // for each of the detected markers, try to find a diamond
419 for(unsigned int i = 0; i < (unsigned int)_markerIds.total(); i++) {
420 if(assigned[i]) continue;
421
422 // calculate marker perimeter
423 float perimeterSq = 0;
424 Mat corners = _markerCorners.getMat(i);
425 for(int c = 0; c < 4; c++) {
426 Point2f edge = corners.at<Point2f>(i0: c) - corners.at<Point2f>(i0: (c + 1) % 4);
427 perimeterSq += edge.x*edge.x + edge.y*edge.y;
428 }
429 // maximum reprojection error relative to perimeter
430 float minRepDistance = sqrt(x: perimeterSq) * minRepDistanceRate;
431
432 int currentId = _markerIds.getMat().at<int>(i0: i);
433
434 // prepare data to call refineDetectedMarkers()
435 // detected markers (only the current one)
436 vector<Mat> currentMarker;
437 vector<int> currentMarkerId;
438 currentMarker.push_back(x: _markerCorners.getMat(i));
439 currentMarkerId.push_back(x: currentId);
440
441 // marker candidates (the rest of markers if they have not been assigned)
442 vector<Mat> candidates;
443 vector<int> candidatesIdxs;
444 for(unsigned int k = 0; k < assigned.size(); k++) {
445 if(k == i) continue;
446 if(!assigned[k]) {
447 candidates.push_back(x: _markerCorners.getMat(i: k));
448 candidatesIdxs.push_back(x: k);
449 }
450 }
451 if(candidates.size() < 3ull) break; // we need at least 3 free markers
452 // modify charuco layout id to make sure all the ids are different than current id
453 vector<int> tmpIds(4ull);
454 for(int k = 1; k < 4; k++)
455 tmpIds[k] = currentId + 1 + k;
456 // current id is assigned to [0], so it is the marker on the top
457 tmpIds[0] = currentId;
458
459 // create Charuco board layout for diamond (3x3 layout)
460 charucoDetectorImpl->board = CharucoBoard(Size(3, 3), board.getSquareLength(),
461 board.getMarkerLength(), board.getDictionary(), tmpIds);
462
463 // try to find the rest of markers in the diamond
464 vector<int> acceptedIdxs;
465 if (currentMarker.size() != 4ull)
466 {
467 RefineParameters refineParameters(minRepDistance, -1.f, false);
468 RefineParameters tmp = charucoDetectorImpl->arucoDetector.getRefineParameters();
469 charucoDetectorImpl->arucoDetector.setRefineParameters(refineParameters);
470 charucoDetectorImpl->arucoDetector.refineDetectedMarkers(image: grey, board: getBoard(), detectedCorners: currentMarker, detectedIds: currentMarkerId,
471 rejectedCorners: candidates,
472 cameraMatrix: noArray(), distCoeffs: noArray(), recoveredIdxs: acceptedIdxs);
473 charucoDetectorImpl->arucoDetector.setRefineParameters(tmp);
474 }
475
476 // if found, we have a diamond
477 if(currentMarker.size() == 4ull) {
478 assigned[i] = true;
479 // calculate diamond id, acceptedIdxs array indicates the markers taken from candidates array
480 Vec4i markerId;
481 markerId[0] = currentId;
482 for(int k = 1; k < 4; k++) {
483 int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]];
484 markerId[k] = _markerIds.getMat().at<int>(i0: currentMarkerIdx);
485 assigned[currentMarkerIdx] = true;
486 }
487
488 // interpolate the charuco corners of the diamond
489 vector<Point2f> currentMarkerCorners;
490 Mat aux;
491 charucoDetectorImpl->detectBoardWithCheck(image: grey, charucoCorners: currentMarkerCorners, charucoIds: aux, markerCorners: currentMarker, markerIds: currentMarkerId);
492
493 // if everything is ok, save the diamond
494 if(currentMarkerCorners.size() > 0ull) {
495 // reorder corners
496 vector<Point2f> currentMarkerCornersReorder;
497 currentMarkerCornersReorder.resize(new_size: 4);
498 currentMarkerCornersReorder[0] = currentMarkerCorners[0];
499 currentMarkerCornersReorder[1] = currentMarkerCorners[1];
500 currentMarkerCornersReorder[2] = currentMarkerCorners[3];
501 currentMarkerCornersReorder[3] = currentMarkerCorners[2];
502
503 diamondCorners.push_back(x: currentMarkerCornersReorder);
504 diamondIds.push_back(x: markerId);
505 }
506 }
507 }
508 charucoDetectorImpl->board = board;
509
510 if(diamondIds.size() > 0ull) {
511 // parse output
512 Mat(diamondIds).copyTo(m: _diamondIds);
513
514 _diamondCorners.create(rows: (int)diamondCorners.size(), cols: 1, CV_32FC2);
515 for(unsigned int i = 0; i < diamondCorners.size(); i++) {
516 _diamondCorners.create(rows: 4, cols: 1, CV_32FC2, i, allowTransposed: true);
517 for(int j = 0; j < 4; j++) {
518 _diamondCorners.getMat(i).at<Point2f>(i0: j) = diamondCorners[i][j];
519 }
520 }
521 }
522}
523
524void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorners,
525 InputArray _charucoIds, Scalar cornerColor) {
526 CV_Assert(!_image.getMat().empty() &&
527 (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
528 CV_Assert((_charucoCorners.total() == _charucoIds.total()) ||
529 _charucoIds.total() == 0);
530 CV_Assert(_charucoCorners.channels() == 2);
531
532 Mat charucoCorners = _charucoCorners.getMat();
533 if (charucoCorners.type() != CV_32SC2)
534 charucoCorners.convertTo(m: charucoCorners, CV_32SC2);
535 Mat charucoIds;
536 if (!_charucoIds.empty())
537 charucoIds = _charucoIds.getMat();
538 size_t nCorners = charucoCorners.total();
539 for(size_t i = 0; i < nCorners; i++) {
540 Point corner = charucoCorners.at<Point>(i0: (int)i);
541 // draw first corner mark
542 rectangle(img: _image, pt1: corner - Point(3, 3), pt2: corner + Point(3, 3), color: cornerColor, thickness: 1, lineType: LINE_AA);
543 // draw ID
544 if(!_charucoIds.empty()) {
545 int id = charucoIds.at<int>(i0: (int)i);
546 stringstream s;
547 s << "id=" << id;
548 putText(img: _image, text: s.str(), org: corner + Point(5, -5), fontFace: FONT_HERSHEY_SIMPLEX, fontScale: 0.5,
549 color: cornerColor, thickness: 2);
550 }
551 }
552}
553
554void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners, InputArray _ids, Scalar borderColor) {
555 CV_Assert(_image.getMat().total() != 0 &&
556 (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
557 CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0);
558
559 // calculate colors
560 Scalar textColor, cornerColor;
561 textColor = cornerColor = borderColor;
562 swap(a&: textColor.val[0], b&: textColor.val[1]); // text color just sawp G and R
563 swap(a&: cornerColor.val[1], b&: cornerColor.val[2]); // corner color just sawp G and B
564
565 int nMarkers = (int)_corners.total();
566 for(int i = 0; i < nMarkers; i++) {
567 Mat currentMarker = _corners.getMat(i);
568 CV_Assert(currentMarker.total() == 4 && currentMarker.channels() == 2);
569 if (currentMarker.type() != CV_32SC2)
570 currentMarker.convertTo(m: currentMarker, CV_32SC2);
571
572 // draw marker sides
573 for(int j = 0; j < 4; j++) {
574 Point p0, p1;
575 p0 = currentMarker.at<Point>(i0: j);
576 p1 = currentMarker.at<Point>(i0: (j + 1) % 4);
577 line(img: _image, pt1: p0, pt2: p1, color: borderColor, thickness: 1);
578 }
579
580 // draw first corner mark
581 rectangle(img: _image, pt1: currentMarker.at<Point>(i0: 0) - Point(3, 3),
582 pt2: currentMarker.at<Point>(i0: 0) + Point(3, 3), color: cornerColor, thickness: 1, lineType: LINE_AA);
583
584 // draw id composed by four numbers
585 if(_ids.total() != 0) {
586 Point cent(0, 0);
587 for(int p = 0; p < 4; p++)
588 cent += currentMarker.at<Point>(i0: p);
589 cent = cent / 4.;
590 stringstream s;
591 s << "id=" << _ids.getMat().at< Vec4i >(i0: i);
592 putText(img: _image, text: s.str(), org: cent, fontFace: FONT_HERSHEY_SIMPLEX, fontScale: 0.5, color: textColor, thickness: 2);
593 }
594 }
595}
596
597}
598}
599

source code of opencv/modules/objdetect/src/aruco/charuco_detector.cpp