text: /*
  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) {
  
      CV_Assert(!outSize.empty());
      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;
      aruco::_drawPlanarBoardImpl(this, chessboardZoneImg.size(), markersImg,
                                  diffSquareMarkerLengthPixels, borderBits);
  
      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);
          }
      }
  }
  
  
  
  /**
   */
  Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareLength,
                                    float markerLength, const Ptr<Dictionary> &dictionary) {
  
      CV_Assert(squaresX > 1 && squaresY > 1 && markerLength > 0 && squareLength > markerLength);
      Ptr<CharucoBoard> res = makePtr<CharucoBoard>();
  
      res->_squaresX = squaresX;
      res->_squaresY = squaresY;
      res->_squareLength = squareLength;
      res->_markerLength = markerLength;
      res->dictionary = dictionary;
  
      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);
              res->objPoints.push_back(corners);
              // first ids in dictionary
              int nextId = (int)res->ids.size();
              res->ids.push_back(nextId);
          }
      }
  
      // 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;
              res->chessboardCorners.push_back(corner);
          }
      }
  
      res->_getNearestMarkerCorners();
  
      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;
              if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * _squareLength, 2)) {
                  // 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
    */
  static int _filterCornersWithoutMinMarkers(const Ptr<CharucoBoard> &_board,
                                                      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++) {
          int currentCharucoId = _allCharucoIds.getMat().at< int >(i);
          int totalMarkers = 0; // nomber of closest marker detected
          // look for closest markers
          for(unsigned int m = 0; m < _board->nearestMarkerIdx[currentCharucoId].size(); m++) {
              int markerId = _board->ids[_board->nearestMarkerIdx[currentCharucoId][m]];
              bool found = false;
              for(unsigned int k = 0; k < _allArucoIds.getMat().total(); k++) {
                  if(_allArucoIds.getMat().at< int >(k) == markerId) {
                      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);
              filteredCharucoCorners.push_back(_allCharucoCorners.getMat().at< Point2f >(i));
          }
      }
  
      // parse output
      Mat(filteredCharucoCorners).copyTo(_filteredCharucoCorners);
      Mat(filteredCharucoIds).copyTo(_filteredCharucoIds);
      return (int)_filteredCharucoIds.total();
  }
  
  /**
    * @brief From all projected chessboard corners, select those inside the image and apply subpixel
    * refinement. Returns number of valid corners.
    */
  static int _selectAndRefineChessboardCorners(InputArray _allCorners, InputArray _image,
                                                        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++) {
          if(innerRect.contains(_allCorners.getMat().at< Point2f >(i))) {
              filteredChessboardImgPoints.push_back(_allCorners.getMat().at< Point2f >(i));
              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;
      if(_image.type() == CV_8UC3)
          cvtColor(_image, grey, COLOR_BGR2GRAY);
      else
          _image.copyTo(grey);
  
      const Ptr<DetectorParameters> params = DetectorParameters::create(); // use default params for corner refinement
  
      //// For each of the charuco corners, apply subpixel refinement using its correspondind winSize
      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];
          }
      });
  
      // parse output
      Mat(filteredChessboardImgPoints).copyTo(_selectedCorners);
      Mat(filteredIds).copyTo(_selectedIds);
      return (int)filteredChessboardImgPoints.size();
  }
  
  
  /**
    * 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,
                                           InputArray charucoCorners, const Ptr<CharucoBoard> &board,
                                           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++) {
          if(charucoCorners.getMat().at< Point2f >(i) == Point2f(-1, -1)) continue;
          if(board->nearestMarkerIdx[i].size() == 0) continue;
  
          double minDist = -1;
          int counter = 0;
  
          // calculate the distance to each of the closest corner of each closest marker
          for(unsigned int j = 0; j < board->nearestMarkerIdx[i].size(); j++) {
              // find marker
              int markerId = board->ids[board->nearestMarkerIdx[i][j]];
              int markerIdx = -1;
              for(unsigned int k = 0; k < markerIds.getMat().total(); k++) {
                  if(markerIds.getMat().at< int >(k) == markerId) {
                      markerIdx = k;
                      break;
                  }
              }
              if(markerIdx == -1) continue;
              Point2f markerCorner =
                  markerCorners.getMat(markerIdx).at< Point2f >(board->nearestMarkerCorners[i][j]);
              Point2f charucoCorner = charucoCorners.getMat().at< Point2f >(i);
              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,
                                                   const Ptr<CharucoBoard> &_board,
                                                   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;
      Ptr<Board> _b = _board.staticCast<Board>();
      detectedBoardMarkers =
          aruco::estimatePoseBoard(_markerCorners, _markerIds, _b,
                                   _cameraMatrix, _distCoeffs, approximatedRvec, approximatedTvec);
  
      if(detectedBoardMarkers == 0) return 0;
  
      // project chessboard corners
      vector< Point2f > allChessboardImgPoints;
  
      projectPoints(_board->chessboardCorners, approximatedRvec, approximatedTvec, _cameraMatrix,
                    _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;
      _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
                                   subPixWinSizes);
  
      // filter corners outside the image and subpixel-refine charuco corners
      return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
                                               _charucoIds, subPixWinSizes);
  }
  
  
  
  /**
    * Interpolate charuco corners using local homography
    */
  static int _interpolateCornersCharucoLocalHom(InputArrayOfArrays _markerCorners,
                                                InputArray _markerIds, InputArray _image,
                                                const Ptr<CharucoBoard> &_board,
                                                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);
  
      vector< bool > validTransform(nMarkers, false);
  
      for(unsigned int i = 0; i < nMarkers; i++) {
          vector< Point2f > markerObjPoints2D;
          int markerId = _markerIds.getMat().at< int >(i);
          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);
          markerObjPoints2D.resize(4);
          for(unsigned int j = 0; j < 4; j++)
              markerObjPoints2D[j] =
                  Point2f(_board->objPoints[boardIdx][j].x, _board->objPoints[boardIdx][j].y);
  
          transformations[i] = getPerspectiveTransform(markerObjPoints2D, _markerCorners.getMat(i));
  
          // set transform as valid if transformation is non-singular
          double det = determinant(transformations[i]);
          validTransform[i] = std::abs(det) > 1e-6;
      }
  
      unsigned int nCharucoCorners = (unsigned int)_board->chessboardCorners.size();
      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++) {
          Point2f objPoint2D = Point2f(_board->chessboardCorners[i].x, _board->chessboardCorners[i].y);
  
          vector< Point2f > interpolatedPositions;
          for(unsigned int j = 0; j < _board->nearestMarkerIdx[i].size(); j++) {
              int markerId = _board->ids[_board->nearestMarkerIdx[i][j]];
              int markerIdx = -1;
              for(unsigned int k = 0; k < _markerIds.getMat().total(); k++) {
                  if(_markerIds.getMat().at< int >(k) == markerId) {
                      markerIdx = k;
                      break;
                  }
              }
              if (markerIdx != -1 &&
                  validTransform[markerIdx])
              {
                  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;
      _getMaximumSubPixWindowSizes(_markerCorners, _markerIds, allChessboardImgPoints, _board,
                                   subPixWinSizes);
  
  
      // filter corners outside the image and subpixel-refine charuco corners
      return _selectAndRefineChessboardCorners(allChessboardImgPoints, _image, _charucoCorners,
                                               _charucoIds, subPixWinSizes);
  }
  
  
  
  /**
    */
  int interpolateCornersCharuco(InputArrayOfArrays _markerCorners, InputArray _markerIds,
                                InputArray _image, const Ptr<CharucoBoard> &_board,
                                OutputArray _charucoCorners, OutputArray _charucoIds,
                                InputArray _cameraMatrix, InputArray _distCoeffs, int minMarkers) {
  
      // if camera parameters are avaible, use approximated calibration
      if(_cameraMatrix.total() != 0) {
          _interpolateCornersCharucoApproxCalib(_markerCorners, _markerIds, _image, _board,
                                                       _cameraMatrix, _distCoeffs, _charucoCorners,
                                                       _charucoIds);
      }
      // else use local homography
      else {
          _interpolateCornersCharucoLocalHom(_markerCorners, _markerIds, _image, _board,
                                                    _charucoCorners, _charucoIds);
      }
  
      // to return a charuco corner, its closest aruco markers should have been detected
      return _filterCornersWithoutMinMarkers(_board, _charucoCorners, _charucoIds, _markerIds,
                                             minMarkers, _charucoCorners, _charucoIds);
  }
  
  
  
  /**
    */
  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++) {
          Point2f corner = _charucoCorners.getMat().at< Point2f >(i);
  
          // draw first corner mark
          rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 1, LINE_AA);
  
          // draw ID
          if(_charucoIds.total() != 0) {
              int id = _charucoIds.getMat().at< int >(i);
              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 parallel 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,
                                const Ptr<CharucoBoard> &_board, InputArray _cameraMatrix, InputArray _distCoeffs,
                                InputOutputArray _rvec, InputOutputArray _tvec, bool useExtrinsicGuess) {
  
      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++) {
          int currId = _charucoIds.getMat().at< int >(i);
          CV_Assert(currId >= 0 && currId < (int)_board->chessboardCorners.size());
          objPoints.push_back(_board->chessboardCorners[currId]);
      }
  
      // points need to be in different lines, check if detected points are enough
      if(!_arePointsEnoughForPoseEstimation(objPoints)) return false;
  
      solvePnP(objPoints, _charucoCorners, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
  
      return true;
  }
  
  
  
  
  /**
    */
  double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
                                const Ptr<CharucoBoard> &_board, Size imageSize,
                                InputOutputArray _cameraMatrix, InputOutputArray _distCoeffs,
                                OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
                                OutputArray _stdDeviationsIntrinsics,
                                OutputArray _stdDeviationsExtrinsics,
                                OutputArray _perViewErrors,
                                int flags, TermCriteria criteria) {
  
      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++) {
              int pointId = _charucoIds.getMat(i).at< int >(j);
              CV_Assert(pointId >= 0 && pointId < (int)_board->chessboardCorners.size());
              allObjPoints[i].push_back(_board->chessboardCorners[pointId]);
          }
      }
  
      return calibrateCamera(allObjPoints, _charucoCorners, imageSize, _cameraMatrix, _distCoeffs,
                             _rvecs, _tvecs, _stdDeviationsIntrinsics, _stdDeviationsExtrinsics,
                             _perViewErrors, flags, criteria);
  }
  
  
  
  /**
   */
  double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfArrays _charucoIds,
    const Ptr<CharucoBoard> &_board, Size imageSize,
    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);
  }
  
  
  /**
   */
  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());
  
      const float minRepDistanceRate = 1.302455f;
  
      // create Charuco board layout for diamond (3x3 layout)
      Ptr<Dictionary> dict = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0));
      Ptr<CharucoBoard> _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dict);
  
  
      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;
      if(_image.type() == CV_8UC3)
          cvtColor(_image, grey, COLOR_BGR2GRAY);
      else
          _image.copyTo(grey);
  
      // 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++) {
            Point2f edge = corners.at< Point2f >(c) - corners.at< Point2f >((c + 1) % 4);
            perimeterSq += edge.x*edge.x + edge.y*edge.y;
          }
          // maximum reprojection error relative to perimeter
          float minRepDistance = sqrt(perimeterSq) * minRepDistanceRate;
  
          int currentId = _markerIds.getMat().at< int >(i);
  
          // 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++)
              _charucoDiamondLayout->ids[k] = currentId + 1 + k;
          // current id is assigned to [0], so it is the marker on the top
          _charucoDiamondLayout->ids[0] = currentId;
  
          // try to find the rest of markers in the diamond
          vector< int > acceptedIdxs;
          Ptr<Board> _b = _charucoDiamondLayout.staticCast<Board>();
          aruco::refineDetectedMarkers(grey, _b,
                                       currentMarker, currentMarkerId,
                                       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]];
                  markerId[k] = _markerIds.getMat().at< int >(currentMarkerIdx);
                  assigned[currentMarkerIdx] = true;
              }
  
              // interpolate the charuco corners of the diamond
              vector< Point2f > currentMarkerCorners;
              Mat aux;
              interpolateCornersCharuco(currentMarker, currentMarkerId, grey, _charucoDiamondLayout,
                                        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
          Mat(diamondIds).copyTo(_diamondIds);
  
          _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++) {
                  _diamondCorners.getMat(i).at< Point2f >(j) = diamondCorners[i][j];
              }
          }
      }
  }
  
  
  
  
  /**
    */
  void drawCharucoDiamond(const Ptr<Dictionary> &dictionary, Vec4i ids, int squareLength, int markerLength,
                          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
      Ptr<CharucoBoard> board =
          CharucoBoard::create(3, 3, (float)squareLength, (float)markerLength, dictionary);
  
      // assign the charuco marker ids
      for(int i = 0; i < 4; i++)
          board->ids[i] = ids[i];
  
      Size outSize(3 * squareLength + 2 * marginSize, 3 * squareLength + 2 * marginSize);
      board->draw(outSize, _img, marginSize, borderBits);
  }
  
  
  /**
   */
  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;
              p0 = currentMarker.at< Point2f >(j);
              p1 = currentMarker.at< Point2f >((j + 1) % 4);
              line(_image, p0, p1, borderColor, 1);
          }
  
          // draw first corner mark
          rectangle(_image, currentMarker.at< Point2f >(0) - Point2f(3, 3),
                    currentMarker.at< Point2f >(0) + Point2f(3, 3), cornerColor, 1, LINE_AA);
  
          // draw id composed by four numbers
          if(_ids.total() != 0) {
              Point2f cent(0, 0);
              for(int p = 0; p < 4; p++)
                  cent += currentMarker.at< Point2f >(p);
              cent = cent / 4.;
              stringstream s;
              s << "id=" << _ids.getMat().at< Vec4i >(i);
              putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2);
          }
      }
  }
  
  /**
     @param board layout of ChArUco board.
   * @param image charucoIds list of identifiers for each corner in charucoCorners.
   * @return bool value, 1 (true) for detected corners form a line, 0 for non-linear.
        solvePnP will fail if the corners are collinear (true).
    * Check that the set of charuco markers in _charucoIds does not identify a straight line on
      the charuco board.  Axis parallel, as well as diagonal and other straight lines detected.
    */
   bool testCharucoCornersCollinear(const Ptr<CharucoBoard> &_board, InputArray _charucoIds){
  
      unsigned int nCharucoCorners = (unsigned int)_charucoIds.getMat().total();
  
      if (nCharucoCorners <= 2)
          return true;
  
      // only test if there are 3 or more corners
      CV_Assert( _board->chessboardCorners.size() >= _charucoIds.getMat().total());
  
      Vec<double, 3> point0( _board->chessboardCorners[_charucoIds.getMat().at< int >(0)].x,
              _board->chessboardCorners[_charucoIds.getMat().at< int >(0)].y,
              1);
  
      Vec<double, 3> point1( _board->chessboardCorners[_charucoIds.getMat().at< int >(1)].x,
              _board->chessboardCorners[_charucoIds.getMat().at< int >(1)].y,
              1);
  
      // create a line from the first two points.
      Vec<double, 3> testLine = point0.cross(point1);
  
      Vec<double, 3> testPoint(0, 0, 1);
  
      double divisor = sqrt(testLine[0]*testLine[0] + testLine[1]*testLine[1]);
  
      CV_Assert( divisor != 0);
  
      // normalize the line with normal
      testLine /= divisor;
  
      double dotProduct;
      for (unsigned int i = 2; i < nCharucoCorners; i++){
          testPoint(0) = _board->chessboardCorners[_charucoIds.getMat().at< int >(i)].x;
          testPoint(1) = _board->chessboardCorners[_charucoIds.getMat().at< int >(i)].y;
  
          // if testPoint is on testLine, dotProduct will be zero (or very, very close)
          dotProduct = testPoint.dot(testLine);
  
          if (std::abs(dotProduct) > 1e-6){
              return false;
          }
      }
  
      // no points found that were off of testLine, return true that all points collinear.
      return true;
  }
  }
  }
  