/*
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.area() > 0);
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) < 0.0001) {
// 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();
}
/**
* 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,
vector< Size > *_filteredWinSizes, const Ptr<DetectorParameters> &_params)
: grey(_grey), filteredChessboardImgPoints(_filteredChessboardImgPoints),
filteredWinSizes(_filteredWinSizes), params(_params) {}
void operator()(const Range &range) const CV_OVERRIDE {
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;
const Ptr<DetectorParameters> ¶ms;
};
/**
* @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.getMat().type() == CV_8UC3)
cvtColor(_image.getMat(), grey, COLOR_BGR2GRAY);
else
_image.getMat().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
// 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()),
CharucoSubpixelParallel(&grey, &filteredChessboardImgPoints, &filteredWinSizes, params));
// 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);
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));
}
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) {
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 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,
const Ptr<CharucoBoard> &_board, InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _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.getMat().type() == CV_8UC3)
cvtColor(_image.getMat(), grey, COLOR_BGR2GRAY);
else
_image.getMat().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);
}
}
}
}
}