mirror of
https://github.com/CopterExpress/clover.git
synced 2026-06-03 08:29:32 +00:00
935 lines
38 KiB
C++
935 lines
38 KiB
C++
/*
|
|
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) {
|
|
|
|
/* NOTE: OpenCV v<3.3 does not have .empty() as a Size member */
|
|
CV_Assert(outSize.height > 0 && outSize.width > 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) < 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();
|
|
}
|
|
|
|
|
|
/**
|
|
* 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.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
|
|
// 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.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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|