charuco.cpp 35.9 KB
Newer Older
S. Garrido's avatar
S. Garrido committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55
/*
By downloading, copying, installing or using the software you agree to this
license. If you do not agree to this license, do not download, install,
copy or use the software.

                          License Agreement
               For Open Source Computer Vision Library
                       (3-clause BSD License)

Copyright (C) 2013, OpenCV Foundation, all rights reserved.
Third party copyrights are property of their respective owners.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  * Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

  * Neither the names of the copyright holders nor the names of the contributors
    may be used to endorse or promote products derived from this software
    without specific prior written permission.

This software is provided by the copyright holders and contributors "as is" and
any express or implied warranties, including, but not limited to, the implied
warranties of merchantability and fitness for a particular purpose are
disclaimed. In no event shall copyright holders or contributors be liable for
any direct, indirect, incidental, special, exemplary, or consequential damages
(including, but not limited to, procurement of substitute goods or services;
loss of use, data, or profits; or business interruption) however caused
and on any theory of liability, whether in contract, strict liability,
or tort (including negligence or otherwise) arising in any way out of
the use of this software, even if advised of the possibility of such damage.
*/

#include "precomp.hpp"
#include "opencv2/aruco/charuco.hpp"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>


namespace cv {
namespace aruco {

using namespace std;



/**
 */
void CharucoBoard::draw(Size outSize, OutputArray _img, int marginSize, int borderBits) {

56
    CV_Assert(!outSize.empty());
S. Garrido's avatar
S. Garrido committed
57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95
    CV_Assert(marginSize >= 0);

    _img.create(outSize, CV_8UC1);
    _img.setTo(255);
    Mat out = _img.getMat();
    Mat noMarginsImg =
        out.colRange(marginSize, out.cols - marginSize).rowRange(marginSize, out.rows - marginSize);

    double totalLengthX, totalLengthY;
    totalLengthX = _squareLength * _squaresX;
    totalLengthY = _squareLength * _squaresY;

    // proportional transformation
    double xReduction = totalLengthX / double(noMarginsImg.cols);
    double yReduction = totalLengthY / double(noMarginsImg.rows);

    // determine the zone where the chessboard is placed
    Mat chessboardZoneImg;
    if(xReduction > yReduction) {
        int nRows = int(totalLengthY / xReduction);
        int rowsMargins = (noMarginsImg.rows - nRows) / 2;
        chessboardZoneImg = noMarginsImg.rowRange(rowsMargins, noMarginsImg.rows - rowsMargins);
    } else {
        int nCols = int(totalLengthX / yReduction);
        int colsMargins = (noMarginsImg.cols - nCols) / 2;
        chessboardZoneImg = noMarginsImg.colRange(colsMargins, noMarginsImg.cols - colsMargins);
    }

    // determine the margins to draw only the markers
    // take the minimum just to be sure
    double squareSizePixels = min(double(chessboardZoneImg.cols) / double(_squaresX),
                                  double(chessboardZoneImg.rows) / double(_squaresY));

    double diffSquareMarkerLength = (_squareLength - _markerLength) / 2;
    int diffSquareMarkerLengthPixels =
        int(diffSquareMarkerLength * squareSizePixels / _squareLength);

    // draw markers
    Mat markersImg;
96 97
    aruco::_drawPlanarBoardImpl(this, chessboardZoneImg.size(), markersImg,
                                diffSquareMarkerLengthPixels, borderBits);
S. Garrido's avatar
S. Garrido committed
98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122

    markersImg.copyTo(chessboardZoneImg);

    // now draw black squares
    for(int y = 0; y < _squaresY; y++) {
        for(int x = 0; x < _squaresX; x++) {

            if(y % 2 != x % 2) continue; // white corner, dont do anything

            double startX, startY;
            startX = squareSizePixels * double(x);
            startY = double(chessboardZoneImg.rows) - squareSizePixels * double(y + 1);

            Mat squareZone = chessboardZoneImg.rowRange(int(startY), int(startY + squareSizePixels))
                                 .colRange(int(startX), int(startX + squareSizePixels));

            squareZone.setTo(0);
        }
    }
}



/**
 */
123
Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareLength,
124
                                  float markerLength, const Ptr<Dictionary> &dictionary) {
S. Garrido's avatar
S. Garrido committed
125 126

    CV_Assert(squaresX > 1 && squaresY > 1 && markerLength > 0 && squareLength > markerLength);
127
    Ptr<CharucoBoard> res = makePtr<CharucoBoard>();
S. Garrido's avatar
S. Garrido committed
128

129 130 131 132 133
    res->_squaresX = squaresX;
    res->_squaresY = squaresY;
    res->_squareLength = squareLength;
    res->_markerLength = markerLength;
    res->dictionary = dictionary;
S. Garrido's avatar
S. Garrido committed
134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149

    float diffSquareMarkerLength = (squareLength - markerLength) / 2;

    // calculate Board objPoints
    for(int y = squaresY - 1; y >= 0; y--) {
        for(int x = 0; x < squaresX; x++) {

            if(y % 2 == x % 2) continue; // black corner, no marker here

            vector< Point3f > corners;
            corners.resize(4);
            corners[0] = Point3f(x * squareLength + diffSquareMarkerLength,
                                 y * squareLength + diffSquareMarkerLength + markerLength, 0);
            corners[1] = corners[0] + Point3f(markerLength, 0, 0);
            corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0);
            corners[3] = corners[0] + Point3f(0, -markerLength, 0);
150
            res->objPoints.push_back(corners);
S. Garrido's avatar
S. Garrido committed
151
            // first ids in dictionary
152 153
            int nextId = (int)res->ids.size();
            res->ids.push_back(nextId);
S. Garrido's avatar
S. Garrido committed
154 155 156 157 158 159 160 161 162 163
        }
    }

    // now fill chessboardCorners
    for(int y = 0; y < squaresY - 1; y++) {
        for(int x = 0; x < squaresX - 1; x++) {
            Point3f corner;
            corner.x = (x + 1) * squareLength;
            corner.y = (y + 1) * squareLength;
            corner.z = 0;
164
            res->chessboardCorners.push_back(corner);
S. Garrido's avatar
S. Garrido committed
165 166 167
        }
    }

168
    res->_getNearestMarkerCorners();
S. Garrido's avatar
S. Garrido committed
169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196

    return res;
}



/**
  * Fill nearestMarkerIdx and nearestMarkerCorners arrays
  */
void CharucoBoard::_getNearestMarkerCorners() {

    nearestMarkerIdx.resize(chessboardCorners.size());
    nearestMarkerCorners.resize(chessboardCorners.size());

    unsigned int nMarkers = (unsigned int)ids.size();
    unsigned int nCharucoCorners = (unsigned int)chessboardCorners.size();
    for(unsigned int i = 0; i < nCharucoCorners; i++) {
        double minDist = -1; // distance of closest markers
        Point3f charucoCorner = chessboardCorners[i];
        for(unsigned int j = 0; j < nMarkers; j++) {
            // calculate distance from marker center to charuco corner
            Point3f center = Point3f(0, 0, 0);
            for(unsigned int k = 0; k < 4; k++)
                center += objPoints[j][k];
            center /= 4.;
            double sqDistance;
            Point3f distVector = charucoCorner - center;
            sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
197
            if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * _squareLength, 2)) {
S. Garrido's avatar
S. Garrido committed
198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232
                // if same minimum distance (or first iteration), add to nearestMarkerIdx vector
                nearestMarkerIdx[i].push_back(j);
                minDist = sqDistance;
            } else if(sqDistance < minDist) {
                // if finding a closest marker to the charuco corner
                nearestMarkerIdx[i].clear(); // remove any previous added marker
                nearestMarkerIdx[i].push_back(j); // add the new closest marker index
                minDist = sqDistance;
            }
        }

        // for each of the closest markers, search the marker corner index closer
        // to the charuco corner
        for(unsigned int j = 0; j < nearestMarkerIdx[i].size(); j++) {
            nearestMarkerCorners[i].resize(nearestMarkerIdx[i].size());
            double minDistCorner = -1;
            for(unsigned int k = 0; k < 4; k++) {
                double sqDistance;
                Point3f distVector = charucoCorner - objPoints[nearestMarkerIdx[i][j]][k];
                sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
                if(k == 0 || sqDistance < minDistCorner) {
                    // if this corner is closer to the charuco corner, assing its index
                    // to nearestMarkerCorners
                    minDistCorner = sqDistance;
                    nearestMarkerCorners[i][j] = k;
                }
            }
        }
    }
}


/**
  * Remove charuco corners if any of their minMarkers closest markers has not been detected
  */
233
static int _filterCornersWithoutMinMarkers(const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
234 235 236 237 238 239 240 241 242 243 244 245
                                                    InputArray _allCharucoCorners,
                                                    InputArray _allCharucoIds,
                                                    InputArray _allArucoIds, int minMarkers,
                                                    OutputArray _filteredCharucoCorners,
                                                    OutputArray _filteredCharucoIds) {

    CV_Assert(minMarkers >= 0 && minMarkers <= 2);

    vector< Point2f > filteredCharucoCorners;
    vector< int > filteredCharucoIds;
    // for each charuco corner
    for(unsigned int i = 0; i < _allCharucoIds.getMat().total(); i++) {
246
        int currentCharucoId = _allCharucoIds.getMat().at< int >(i);
S. Garrido's avatar
S. Garrido committed
247 248
        int totalMarkers = 0; // nomber of closest marker detected
        // look for closest markers
249 250
        for(unsigned int m = 0; m < _board->nearestMarkerIdx[currentCharucoId].size(); m++) {
            int markerId = _board->ids[_board->nearestMarkerIdx[currentCharucoId][m]];
S. Garrido's avatar
S. Garrido committed
251 252
            bool found = false;
            for(unsigned int k = 0; k < _allArucoIds.getMat().total(); k++) {
253
                if(_allArucoIds.getMat().at< int >(k) == markerId) {
S. Garrido's avatar
S. Garrido committed
254 255 256 257 258 259 260 261 262
                    found = true;
                    break;
                }
            }
            if(found) totalMarkers++;
        }
        // if enough markers detected, add the charuco corner to the final list
        if(totalMarkers >= minMarkers) {
            filteredCharucoIds.push_back(currentCharucoId);
263
            filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at< Point2f >(i));
S. Garrido's avatar
S. Garrido committed
264 265 266 267
        }
    }

    // parse output
268 269 270
    Mat(filteredCharucoCorners).copyTo(_filteredCharucoCorners);
    Mat(filteredCharucoIds).copyTo(_filteredCharucoIds);
    return (int)_filteredCharucoIds.total();
S. Garrido's avatar
S. Garrido committed
271 272 273 274 275 276
}

/**
  * @brief From all projected chessboard corners, select those inside the image and apply subpixel
  * refinement. Returns number of valid corners.
  */
277
static int _selectAndRefineChessboardCorners(InputArray _allCorners, InputArray _image,
S. Garrido's avatar
S. Garrido committed
278 279 280 281 282 283 284 285 286 287 288 289 290 291
                                                      OutputArray _selectedCorners,
                                                      OutputArray _selectedIds,
                                                      const vector< Size > &winSizes) {

    const int minDistToBorder = 2; // minimum distance of the corner to the image border
    // remaining corners, ids and window refinement sizes after removing corners outside the image
    vector< Point2f > filteredChessboardImgPoints;
    vector< Size > filteredWinSizes;
    vector< int > filteredIds;

    // filter corners outside the image
    Rect innerRect(minDistToBorder, minDistToBorder, _image.getMat().cols - 2 * minDistToBorder,
                   _image.getMat().rows - 2 * minDistToBorder);
    for(unsigned int i = 0; i < _allCorners.getMat().total(); i++) {
292 293
        if(innerRect.contains(_allCorners.getMat().at< Point2f >(i))) {
            filteredChessboardImgPoints.push_back(_allCorners.getMat().at< Point2f >(i));
S. Garrido's avatar
S. Garrido committed
294 295 296 297 298 299 300 301 302 303
            filteredIds.push_back(i);
            filteredWinSizes.push_back(winSizes[i]);
        }
    }

    // if none valid, return 0
    if(filteredChessboardImgPoints.size() == 0) return 0;

    // corner refinement, first convert input image to grey
    Mat grey;
Suleyman TURKMEN's avatar
Suleyman TURKMEN committed
304 305
    if(_image.type() == CV_8UC3)
        cvtColor(_image, grey, COLOR_BGR2GRAY);
S. Garrido's avatar
S. Garrido committed
306
    else
Suleyman TURKMEN's avatar
Suleyman TURKMEN committed
307
        _image.copyTo(grey);
S. Garrido's avatar
S. Garrido committed
308

309
    const Ptr<DetectorParameters> params = DetectorParameters::create(); // use default params for corner refinement
S. Garrido's avatar
S. Garrido committed
310 311

    //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize
312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330
    parallel_for_(Range(0, (int)filteredChessboardImgPoints.size()), [&](const Range& range) {
        const int begin = range.start;
        const int end = range.end;

        for (int i = begin; i < end; i++) {
            vector<Point2f> in;
            in.push_back(filteredChessboardImgPoints[i]);
            Size winSize = filteredWinSizes[i];
            if (winSize.height == -1 || winSize.width == -1)
                winSize = Size(params->cornerRefinementWinSize, params->cornerRefinementWinSize);

            cornerSubPix(grey, in, winSize, Size(),
                         TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
                                      params->cornerRefinementMaxIterations,
                                      params->cornerRefinementMinAccuracy));

            filteredChessboardImgPoints[i] = in[0];
        }
    });
S. Garrido's avatar
S. Garrido committed
331 332

    // parse output
333 334 335
    Mat(filteredChessboardImgPoints).copyTo(_selectedCorners);
    Mat(filteredIds).copyTo(_selectedIds);
    return (int)filteredChessboardImgPoints.size();
S. Garrido's avatar
S. Garrido committed
336 337 338 339 340 341 342 343
}


/**
  * Calculate the maximum window sizes for corner refinement for each charuco corner based on the
  * distance to their closest markers
  */
static void _getMaximumSubPixWindowSizes(InputArrayOfArrays markerCorners, InputArray markerIds,
344
                                         InputArray charucoCorners, const Ptr<CharucoBoard> &board,
S. Garrido's avatar
S. Garrido committed
345 346 347 348 349 350
                                         vector< Size > &sizes) {

    unsigned int nCharucoCorners = (unsigned int)charucoCorners.getMat().total();
    sizes.resize(nCharucoCorners, Size(-1, -1));

    for(unsigned int i = 0; i < nCharucoCorners; i++) {
351
        if(charucoCorners.getMat().at< Point2f >(i) == Point2f(-1, -1)) continue;
352
        if(board->nearestMarkerIdx[i].size() == 0) continue;
S. Garrido's avatar
S. Garrido committed
353 354 355 356 357

        double minDist = -1;
        int counter = 0;

        // calculate the distance to each of the closest corner of each closest marker
358
        for(unsigned int j = 0; j < board->nearestMarkerIdx[i].size(); j++) {
S. Garrido's avatar
S. Garrido committed
359
            // find marker
360
            int markerId = board->ids[board->nearestMarkerIdx[i][j]];
S. Garrido's avatar
S. Garrido committed
361 362
            int markerIdx = -1;
            for(unsigned int k = 0; k < markerIds.getMat().total(); k++) {
363
                if(markerIds.getMat().at< int >(k) == markerId) {
S. Garrido's avatar
S. Garrido committed
364 365 366 367 368 369
                    markerIdx = k;
                    break;
                }
            }
            if(markerIdx == -1) continue;
            Point2f markerCorner =
370 371
                markerCorners.getMat(markerIdx).at< Point2f >(board->nearestMarkerCorners[i][j]);
            Point2f charucoCorner = charucoCorners.getMat().at< Point2f >(i);
S. Garrido's avatar
S. Garrido committed
372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397
            double dist = norm(markerCorner - charucoCorner);
            if(minDist == -1) minDist = dist; // if first distance, just assign it
            minDist = min(dist, minDist);
            counter++;
        }

        // if this is the first closest marker, dont do anything
        if(counter == 0)
            continue;
        else {
            // else, calculate the maximum window size
            int winSizeInt = int(minDist - 2); // remove 2 pixels for safety
            if(winSizeInt < 1) winSizeInt = 1; // minimum size is 1
            if(winSizeInt > 10) winSizeInt = 10; // maximum size is 10
            sizes[i] = Size(winSizeInt, winSizeInt);
        }
    }
}



/**
  * Interpolate charuco corners using approximated pose estimation
  */
static int _interpolateCornersCharucoApproxCalib(InputArrayOfArrays _markerCorners,
                                                 InputArray _markerIds, InputArray _image,
398
                                                 const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
399 400 401 402 403 404 405 406 407 408 409
                                                 InputArray _cameraMatrix, InputArray _distCoeffs,
                                                 OutputArray _charucoCorners,
                                                 OutputArray _charucoIds) {

    CV_Assert(_image.getMat().channels() == 1 || _image.getMat().channels() == 3);
    CV_Assert(_markerCorners.total() == _markerIds.getMat().total() &&
              _markerIds.getMat().total() > 0);

    // approximated pose estimation using marker corners
    Mat approximatedRvec, approximatedTvec;
    int detectedBoardMarkers;
410
    Ptr<Board> _b = _board.staticCast<Board>();
S. Garrido's avatar
S. Garrido committed
411
    detectedBoardMarkers =
412 413
        aruco::estimatePoseBoard(_markerCorners, _markerIds, _b,
                                 _cameraMatrix, _distCoeffs, approximatedRvec, approximatedTvec);
S. Garrido's avatar
S. Garrido committed
414 415 416 417 418

    if(detectedBoardMarkers == 0) return 0;

    // project chessboard corners
    vector< Point2f > allChessboardImgPoints;
419 420

    projectPoints(_board->chessboardCorners, approximatedRvec, approximatedTvec, _cameraMatrix,
S. Garrido's avatar
S. Garrido committed
421 422 423 424 425 426
                  _distCoeffs, allChessboardImgPoints);


    // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
    // to the closes marker corner to avoid erroneous displacements to marker corners
    vector< Size > subPixWinSizes;
427
    _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
S. Garrido's avatar
S. Garrido committed
428 429 430
                                 subPixWinSizes);

    // filter corners outside the image and subpixel-refine charuco corners
431 432
    return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
                                             _charucoIds, subPixWinSizes);
S. Garrido's avatar
S. Garrido committed
433 434 435 436 437 438 439 440 441
}



/**
  * Interpolate charuco corners using local homography
  */
static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
                                              InputArray _markerIds, InputArray _image,
442
                                              const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
443 444 445 446 447 448 449 450 451 452 453 454 455 456
                                              OutputArray _charucoCorners,
                                              OutputArray _charucoIds) {

    CV_Assert(_image.getMat().channels() == 1 || _image.getMat().channels() == 3);
    CV_Assert(_markerCorners.total() == _markerIds.getMat().total() &&
              _markerIds.getMat().total() > 0);

    unsigned int nMarkers = (unsigned int)_markerIds.getMat().total();

    // calculate local homographies for each marker
    vector< Mat > transformations;
    transformations.resize(nMarkers);
    for(unsigned int i = 0; i < nMarkers; i++) {
        vector< Point2f > markerObjPoints2D;
457
        int markerId = _markerIds.getMat().at< int >(i);
458 459 460
        vector< int >::const_iterator it = find(_board->ids.begin(), _board->ids.end(), markerId);
        if(it == _board->ids.end()) continue;
        int boardIdx = (int)std::distance<std::vector<int>::const_iterator>(_board->ids.begin(), it);
S. Garrido's avatar
S. Garrido committed
461 462 463
        markerObjPoints2D.resize(4);
        for(unsigned int j = 0; j < 4; j++)
            markerObjPoints2D[j] =
464
                Point2f(_board->objPoints[boardIdx][j].x, _board->objPoints[boardIdx][j].y);
S. Garrido's avatar
S. Garrido committed
465 466 467 468

        transformations[i] = getPerspectiveTransform(markerObjPoints2D, _markerCorners.getMat(i));
    }

469
    unsigned int nCharucoCorners = (unsigned int)_board->chessboardCorners.size();
S. Garrido's avatar
S. Garrido committed
470 471 472 473 474
    vector< Point2f > allChessboardImgPoints(nCharucoCorners, Point2f(-1, -1));

    // for each charuco corner, calculate its interpolation position based on the closest markers
    // homographies
    for(unsigned int i = 0; i < nCharucoCorners; i++) {
475
        Point2f objPoint2D = Point2f(_board->chessboardCorners[i].x, _board->chessboardCorners[i].y);
S. Garrido's avatar
S. Garrido committed
476 477

        vector< Point2f > interpolatedPositions;
478 479
        for(unsigned int j = 0; j < _board->nearestMarkerIdx[i].size(); j++) {
            int markerId = _board->ids[_board->nearestMarkerIdx[i][j]];
S. Garrido's avatar
S. Garrido committed
480 481
            int markerIdx = -1;
            for(unsigned int k = 0; k < _markerIds.getMat().total(); k++) {
482
                if(_markerIds.getMat().at< int >(k) == markerId) {
S. Garrido's avatar
S. Garrido committed
483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508
                    markerIdx = k;
                    break;
                }
            }
            if(markerIdx != -1) {
                vector< Point2f > in, out;
                in.push_back(objPoint2D);
                perspectiveTransform(in, out, transformations[markerIdx]);
                interpolatedPositions.push_back(out[0]);
            }
        }

        // none of the closest markers detected
        if(interpolatedPositions.size() == 0) continue;

        // more than one closest marker detected, take middle point
        if(interpolatedPositions.size() > 1) {
            allChessboardImgPoints[i] = (interpolatedPositions[0] + interpolatedPositions[1]) / 2.;
        }
        // a single closest marker detected
        else allChessboardImgPoints[i] = interpolatedPositions[0];
    }

    // calculate maximum window sizes for subpixel refinement. The size is limited by the distance
    // to the closes marker corner to avoid erroneous displacements to marker corners
    vector< Size > subPixWinSizes;
509
    _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
S. Garrido's avatar
S. Garrido committed
510 511 512 513
                                 subPixWinSizes);


    // filter corners outside the image and subpixel-refine charuco corners
514 515
    return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
                                             _charucoIds, subPixWinSizes);
S. Garrido's avatar
S. Garrido committed
516 517 518 519 520 521 522
}



/**
  */
int interpolateCornersCharuco(InputArrayOfArrays _markerCorners, InputArray _markerIds,
523
                              InputArray _image, const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
524
                              OutputArray _charucoCorners, OutputArray _charucoIds,
525
                              InputArray _cameraMatrix, InputArray _distCoeffs, int minMarkers) {
S. Garrido's avatar
S. Garrido committed
526 527 528

    // if camera parameters are avaible, use approximated calibration
    if(_cameraMatrix.total() != 0) {
529
        _interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, _image, _board,
S. Garrido's avatar
S. Garrido committed
530 531 532 533 534
                                                     _cameraMatrix, _distCoeffs, _charucoCorners,
                                                     _charucoIds);
    }
    // else use local homography
    else {
535
        _interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, _image, _board,
S. Garrido's avatar
S. Garrido committed
536 537
                                                  _charucoCorners, _charucoIds);
    }
538 539 540 541

    // to return a charuco corner, its closest aruco markers should have been detected
    return _filterCornersWithoutMinMarkers(_board, _charucoCorners, _charucoIds, _markerIds,
                                           minMarkers, _charucoCorners, _charucoIds);
S. Garrido's avatar
S. Garrido committed
542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557
}



/**
  */
void drawDetectedCornersCharuco(InputOutputArray _image, InputArray _charucoCorners,
                                InputArray _charucoIds, Scalar cornerColor) {

    CV_Assert(_image.getMat().total() != 0 &&
              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
    CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()) ||
              _charucoIds.getMat().total() == 0);

    unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();
    for(unsigned int i = 0; i < nCorners; i++) {
558
        Point2f corner = _charucoCorners.getMat().at< Point2f >(i);
S. Garrido's avatar
S. Garrido committed
559 560 561 562 563 564

        // draw first corner mark
        rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA);

        // draw ID
        if(_charucoIds.total() != 0) {
565
            int id = _charucoIds.getMat().at< int >(i);
S. Garrido's avatar
S. Garrido committed
566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615
            stringstream s;
            s << "id=" << id;
            putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 0.5,
                    cornerColor, 2);
        }
    }
}


/**
  * Check if a set of 3d points are enough for calibration. Z coordinate is ignored.
  * Only axis paralel lines are considered
  */
static bool _arePointsEnoughForPoseEstimation(const vector< Point3f > &points) {

    if(points.size() < 4) return false;

    vector< double > sameXValue; // different x values in points
    vector< int > sameXCounter;  // number of points with the x value in sameXValue
    for(unsigned int i = 0; i < points.size(); i++) {
        bool found = false;
        for(unsigned int j = 0; j < sameXValue.size(); j++) {
            if(sameXValue[j] == points[i].x) {
                found = true;
                sameXCounter[j]++;
            }
        }
        if(!found) {
            sameXValue.push_back(points[i].x);
            sameXCounter.push_back(1);
        }
    }

    // count how many x values has more than 2 points
    int moreThan2 = 0;
    for(unsigned int i = 0; i < sameXCounter.size(); i++) {
        if(sameXCounter[i] >= 2) moreThan2++;
    }

    // if we have more than 1 two xvalues with more than 2 points, calibration is ok
    if(moreThan2 > 1)
        return true;
    else
        return false;
}


/**
  */
bool estimatePoseCharucoBoard(InputArray _charucoCorners, InputArray _charucoIds,
616
                              const Ptr<CharucoBoard> &_board, InputArray _cameraMatrix, InputArray _distCoeffs,
617
                              InputOutputArray _rvec, InputOutputArray _tvec, bool useExtrinsicGuess) {
S. Garrido's avatar
S. Garrido committed
618 619 620 621 622 623 624 625 626

    CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()));

    // need, at least, 4 corners
    if(_charucoIds.getMat().total() < 4) return false;

    vector< Point3f > objPoints;
    objPoints.reserve(_charucoIds.getMat().total());
    for(unsigned int i = 0; i < _charucoIds.getMat().total(); i++) {
627
        int currId = _charucoIds.getMat().at< int >(i);
628 629
        CV_Assert(currId >= 0 && currId < (int)_board->chessboardCorners.size());
        objPoints.push_back(_board->chessboardCorners[currId]);
S. Garrido's avatar
S. Garrido committed
630 631 632 633 634
    }

    // points need to be in different lines, check if detected points are enough
    if(!_arePointsEnoughForPoseEstimation(objPoints)) return false;

635
    solvePnP(objPoints, _charucoCorners, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
S. Garrido's avatar
S. Garrido committed
636 637 638 639 640 641 642 643 644 645

    return true;
}




/**
  */
double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
646
                              const Ptr<CharucoBoard> &_board, Size imageSize,
S. Garrido's avatar
S. Garrido committed
647
                              InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
648 649 650 651 652
                              OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
                              OutputArray _stdDeviationsIntrinsics,
                              OutputArray _stdDeviationsExtrinsics,
                              OutputArray _perViewErrors,
                              int flags, TermCriteria criteria) {
S. Garrido's avatar
S. Garrido committed
653 654 655 656 657 658 659 660 661 662 663 664

    CV_Assert(_charucoIds.total() > 0 && (_charucoIds.total() == _charucoCorners.total()));

    // Join object points of charuco corners in a single vector for calibrateCamera() function
    vector< vector< Point3f > > allObjPoints;
    allObjPoints.resize(_charucoIds.total());
    for(unsigned int i = 0; i < _charucoIds.total(); i++) {
        unsigned int nCorners = (unsigned int)_charucoIds.getMat(i).total();
        CV_Assert(nCorners > 0 && nCorners == _charucoCorners.getMat(i).total());
        allObjPoints[i].reserve(nCorners);

        for(unsigned int j = 0; j < nCorners; j++) {
665
            int pointId = _charucoIds.getMat(i).at< int >(j);
666 667
            CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size());
            allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
S. Garrido's avatar
S. Garrido committed
668 669 670 671
        }
    }

    return calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs,
672 673
                           _rvecs, _tvecs, _stdDeviationsIntrinsics, _stdDeviationsExtrinsics,
                           _perViewErrors, flags, criteria);
S. Garrido's avatar
S. Garrido committed
674 675 676 677
}



678 679 680
/**
 */
double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
681
  const Ptr<CharucoBoard> &_board, Size imageSize,
682 683 684 685 686 687 688 689
  InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
  OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags,
  TermCriteria criteria) {
    return calibrateCameraCharuco(_charucoCorners, _charucoIds, _board, imageSize, _cameraMatrix, _distCoeffs, _rvecs,
      _tvecs, noArray(), noArray(), noArray(), flags, criteria);
}


S. Garrido's avatar
S. Garrido committed
690 691 692 693 694 695 696 697 698
/**
 */
void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners,
                          InputArray _markerIds, float squareMarkerLengthRate,
                          OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds,
                          InputArray _cameraMatrix, InputArray _distCoeffs) {

    CV_Assert(_markerIds.total() > 0 && _markerIds.total() == _markerCorners.total());

699
    const float minRepDistanceRate = 1.302455f;
S. Garrido's avatar
S. Garrido committed
700 701

    // create Charuco board layout for diamond (3x3 layout)
702 703 704
    Ptr<Dictionary> dict = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0));
    Ptr<CharucoBoard> _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dict);

S. Garrido's avatar
S. Garrido committed
705 706 707 708 709 710 711 712 713 714

    vector< vector< Point2f > > diamondCorners;
    vector< Vec4i > diamondIds;

    // stores if the detected markers have been assigned or not to a diamond
    vector< bool > assigned(_markerIds.total(), false);
    if(_markerIds.total() < 4) return; // a diamond need at least 4 markers

    // convert input image to grey
    Mat grey;
Suleyman TURKMEN's avatar
Suleyman TURKMEN committed
715 716
    if(_image.type() == CV_8UC3)
        cvtColor(_image, grey, COLOR_BGR2GRAY);
S. Garrido's avatar
S. Garrido committed
717
    else
Suleyman TURKMEN's avatar
Suleyman TURKMEN committed
718
        _image.copyTo(grey);
S. Garrido's avatar
S. Garrido committed
719 720 721 722 723 724 725 726 727

    // for each of the detected markers, try to find a diamond
    for(unsigned int i = 0; i < _markerIds.total(); i++) {
        if(assigned[i]) continue;

        // calculate marker perimeter
        float perimeterSq = 0;
        Mat corners = _markerCorners.getMat(i);
        for(int c = 0; c < 4; c++) {
728 729
          Point2f edge = corners.at< Point2f >(c) - corners.at< Point2f >((c + 1) % 4);
          perimeterSq += edge.x*edge.x + edge.y*edge.y;
S. Garrido's avatar
S. Garrido committed
730 731
        }
        // maximum reprojection error relative to perimeter
732
        float minRepDistance = sqrt(perimeterSq) * minRepDistanceRate;
S. Garrido's avatar
S. Garrido committed
733

734
        int currentId = _markerIds.getMat().at< int >(i);
S. Garrido's avatar
S. Garrido committed
735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756

        // prepare data to call refineDetectedMarkers()
        // detected markers (only the current one)
        vector< Mat > currentMarker;
        vector< int > currentMarkerId;
        currentMarker.push_back(_markerCorners.getMat(i));
        currentMarkerId.push_back(currentId);

        // marker candidates (the rest of markers if they have not been assigned)
        vector< Mat > candidates;
        vector< int > candidatesIdxs;
        for(unsigned int k = 0; k < assigned.size(); k++) {
            if(k == i) continue;
            if(!assigned[k]) {
                candidates.push_back(_markerCorners.getMat(k));
                candidatesIdxs.push_back(k);
            }
        }
        if(candidates.size() < 3) break; // we need at least 3 free markers

        // modify charuco layout id to make sure all the ids are different than current id
        for(int k = 1; k < 4; k++)
757
            _charucoDiamondLayout->ids[k] = currentId + 1 + k;
S. Garrido's avatar
S. Garrido committed
758
        // current id is assigned to [0], so it is the marker on the top
759
        _charucoDiamondLayout->ids[0] = currentId;
S. Garrido's avatar
S. Garrido committed
760 761 762

        // try to find the rest of markers in the diamond
        vector< int > acceptedIdxs;
763 764 765
        Ptr<Board> _b = _charucoDiamondLayout.staticCast<Board>();
        aruco::refineDetectedMarkers(grey, _b,
                                     currentMarker, currentMarkerId,
S. Garrido's avatar
S. Garrido committed
766 767 768 769 770 771 772 773 774 775 776 777 778 779
                                     candidates, noArray(), noArray(), minRepDistance, -1, false,
                                     acceptedIdxs);

        // if found, we have a diamond
        if(currentMarker.size() == 4) {

            assigned[i] = true;

            // calculate diamond id, acceptedIdxs array indicates the markers taken from candidates
            // array
            Vec4i markerId;
            markerId[0] = currentId;
            for(int k = 1; k < 4; k++) {
                int currentMarkerIdx = candidatesIdxs[acceptedIdxs[k - 1]];
780
                markerId[k] = _markerIds.getMat().at< int >(currentMarkerIdx);
S. Garrido's avatar
S. Garrido committed
781 782 783 784 785 786
                assigned[currentMarkerIdx] = true;
            }

            // interpolate the charuco corners of the diamond
            vector< Point2f > currentMarkerCorners;
            Mat aux;
787
            interpolateCornersCharuco(currentMarker, currentMarkerId, grey, _charucoDiamondLayout,
S. Garrido's avatar
S. Garrido committed
788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808
                                      currentMarkerCorners, aux, _cameraMatrix, _distCoeffs);

            // if everything is ok, save the diamond
            if(currentMarkerCorners.size() > 0) {
                // reorder corners
                vector< Point2f > currentMarkerCornersReorder;
                currentMarkerCornersReorder.resize(4);
                currentMarkerCornersReorder[0] = currentMarkerCorners[2];
                currentMarkerCornersReorder[1] = currentMarkerCorners[3];
                currentMarkerCornersReorder[2] = currentMarkerCorners[1];
                currentMarkerCornersReorder[3] = currentMarkerCorners[0];

                diamondCorners.push_back(currentMarkerCornersReorder);
                diamondIds.push_back(markerId);
            }
        }
    }


    if(diamondIds.size() > 0) {
        // parse output
809
        Mat(diamondIds).copyTo(_diamondIds);
S. Garrido's avatar
S. Garrido committed
810 811 812 813 814

        _diamondCorners.create((int)diamondCorners.size(), 1, CV_32FC2);
        for(unsigned int i = 0; i < diamondCorners.size(); i++) {
            _diamondCorners.create(4, 1, CV_32FC2, i, true);
            for(int j = 0; j < 4; j++) {
815
                _diamondCorners.getMat(i).at< Point2f >(j) = diamondCorners[i][j];
S. Garrido's avatar
S. Garrido committed
816 817 818 819 820 821 822 823 824 825
            }
        }
    }
}




/**
  */
826
void drawCharucoDiamond(const Ptr<Dictionary> &dictionary, Vec4i ids, int squareLength, int markerLength,
S. Garrido's avatar
S. Garrido committed
827 828 829 830 831 832
                        OutputArray _img, int marginSize, int borderBits) {

    CV_Assert(squareLength > 0 && markerLength > 0 && squareLength > markerLength);
    CV_Assert(marginSize >= 0 && borderBits > 0);

    // create a charuco board similar to a charuco marker and print it
833
    Ptr<CharucoBoard> board =
S. Garrido's avatar
S. Garrido committed
834 835 836 837
        CharucoBoard::create(3, 3, (float)squareLength, (float)markerLength, dictionary);

    // assign the charuco marker ids
    for(int i = 0; i < 4; i++)
838
        board->ids[i] = ids[i];
S. Garrido's avatar
S. Garrido committed
839 840

    Size outSize(3 * squareLength + 2 * marginSize, 3 * squareLength + 2 * marginSize);
841
    board->draw(outSize, _img, marginSize, borderBits);
S. Garrido's avatar
S. Garrido committed
842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868
}


/**
 */
void drawDetectedDiamonds(InputOutputArray _image, InputArrayOfArrays _corners,
                          InputArray _ids, Scalar borderColor) {


    CV_Assert(_image.getMat().total() != 0 &&
              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
    CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0);

    // calculate colors
    Scalar textColor, cornerColor;
    textColor = cornerColor = borderColor;
    swap(textColor.val[0], textColor.val[1]);     // text color just sawp G and R
    swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B

    int nMarkers = (int)_corners.total();
    for(int i = 0; i < nMarkers; i++) {
        Mat currentMarker = _corners.getMat(i);
        CV_Assert(currentMarker.total() == 4 && currentMarker.type() == CV_32FC2);

        // draw marker sides
        for(int j = 0; j < 4; j++) {
            Point2f p0, p1;
869 870
            p0 = currentMarker.at< Point2f >(j);
            p1 = currentMarker.at< Point2f >((j + 1) % 4);
S. Garrido's avatar
S. Garrido committed
871 872 873 874
            line(_image, p0, p1, borderColor, 1);
        }

        // draw first corner mark
875 876
        rectangle(_image, currentMarker.at< Point2f >(0) - Point2f(3, 3),
                  currentMarker.at< Point2f >(0) + Point2f(3, 3), cornerColor, 1, LINE_AA);
S. Garrido's avatar
S. Garrido committed
877 878 879 880 881

        // draw id composed by four numbers
        if(_ids.total() != 0) {
            Point2f cent(0, 0);
            for(int p = 0; p < 4; p++)
882
                cent += currentMarker.at< Point2f >(p);
S. Garrido's avatar
S. Garrido committed
883 884
            cent = cent / 4.;
            stringstream s;
885
            s << "id=" << _ids.getMat().at< Vec4i >(i);
S. Garrido's avatar
S. Garrido committed
886 887 888 889 890 891
            putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2);
        }
    }
}
}
}