charuco.cpp 37.6 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 277 278 279 280
}


/**
  * ParallelLoopBody class for the parallelization of the charuco corners subpixel refinement
  * Called from function _selectAndRefineChessboardCorners()
  */
class CharucoSubpixelParallel : public ParallelLoopBody {
    public:
    CharucoSubpixelParallel(const Mat *_grey, vector< Point2f > *_filteredChessboardImgPoints,
281
                            vector< Size > *_filteredWinSizes, const Ptr<DetectorParameters> &_params)
S. Garrido's avatar
S. Garrido committed
282 283 284
        : grey(_grey), filteredChessboardImgPoints(_filteredChessboardImgPoints),
          filteredWinSizes(_filteredWinSizes), params(_params) {}

285
    void operator()(const Range &range) const CV_OVERRIDE {
S. Garrido's avatar
S. Garrido committed
286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310
        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];
        }
    }

    private:
    CharucoSubpixelParallel &operator=(const CharucoSubpixelParallel &); // to quiet MSVC

    const Mat *grey;
    vector< Point2f > *filteredChessboardImgPoints;
    vector< Size > *filteredWinSizes;
311
    const Ptr<DetectorParameters> &params;
S. Garrido's avatar
S. Garrido committed
312 313 314 315 316 317 318 319 320
};




/**
  * @brief From all projected chessboard corners, select those inside the image and apply subpixel
  * refinement. Returns number of valid corners.
  */
321
static int _selectAndRefineChessboardCorners(InputArray _allCorners, InputArray _image,
S. Garrido's avatar
S. Garrido committed
322 323 324 325 326 327 328 329 330 331 332 333 334 335
                                                      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++) {
336 337
        if(innerRect.contains(_allCorners.getMat().at< Point2f >(i))) {
            filteredChessboardImgPoints.push_back(_allCorners.getMat().at< Point2f >(i));
S. Garrido's avatar
S. Garrido committed
338 339 340 341 342 343 344 345 346 347
            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
348 349
    if(_image.type() == CV_8UC3)
        cvtColor(_image, grey, COLOR_BGR2GRAY);
S. Garrido's avatar
S. Garrido committed
350
    else
Suleyman TURKMEN's avatar
Suleyman TURKMEN committed
351
        _image.copyTo(grey);
S. Garrido's avatar
S. Garrido committed
352

353
    const Ptr<DetectorParameters> params = DetectorParameters::create(); // use default params for corner refinement
S. Garrido's avatar
S. Garrido committed
354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371

    //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize
    // for(unsigned int i=0; i<filteredChessboardImgPoints.size(); 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];
    //}

    // this is the parallel call for the previous commented loop (result is equivalent)
    parallel_for_(
        Range(0, (int)filteredChessboardImgPoints.size()),
372
        CharucoSubpixelParallel(&grey, &filteredChessboardImgPoints, &filteredWinSizes, params));
S. Garrido's avatar
S. Garrido committed
373 374

    // parse output
375 376 377
    Mat(filteredChessboardImgPoints).copyTo(_selectedCorners);
    Mat(filteredIds).copyTo(_selectedIds);
    return (int)filteredChessboardImgPoints.size();
S. Garrido's avatar
S. Garrido committed
378 379 380 381 382 383 384 385
}


/**
  * 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,
386
                                         InputArray charucoCorners, const Ptr<CharucoBoard> &board,
S. Garrido's avatar
S. Garrido committed
387 388 389 390 391 392
                                         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++) {
393
        if(charucoCorners.getMat().at< Point2f >(i) == Point2f(-1, -1)) continue;
394
        if(board->nearestMarkerIdx[i].size() == 0) continue;
S. Garrido's avatar
S. Garrido committed
395 396 397 398 399

        double minDist = -1;
        int counter = 0;

        // calculate the distance to each of the closest corner of each closest marker
400
        for(unsigned int j = 0; j < board->nearestMarkerIdx[i].size(); j++) {
S. Garrido's avatar
S. Garrido committed
401
            // find marker
402
            int markerId = board->ids[board->nearestMarkerIdx[i][j]];
S. Garrido's avatar
S. Garrido committed
403 404
            int markerIdx = -1;
            for(unsigned int k = 0; k < markerIds.getMat().total(); k++) {
405
                if(markerIds.getMat().at< int >(k) == markerId) {
S. Garrido's avatar
S. Garrido committed
406 407 408 409 410 411
                    markerIdx = k;
                    break;
                }
            }
            if(markerIdx == -1) continue;
            Point2f markerCorner =
412 413
                markerCorners.getMat(markerIdx).at< Point2f >(board->nearestMarkerCorners[i][j]);
            Point2f charucoCorner = charucoCorners.getMat().at< Point2f >(i);
S. Garrido's avatar
S. Garrido committed
414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439
            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,
440
                                                 const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
441 442 443 444 445 446 447 448 449 450 451
                                                 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;
452
    Ptr<Board> _b = _board.staticCast<Board>();
S. Garrido's avatar
S. Garrido committed
453
    detectedBoardMarkers =
454 455
        aruco::estimatePoseBoard(_markerCorners, _markerIds, _b,
                                 _cameraMatrix, _distCoeffs, approximatedRvec, approximatedTvec);
S. Garrido's avatar
S. Garrido committed
456 457 458 459 460

    if(detectedBoardMarkers == 0) return 0;

    // project chessboard corners
    vector< Point2f > allChessboardImgPoints;
461 462

    projectPoints(_board->chessboardCorners, approximatedRvec, approximatedTvec, _cameraMatrix,
S. Garrido's avatar
S. Garrido committed
463 464 465 466 467 468
                  _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;
469
    _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
S. Garrido's avatar
S. Garrido committed
470 471 472
                                 subPixWinSizes);

    // filter corners outside the image and subpixel-refine charuco corners
473 474
    return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
                                             _charucoIds, subPixWinSizes);
S. Garrido's avatar
S. Garrido committed
475 476 477 478 479 480 481 482 483
}



/**
  * Interpolate charuco corners using local homography
  */
static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
                                              InputArray _markerIds, InputArray _image,
484
                                              const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
485 486 487 488 489 490 491 492 493 494 495 496 497 498
                                              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;
499
        int markerId = _markerIds.getMat().at< int >(i);
500 501 502
        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
503 504 505
        markerObjPoints2D.resize(4);
        for(unsigned int j = 0; j < 4; j++)
            markerObjPoints2D[j] =
506
                Point2f(_board->objPoints[boardIdx][j].x, _board->objPoints[boardIdx][j].y);
S. Garrido's avatar
S. Garrido committed
507 508 509 510

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

511
    unsigned int nCharucoCorners = (unsigned int)_board->chessboardCorners.size();
S. Garrido's avatar
S. Garrido committed
512 513 514 515 516
    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++) {
517
        Point2f objPoint2D = Point2f(_board->chessboardCorners[i].x, _board->chessboardCorners[i].y);
S. Garrido's avatar
S. Garrido committed
518 519

        vector< Point2f > interpolatedPositions;
520 521
        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
522 523
            int markerIdx = -1;
            for(unsigned int k = 0; k < _markerIds.getMat().total(); k++) {
524
                if(_markerIds.getMat().at< int >(k) == markerId) {
S. Garrido's avatar
S. Garrido committed
525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550
                    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;
551
    _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
S. Garrido's avatar
S. Garrido committed
552 553 554 555
                                 subPixWinSizes);


    // filter corners outside the image and subpixel-refine charuco corners
556 557
    return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
                                             _charucoIds, subPixWinSizes);
S. Garrido's avatar
S. Garrido committed
558 559 560 561 562 563 564
}



/**
  */
int interpolateCornersCharuco(InputArrayOfArrays _markerCorners, InputArray _markerIds,
565
                              InputArray _image, const Ptr<CharucoBoard> &_board,
S. Garrido's avatar
S. Garrido committed
566
                              OutputArray _charucoCorners, OutputArray _charucoIds,
567
                              InputArray _cameraMatrix, InputArray _distCoeffs, int minMarkers) {
S. Garrido's avatar
S. Garrido committed
568 569 570

    // if camera parameters are avaible, use approximated calibration
    if(_cameraMatrix.total() != 0) {
571
        _interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, _image, _board,
S. Garrido's avatar
S. Garrido committed
572 573 574 575 576
                                                     _cameraMatrix, _distCoeffs, _charucoCorners,
                                                     _charucoIds);
    }
    // else use local homography
    else {
577
        _interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, _image, _board,
S. Garrido's avatar
S. Garrido committed
578 579
                                                  _charucoCorners, _charucoIds);
    }
580 581 582 583

    // 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
584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599
}



/**
  */
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++) {
600
        Point2f corner = _charucoCorners.getMat().at< Point2f >(i);
S. Garrido's avatar
S. Garrido committed
601 602 603 604 605 606

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

        // draw ID
        if(_charucoIds.total() != 0) {
607
            int id = _charucoIds.getMat().at< int >(i);
S. Garrido's avatar
S. Garrido committed
608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657
            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,
658
                              const Ptr<CharucoBoard> &_board, InputArray _cameraMatrix, InputArray _distCoeffs,
659
                              OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess) {
S. Garrido's avatar
S. Garrido committed
660 661 662 663 664 665 666 667 668

    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++) {
669
        int currId = _charucoIds.getMat().at< int >(i);
670 671
        CV_Assert(currId >= 0 && currId < (int)_board->chessboardCorners.size());
        objPoints.push_back(_board->chessboardCorners[currId]);
S. Garrido's avatar
S. Garrido committed
672 673 674 675 676
    }

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

677
    solvePnP(objPoints, _charucoCorners, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
S. Garrido's avatar
S. Garrido committed
678 679 680 681 682 683 684 685 686 687

    return true;
}




/**
  */
double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
688
                              const Ptr<CharucoBoard> &_board, Size imageSize,
S. Garrido's avatar
S. Garrido committed
689
                              InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
690 691 692 693 694
                              OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
                              OutputArray _stdDeviationsIntrinsics,
                              OutputArray _stdDeviationsExtrinsics,
                              OutputArray _perViewErrors,
                              int flags, TermCriteria criteria) {
S. Garrido's avatar
S. Garrido committed
695 696 697 698 699 700 701 702 703 704 705 706

    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++) {
707
            int pointId = _charucoIds.getMat(i).at< int >(j);
708 709
            CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size());
            allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
S. Garrido's avatar
S. Garrido committed
710 711 712 713
        }
    }

    return calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs,
714 715
                           _rvecs, _tvecs, _stdDeviationsIntrinsics, _stdDeviationsExtrinsics,
                           _perViewErrors, flags, criteria);
S. Garrido's avatar
S. Garrido committed
716 717 718 719
}



720 721 722
/**
 */
double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
723
  const Ptr<CharucoBoard> &_board, Size imageSize,
724 725 726 727 728 729 730 731
  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
732 733 734 735 736 737 738 739 740
/**
 */
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());

741
    const float minRepDistanceRate = 1.302455f;
S. Garrido's avatar
S. Garrido committed
742 743

    // create Charuco board layout for diamond (3x3 layout)
744 745 746
    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
747 748 749 750 751 752 753 754 755 756

    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
757 758
    if(_image.type() == CV_8UC3)
        cvtColor(_image, grey, COLOR_BGR2GRAY);
S. Garrido's avatar
S. Garrido committed
759
    else
Suleyman TURKMEN's avatar
Suleyman TURKMEN committed
760
        _image.copyTo(grey);
S. Garrido's avatar
S. Garrido committed
761 762 763 764 765 766 767 768 769

    // 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++) {
770 771
          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
772 773
        }
        // maximum reprojection error relative to perimeter
774
        float minRepDistance = sqrt(perimeterSq) * minRepDistanceRate;
S. Garrido's avatar
S. Garrido committed
775

776
        int currentId = _markerIds.getMat().at< int >(i);
S. Garrido's avatar
S. Garrido committed
777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798

        // 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++)
799
            _charucoDiamondLayout->ids[k] = currentId + 1 + k;
S. Garrido's avatar
S. Garrido committed
800
        // current id is assigned to [0], so it is the marker on the top
801
        _charucoDiamondLayout->ids[0] = currentId;
S. Garrido's avatar
S. Garrido committed
802 803 804

        // try to find the rest of markers in the diamond
        vector< int > acceptedIdxs;
805 806 807
        Ptr<Board> _b = _charucoDiamondLayout.staticCast<Board>();
        aruco::refineDetectedMarkers(grey, _b,
                                     currentMarker, currentMarkerId,
S. Garrido's avatar
S. Garrido committed
808 809 810 811 812 813 814 815 816 817 818 819 820 821
                                     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]];
822
                markerId[k] = _markerIds.getMat().at< int >(currentMarkerIdx);
S. Garrido's avatar
S. Garrido committed
823 824 825 826 827 828
                assigned[currentMarkerIdx] = true;
            }

            // interpolate the charuco corners of the diamond
            vector< Point2f > currentMarkerCorners;
            Mat aux;
829
            interpolateCornersCharuco(currentMarker, currentMarkerId, grey, _charucoDiamondLayout,
S. Garrido's avatar
S. Garrido committed
830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850
                                      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
851
        Mat(diamondIds).copyTo(_diamondIds);
S. Garrido's avatar
S. Garrido committed
852 853 854 855 856

        _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++) {
857
                _diamondCorners.getMat(i).at< Point2f >(j) = diamondCorners[i][j];
S. Garrido's avatar
S. Garrido committed
858 859 860 861 862 863 864 865 866 867
            }
        }
    }
}




/**
  */
868
void drawCharucoDiamond(const Ptr<Dictionary> &dictionary, Vec4i ids, int squareLength, int markerLength,
S. Garrido's avatar
S. Garrido committed
869 870 871 872 873 874
                        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
875
    Ptr<CharucoBoard> board =
S. Garrido's avatar
S. Garrido committed
876 877 878 879
        CharucoBoard::create(3, 3, (float)squareLength, (float)markerLength, dictionary);

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

    Size outSize(3 * squareLength + 2 * marginSize, 3 * squareLength + 2 * marginSize);
883
    board->draw(outSize, _img, marginSize, borderBits);
S. Garrido's avatar
S. Garrido committed
884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910
}


/**
 */
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;
911 912
            p0 = currentMarker.at< Point2f >(j);
            p1 = currentMarker.at< Point2f >((j + 1) % 4);
S. Garrido's avatar
S. Garrido committed
913 914 915 916
            line(_image, p0, p1, borderColor, 1);
        }

        // draw first corner mark
917 918
        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
919 920 921 922 923

        // draw id composed by four numbers
        if(_ids.total() != 0) {
            Point2f cent(0, 0);
            for(int p = 0; p < 4; p++)
924
                cent += currentMarker.at< Point2f >(p);
S. Garrido's avatar
S. Garrido committed
925 926
            cent = cent / 4.;
            stringstream s;
927
            s << "id=" << _ids.getMat().at< Vec4i >(i);
S. Garrido's avatar
S. Garrido committed
928 929 930 931 932 933
            putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2);
        }
    }
}
}
}