mirror of
https://github.com/opencv/opencv.git
synced 2025-06-08 01:53:19 +08:00
1372 lines
60 KiB
C++
1372 lines
60 KiB
C++
// This file is part of OpenCV project.
|
|
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
|
// of this distribution and at http://opencv.org/license.html
|
|
|
|
#include "../precomp.hpp"
|
|
#include <opencv2/calib3d.hpp>
|
|
|
|
#include "opencv2/objdetect/aruco_detector.hpp"
|
|
#include "opencv2/objdetect/aruco_board.hpp"
|
|
#include "apriltag/apriltag_quad_thresh.hpp"
|
|
#include "aruco_utils.hpp"
|
|
#include <cmath>
|
|
|
|
namespace cv {
|
|
namespace aruco {
|
|
|
|
using namespace std;
|
|
|
|
static inline bool readWrite(DetectorParameters ¶ms, const FileNode* readNode,
|
|
FileStorage* writeStorage = nullptr)
|
|
{
|
|
CV_Assert(readNode || writeStorage);
|
|
bool check = false;
|
|
|
|
check |= readWriteParameter("adaptiveThreshWinSizeMin", params.adaptiveThreshWinSizeMin, readNode, writeStorage);
|
|
check |= readWriteParameter("adaptiveThreshWinSizeMax", params.adaptiveThreshWinSizeMax, readNode, writeStorage);
|
|
check |= readWriteParameter("adaptiveThreshWinSizeStep", params.adaptiveThreshWinSizeStep, readNode, writeStorage);
|
|
check |= readWriteParameter("adaptiveThreshConstant", params.adaptiveThreshConstant, readNode, writeStorage);
|
|
check |= readWriteParameter("minMarkerPerimeterRate", params.minMarkerPerimeterRate, readNode, writeStorage);
|
|
check |= readWriteParameter("maxMarkerPerimeterRate", params.maxMarkerPerimeterRate, readNode, writeStorage);
|
|
check |= readWriteParameter("polygonalApproxAccuracyRate", params.polygonalApproxAccuracyRate,
|
|
readNode, writeStorage);
|
|
check |= readWriteParameter("minCornerDistanceRate", params.minCornerDistanceRate, readNode, writeStorage);
|
|
check |= readWriteParameter("minDistanceToBorder", params.minDistanceToBorder, readNode, writeStorage);
|
|
check |= readWriteParameter("minMarkerDistanceRate", params.minMarkerDistanceRate, readNode, writeStorage);
|
|
check |= readWriteParameter("cornerRefinementMethod", params.cornerRefinementMethod, readNode, writeStorage);
|
|
check |= readWriteParameter("cornerRefinementWinSize", params.cornerRefinementWinSize, readNode, writeStorage);
|
|
check |= readWriteParameter("relativeCornerRefinmentWinSize", params.relativeCornerRefinmentWinSize, readNode,
|
|
writeStorage);
|
|
check |= readWriteParameter("cornerRefinementMaxIterations", params.cornerRefinementMaxIterations,
|
|
readNode, writeStorage);
|
|
check |= readWriteParameter("cornerRefinementMinAccuracy", params.cornerRefinementMinAccuracy,
|
|
readNode, writeStorage);
|
|
check |= readWriteParameter("markerBorderBits", params.markerBorderBits, readNode, writeStorage);
|
|
check |= readWriteParameter("perspectiveRemovePixelPerCell", params.perspectiveRemovePixelPerCell,
|
|
readNode, writeStorage);
|
|
check |= readWriteParameter("perspectiveRemoveIgnoredMarginPerCell", params.perspectiveRemoveIgnoredMarginPerCell,
|
|
readNode, writeStorage);
|
|
check |= readWriteParameter("maxErroneousBitsInBorderRate", params.maxErroneousBitsInBorderRate,
|
|
readNode, writeStorage);
|
|
check |= readWriteParameter("minOtsuStdDev", params.minOtsuStdDev, readNode, writeStorage);
|
|
check |= readWriteParameter("errorCorrectionRate", params.errorCorrectionRate, readNode, writeStorage);
|
|
check |= readWriteParameter("minGroupDistance", params.minGroupDistance, readNode, writeStorage);
|
|
// new aruco 3 functionality
|
|
check |= readWriteParameter("useAruco3Detection", params.useAruco3Detection, readNode, writeStorage);
|
|
check |= readWriteParameter("minSideLengthCanonicalImg", params.minSideLengthCanonicalImg, readNode, writeStorage);
|
|
check |= readWriteParameter("minMarkerLengthRatioOriginalImg", params.minMarkerLengthRatioOriginalImg,
|
|
readNode, writeStorage);
|
|
return check;
|
|
}
|
|
|
|
bool DetectorParameters::readDetectorParameters(const FileNode& fn)
|
|
{
|
|
if (fn.empty())
|
|
return false;
|
|
return readWrite(*this, &fn);
|
|
}
|
|
|
|
bool DetectorParameters::writeDetectorParameters(FileStorage& fs, const String& name)
|
|
{
|
|
CV_Assert(fs.isOpened());
|
|
if (!name.empty())
|
|
fs << name << "{";
|
|
bool res = readWrite(*this, nullptr, &fs);
|
|
if (!name.empty())
|
|
fs << "}";
|
|
return res;
|
|
}
|
|
|
|
static inline bool readWrite(RefineParameters& refineParameters, const FileNode* readNode,
|
|
FileStorage* writeStorage = nullptr)
|
|
{
|
|
CV_Assert(readNode || writeStorage);
|
|
bool check = false;
|
|
|
|
check |= readWriteParameter("minRepDistance", refineParameters.minRepDistance, readNode, writeStorage);
|
|
check |= readWriteParameter("errorCorrectionRate", refineParameters.errorCorrectionRate, readNode, writeStorage);
|
|
check |= readWriteParameter("checkAllOrders", refineParameters.checkAllOrders, readNode, writeStorage);
|
|
return check;
|
|
}
|
|
|
|
RefineParameters::RefineParameters(float _minRepDistance, float _errorCorrectionRate, bool _checkAllOrders):
|
|
minRepDistance(_minRepDistance), errorCorrectionRate(_errorCorrectionRate),
|
|
checkAllOrders(_checkAllOrders){}
|
|
|
|
bool RefineParameters::readRefineParameters(const FileNode &fn)
|
|
{
|
|
if (fn.empty())
|
|
return false;
|
|
return readWrite(*this, &fn);
|
|
}
|
|
|
|
bool RefineParameters::writeRefineParameters(FileStorage& fs, const String& name)
|
|
{
|
|
CV_Assert(fs.isOpened());
|
|
if (!name.empty())
|
|
fs << name << "{";
|
|
bool res = readWrite(*this, nullptr, &fs);
|
|
if (!name.empty())
|
|
fs << "}";
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* @brief Threshold input image using adaptive thresholding
|
|
*/
|
|
static void _threshold(InputArray _in, OutputArray _out, int winSize, double constant) {
|
|
|
|
CV_Assert(winSize >= 3);
|
|
if(winSize % 2 == 0) winSize++; // win size must be odd
|
|
adaptiveThreshold(_in, _out, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY_INV, winSize, constant);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Given a tresholded image, find the contours, calculate their polygonal approximation
|
|
* and take those that accomplish some conditions
|
|
*/
|
|
static void _findMarkerContours(const Mat &in, vector<vector<Point2f> > &candidates,
|
|
vector<vector<Point> > &contoursOut, double minPerimeterRate,
|
|
double maxPerimeterRate, double accuracyRate,
|
|
double minCornerDistanceRate, int minDistanceToBorder, int minSize) {
|
|
|
|
CV_Assert(minPerimeterRate > 0 && maxPerimeterRate > 0 && accuracyRate > 0 &&
|
|
minCornerDistanceRate >= 0 && minDistanceToBorder >= 0);
|
|
|
|
// calculate maximum and minimum sizes in pixels
|
|
unsigned int minPerimeterPixels =
|
|
(unsigned int)(minPerimeterRate * max(in.cols, in.rows));
|
|
unsigned int maxPerimeterPixels =
|
|
(unsigned int)(maxPerimeterRate * max(in.cols, in.rows));
|
|
|
|
// for aruco3 functionality
|
|
if (minSize != 0) {
|
|
minPerimeterPixels = 4*minSize;
|
|
}
|
|
|
|
Mat contoursImg;
|
|
in.copyTo(contoursImg);
|
|
vector<vector<Point> > contours;
|
|
findContours(contoursImg, contours, RETR_LIST, CHAIN_APPROX_NONE);
|
|
// now filter list of contours
|
|
for(unsigned int i = 0; i < contours.size(); i++) {
|
|
// check perimeter
|
|
if(contours[i].size() < minPerimeterPixels || contours[i].size() > maxPerimeterPixels)
|
|
continue;
|
|
|
|
// check is square and is convex
|
|
vector<Point> approxCurve;
|
|
approxPolyDP(contours[i], approxCurve, double(contours[i].size()) * accuracyRate, true);
|
|
if(approxCurve.size() != 4 || !isContourConvex(approxCurve)) continue;
|
|
|
|
// check min distance between corners
|
|
double minDistSq =
|
|
max(contoursImg.cols, contoursImg.rows) * max(contoursImg.cols, contoursImg.rows);
|
|
for(int j = 0; j < 4; j++) {
|
|
double d = (double)(approxCurve[j].x - approxCurve[(j + 1) % 4].x) *
|
|
(double)(approxCurve[j].x - approxCurve[(j + 1) % 4].x) +
|
|
(double)(approxCurve[j].y - approxCurve[(j + 1) % 4].y) *
|
|
(double)(approxCurve[j].y - approxCurve[(j + 1) % 4].y);
|
|
minDistSq = min(minDistSq, d);
|
|
}
|
|
double minCornerDistancePixels = double(contours[i].size()) * minCornerDistanceRate;
|
|
if(minDistSq < minCornerDistancePixels * minCornerDistancePixels) continue;
|
|
|
|
// check if it is too near to the image border
|
|
bool tooNearBorder = false;
|
|
for(int j = 0; j < 4; j++) {
|
|
if(approxCurve[j].x < minDistanceToBorder || approxCurve[j].y < minDistanceToBorder ||
|
|
approxCurve[j].x > contoursImg.cols - 1 - minDistanceToBorder ||
|
|
approxCurve[j].y > contoursImg.rows - 1 - minDistanceToBorder)
|
|
tooNearBorder = true;
|
|
}
|
|
if(tooNearBorder) continue;
|
|
|
|
// if it passes all the test, add to candidates vector
|
|
vector<Point2f> currentCandidate;
|
|
currentCandidate.resize(4);
|
|
for(int j = 0; j < 4; j++) {
|
|
currentCandidate[j] = Point2f((float)approxCurve[j].x, (float)approxCurve[j].y);
|
|
}
|
|
candidates.push_back(currentCandidate);
|
|
contoursOut.push_back(contours[i]);
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Assure order of candidate corners is clockwise direction
|
|
*/
|
|
static void _reorderCandidatesCorners(vector<vector<Point2f> > &candidates) {
|
|
|
|
for(unsigned int i = 0; i < candidates.size(); i++) {
|
|
double dx1 = candidates[i][1].x - candidates[i][0].x;
|
|
double dy1 = candidates[i][1].y - candidates[i][0].y;
|
|
double dx2 = candidates[i][2].x - candidates[i][0].x;
|
|
double dy2 = candidates[i][2].y - candidates[i][0].y;
|
|
double crossProduct = (dx1 * dy2) - (dy1 * dx2);
|
|
|
|
if(crossProduct < 0.0) { // not clockwise direction
|
|
swap(candidates[i][1], candidates[i][3]);
|
|
}
|
|
}
|
|
}
|
|
|
|
static float getAverageModuleSize(const vector<Point2f>& markerCorners, int markerSize, int markerBorderBits) {
|
|
float averageArucoModuleSize = 0.f;
|
|
for (size_t i = 0ull; i < 4ull; i++) {
|
|
averageArucoModuleSize += sqrt(normL2Sqr<float>(Point2f(markerCorners[i] - markerCorners[(i+1ull) % 4ull])));
|
|
}
|
|
int numModules = markerSize + markerBorderBits * 2;
|
|
averageArucoModuleSize /= ((float)markerCorners.size()*numModules);
|
|
return averageArucoModuleSize;
|
|
}
|
|
|
|
static bool checkMarker1InMarker2(const vector<Point2f>& marker1, const vector<Point2f>& marker2) {
|
|
return pointPolygonTest(marker2, marker1[0], false) >= 0 && pointPolygonTest(marker2, marker1[1], false) >= 0 &&
|
|
pointPolygonTest(marker2, marker1[2], false) >= 0 && pointPolygonTest(marker2, marker1[3], false) >= 0;
|
|
}
|
|
|
|
struct MarkerCandidate {
|
|
vector<Point2f> corners;
|
|
vector<Point> contour;
|
|
float perimeter = 0.f;
|
|
};
|
|
|
|
struct MarkerCandidateTree : MarkerCandidate{
|
|
int parent = -1;
|
|
int depth = 0;
|
|
vector<MarkerCandidate> closeContours;
|
|
|
|
MarkerCandidateTree() {}
|
|
|
|
MarkerCandidateTree(vector<Point2f>&& corners_, vector<Point>&& contour_) {
|
|
corners = std::move(corners_);
|
|
contour = std::move(contour_);
|
|
perimeter = 0.f;
|
|
for (size_t i = 0ull; i < 4ull; i++) {
|
|
perimeter += sqrt(normL2Sqr<float>(Point2f(corners[i] - corners[(i+1ull) % 4ull])));
|
|
}
|
|
}
|
|
|
|
bool operator<(const MarkerCandidateTree& m) const {
|
|
// sorting the contors in descending order
|
|
return perimeter > m.perimeter;
|
|
}
|
|
};
|
|
|
|
|
|
// returns the average distance between the marker points
|
|
float static inline getAverageDistance(const std::vector<Point2f>& marker1, const std::vector<Point2f>& marker2) {
|
|
float minDistSq = std::numeric_limits<float>::max();
|
|
// fc is the first corner considered on one of the markers, 4 combinations are possible
|
|
for(int fc = 0; fc < 4; fc++) {
|
|
float distSq = 0;
|
|
for(int c = 0; c < 4; c++) {
|
|
// modC is the corner considering first corner is fc
|
|
int modC = (c + fc) % 4;
|
|
distSq += normL2Sqr<float>(marker1[modC] - marker2[c]);
|
|
}
|
|
distSq /= 4.f;
|
|
minDistSq = min(minDistSq, distSq);
|
|
}
|
|
return sqrt(minDistSq);
|
|
}
|
|
|
|
/**
|
|
* @brief Initial steps on finding square candidates
|
|
*/
|
|
static void _detectInitialCandidates(const Mat &grey, vector<vector<Point2f> > &candidates,
|
|
vector<vector<Point> > &contours,
|
|
const DetectorParameters ¶ms) {
|
|
|
|
CV_Assert(params.adaptiveThreshWinSizeMin >= 3 && params.adaptiveThreshWinSizeMax >= 3);
|
|
CV_Assert(params.adaptiveThreshWinSizeMax >= params.adaptiveThreshWinSizeMin);
|
|
CV_Assert(params.adaptiveThreshWinSizeStep > 0);
|
|
|
|
// number of window sizes (scales) to apply adaptive thresholding
|
|
int nScales = (params.adaptiveThreshWinSizeMax - params.adaptiveThreshWinSizeMin) /
|
|
params.adaptiveThreshWinSizeStep + 1;
|
|
|
|
vector<vector<vector<Point2f> > > candidatesArrays((size_t) nScales);
|
|
vector<vector<vector<Point> > > contoursArrays((size_t) nScales);
|
|
|
|
////for each value in the interval of thresholding window sizes
|
|
parallel_for_(Range(0, nScales), [&](const Range& range) {
|
|
const int begin = range.start;
|
|
const int end = range.end;
|
|
|
|
for (int i = begin; i < end; i++) {
|
|
int currScale = params.adaptiveThreshWinSizeMin + i * params.adaptiveThreshWinSizeStep;
|
|
// threshold
|
|
Mat thresh;
|
|
_threshold(grey, thresh, currScale, params.adaptiveThreshConstant);
|
|
|
|
// detect rectangles
|
|
_findMarkerContours(thresh, candidatesArrays[i], contoursArrays[i],
|
|
params.minMarkerPerimeterRate, params.maxMarkerPerimeterRate,
|
|
params.polygonalApproxAccuracyRate, params.minCornerDistanceRate,
|
|
params.minDistanceToBorder, params.minSideLengthCanonicalImg);
|
|
}
|
|
});
|
|
// join candidates
|
|
for(int i = 0; i < nScales; i++) {
|
|
for(unsigned int j = 0; j < candidatesArrays[i].size(); j++) {
|
|
candidates.push_back(candidatesArrays[i][j]);
|
|
contours.push_back(contoursArrays[i][j]);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Given an input image and candidate corners, extract the bits of the candidate, including
|
|
* the border bits
|
|
*/
|
|
static Mat _extractBits(InputArray _image, const vector<Point2f>& corners, int markerSize,
|
|
int markerBorderBits, int cellSize, double cellMarginRate, double minStdDevOtsu) {
|
|
CV_Assert(_image.getMat().channels() == 1);
|
|
CV_Assert(corners.size() == 4ull);
|
|
CV_Assert(markerBorderBits > 0 && cellSize > 0 && cellMarginRate >= 0 && cellMarginRate <= 1);
|
|
CV_Assert(minStdDevOtsu >= 0);
|
|
|
|
// number of bits in the marker
|
|
int markerSizeWithBorders = markerSize + 2 * markerBorderBits;
|
|
int cellMarginPixels = int(cellMarginRate * cellSize);
|
|
|
|
Mat resultImg; // marker image after removing perspective
|
|
int resultImgSize = markerSizeWithBorders * cellSize;
|
|
Mat resultImgCorners(4, 1, CV_32FC2);
|
|
resultImgCorners.ptr<Point2f>(0)[0] = Point2f(0, 0);
|
|
resultImgCorners.ptr<Point2f>(0)[1] = Point2f((float)resultImgSize - 1, 0);
|
|
resultImgCorners.ptr<Point2f>(0)[2] =
|
|
Point2f((float)resultImgSize - 1, (float)resultImgSize - 1);
|
|
resultImgCorners.ptr<Point2f>(0)[3] = Point2f(0, (float)resultImgSize - 1);
|
|
|
|
// remove perspective
|
|
Mat transformation = getPerspectiveTransform(corners, resultImgCorners);
|
|
warpPerspective(_image, resultImg, transformation, Size(resultImgSize, resultImgSize),
|
|
INTER_NEAREST);
|
|
|
|
// output image containing the bits
|
|
Mat bits(markerSizeWithBorders, markerSizeWithBorders, CV_8UC1, Scalar::all(0));
|
|
|
|
// check if standard deviation is enough to apply Otsu
|
|
// if not enough, it probably means all bits are the same color (black or white)
|
|
Mat mean, stddev;
|
|
// Remove some border just to avoid border noise from perspective transformation
|
|
Mat innerRegion = resultImg.colRange(cellSize / 2, resultImg.cols - cellSize / 2)
|
|
.rowRange(cellSize / 2, resultImg.rows - cellSize / 2);
|
|
meanStdDev(innerRegion, mean, stddev);
|
|
if(stddev.ptr< double >(0)[0] < minStdDevOtsu) {
|
|
// all black or all white, depending on mean value
|
|
if(mean.ptr< double >(0)[0] > 127)
|
|
bits.setTo(1);
|
|
else
|
|
bits.setTo(0);
|
|
return bits;
|
|
}
|
|
|
|
// now extract code, first threshold using Otsu
|
|
threshold(resultImg, resultImg, 125, 255, THRESH_BINARY | THRESH_OTSU);
|
|
|
|
// for each cell
|
|
for(int y = 0; y < markerSizeWithBorders; y++) {
|
|
for(int x = 0; x < markerSizeWithBorders; x++) {
|
|
int Xstart = x * (cellSize) + cellMarginPixels;
|
|
int Ystart = y * (cellSize) + cellMarginPixels;
|
|
Mat square = resultImg(Rect(Xstart, Ystart, cellSize - 2 * cellMarginPixels,
|
|
cellSize - 2 * cellMarginPixels));
|
|
// count white pixels on each cell to assign its value
|
|
size_t nZ = (size_t) countNonZero(square);
|
|
if(nZ > square.total() / 2) bits.at<unsigned char>(y, x) = 1;
|
|
}
|
|
}
|
|
|
|
return bits;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* @brief Return number of erroneous bits in border, i.e. number of white bits in border.
|
|
*/
|
|
static int _getBorderErrors(const Mat &bits, int markerSize, int borderSize) {
|
|
|
|
int sizeWithBorders = markerSize + 2 * borderSize;
|
|
|
|
CV_Assert(markerSize > 0 && bits.cols == sizeWithBorders && bits.rows == sizeWithBorders);
|
|
|
|
int totalErrors = 0;
|
|
for(int y = 0; y < sizeWithBorders; y++) {
|
|
for(int k = 0; k < borderSize; k++) {
|
|
if(bits.ptr<unsigned char>(y)[k] != 0) totalErrors++;
|
|
if(bits.ptr<unsigned char>(y)[sizeWithBorders - 1 - k] != 0) totalErrors++;
|
|
}
|
|
}
|
|
for(int x = borderSize; x < sizeWithBorders - borderSize; x++) {
|
|
for(int k = 0; k < borderSize; k++) {
|
|
if(bits.ptr<unsigned char>(k)[x] != 0) totalErrors++;
|
|
if(bits.ptr<unsigned char>(sizeWithBorders - 1 - k)[x] != 0) totalErrors++;
|
|
}
|
|
}
|
|
return totalErrors;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Tries to identify one candidate given the dictionary
|
|
* @return candidate typ. zero if the candidate is not valid,
|
|
* 1 if the candidate is a black candidate (default candidate)
|
|
* 2 if the candidate is a white candidate
|
|
*/
|
|
static uint8_t _identifyOneCandidate(const Dictionary& dictionary, const Mat& _image,
|
|
const vector<Point2f>& _corners, int& idx,
|
|
const DetectorParameters& params, int& rotation,
|
|
const float scale = 1.f) {
|
|
CV_DbgAssert(params.markerBorderBits > 0);
|
|
uint8_t typ=1;
|
|
// get bits
|
|
// scale corners to the correct size to search on the corresponding image pyramid
|
|
vector<Point2f> scaled_corners(4);
|
|
for (int i = 0; i < 4; ++i) {
|
|
scaled_corners[i].x = _corners[i].x * scale;
|
|
scaled_corners[i].y = _corners[i].y * scale;
|
|
}
|
|
|
|
Mat candidateBits =
|
|
_extractBits(_image, scaled_corners, dictionary.markerSize, params.markerBorderBits,
|
|
params.perspectiveRemovePixelPerCell,
|
|
params.perspectiveRemoveIgnoredMarginPerCell, params.minOtsuStdDev);
|
|
|
|
// analyze border bits
|
|
int maximumErrorsInBorder =
|
|
int(dictionary.markerSize * dictionary.markerSize * params.maxErroneousBitsInBorderRate);
|
|
int borderErrors =
|
|
_getBorderErrors(candidateBits, dictionary.markerSize, params.markerBorderBits);
|
|
|
|
// check if it is a white marker
|
|
if(params.detectInvertedMarker){
|
|
// to get from 255 to 1
|
|
Mat invertedImg = ~candidateBits-254;
|
|
int invBError = _getBorderErrors(invertedImg, dictionary.markerSize, params.markerBorderBits);
|
|
// white marker
|
|
if(invBError<borderErrors){
|
|
borderErrors = invBError;
|
|
invertedImg.copyTo(candidateBits);
|
|
typ=2;
|
|
}
|
|
}
|
|
if(borderErrors > maximumErrorsInBorder) return 0; // border is wrong
|
|
|
|
// take only inner bits
|
|
Mat onlyBits =
|
|
candidateBits.rowRange(params.markerBorderBits,
|
|
candidateBits.rows - params.markerBorderBits)
|
|
.colRange(params.markerBorderBits, candidateBits.cols - params.markerBorderBits);
|
|
|
|
// try to indentify the marker
|
|
if(!dictionary.identify(onlyBits, idx, rotation, params.errorCorrectionRate))
|
|
return 0;
|
|
|
|
return typ;
|
|
}
|
|
|
|
/**
|
|
* @brief rotate the initial corner to get to the right position
|
|
*/
|
|
static void correctCornerPosition(vector<Point2f>& _candidate, int rotate){
|
|
std::rotate(_candidate.begin(), _candidate.begin() + 4 - rotate, _candidate.end());
|
|
}
|
|
|
|
static size_t _findOptPyrImageForCanonicalImg(
|
|
const vector<Mat>& img_pyr,
|
|
const int scaled_width,
|
|
const int cur_perimeter,
|
|
const int min_perimeter) {
|
|
CV_Assert(scaled_width > 0);
|
|
size_t optLevel = 0;
|
|
float dist = std::numeric_limits<float>::max();
|
|
for (size_t i = 0; i < img_pyr.size(); ++i) {
|
|
const float scale = img_pyr[i].cols / static_cast<float>(scaled_width);
|
|
const float perimeter_scaled = cur_perimeter * scale;
|
|
// instead of std::abs() favor the larger pyramid level by checking if the distance is postive
|
|
// will slow down the algorithm but find more corners in the end
|
|
const float new_dist = perimeter_scaled - min_perimeter;
|
|
if (new_dist < dist && new_dist > 0.f) {
|
|
dist = new_dist;
|
|
optLevel = i;
|
|
}
|
|
}
|
|
return optLevel;
|
|
}
|
|
|
|
|
|
/**
|
|
* Line fitting A * B = C :: Called from function refineCandidateLines
|
|
* @param nContours contour-container
|
|
*/
|
|
static Point3f _interpolate2Dline(const vector<Point2f>& nContours){
|
|
CV_Assert(nContours.size() >= 2);
|
|
float minX, minY, maxX, maxY;
|
|
minX = maxX = nContours[0].x;
|
|
minY = maxY = nContours[0].y;
|
|
|
|
for(unsigned int i = 0; i< nContours.size(); i++){
|
|
minX = nContours[i].x < minX ? nContours[i].x : minX;
|
|
minY = nContours[i].y < minY ? nContours[i].y : minY;
|
|
maxX = nContours[i].x > maxX ? nContours[i].x : maxX;
|
|
maxY = nContours[i].y > maxY ? nContours[i].y : maxY;
|
|
}
|
|
|
|
Mat A = Mat::ones((int)nContours.size(), 2, CV_32F); // Coefficient Matrix (N x 2)
|
|
Mat B((int)nContours.size(), 1, CV_32F); // Variables Matrix (N x 1)
|
|
Mat C; // Constant
|
|
|
|
if(maxX - minX > maxY - minY){
|
|
for(unsigned int i =0; i < nContours.size(); i++){
|
|
A.at<float>(i,0)= nContours[i].x;
|
|
B.at<float>(i,0)= nContours[i].y;
|
|
}
|
|
|
|
solve(A, B, C, DECOMP_NORMAL);
|
|
|
|
return Point3f(C.at<float>(0, 0), -1., C.at<float>(1, 0));
|
|
}
|
|
else{
|
|
for(unsigned int i =0; i < nContours.size(); i++){
|
|
A.at<float>(i,0)= nContours[i].y;
|
|
B.at<float>(i,0)= nContours[i].x;
|
|
}
|
|
|
|
solve(A, B, C, DECOMP_NORMAL);
|
|
|
|
return Point3f(-1., C.at<float>(0, 0), C.at<float>(1, 0));
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* Find the Point where the lines crosses :: Called from function refineCandidateLines
|
|
* @param nLine1
|
|
* @param nLine2
|
|
* @return Crossed Point
|
|
*/
|
|
static Point2f _getCrossPoint(Point3f nLine1, Point3f nLine2){
|
|
Matx22f A(nLine1.x, nLine1.y, nLine2.x, nLine2.y);
|
|
Vec2f B(-nLine1.z, -nLine2.z);
|
|
return Vec2f(A.solve(B).val);
|
|
}
|
|
|
|
/**
|
|
* Refine Corners using the contour vector :: Called from function detectMarkers
|
|
* @param nContours contour-container
|
|
* @param nCorners candidate Corners
|
|
*/
|
|
static void _refineCandidateLines(vector<Point>& nContours, vector<Point2f>& nCorners){
|
|
vector<Point2f> contour2f(nContours.begin(), nContours.end());
|
|
/* 5 groups :: to group the edges
|
|
* 4 - classified by its corner
|
|
* extra group - (temporary) if contours do not begin with a corner
|
|
*/
|
|
vector<Point2f> cntPts[5];
|
|
int cornerIndex[4]={-1};
|
|
int group=4;
|
|
|
|
for ( unsigned int i =0; i < nContours.size(); i++ ) {
|
|
for(unsigned int j=0; j<4; j++){
|
|
if ( nCorners[j] == contour2f[i] ){
|
|
cornerIndex[j] = i;
|
|
group=j;
|
|
}
|
|
}
|
|
cntPts[group].push_back(contour2f[i]);
|
|
}
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
CV_Assert(cornerIndex[i] != -1);
|
|
}
|
|
// saves extra group into corresponding
|
|
if( !cntPts[4].empty() ){
|
|
for( unsigned int i=0; i < cntPts[4].size() ; i++ )
|
|
cntPts[group].push_back(cntPts[4].at(i));
|
|
cntPts[4].clear();
|
|
}
|
|
|
|
//Evaluate contour direction :: using the position of the detected corners
|
|
int inc=1;
|
|
|
|
inc = ( (cornerIndex[0] > cornerIndex[1]) && (cornerIndex[3] > cornerIndex[0]) ) ? -1:inc;
|
|
inc = ( (cornerIndex[2] > cornerIndex[3]) && (cornerIndex[1] > cornerIndex[2]) ) ? -1:inc;
|
|
|
|
// calculate the line :: who passes through the grouped points
|
|
Point3f lines[4];
|
|
for(int i=0; i<4; i++){
|
|
lines[i]=_interpolate2Dline(cntPts[i]);
|
|
}
|
|
|
|
/*
|
|
* calculate the corner :: where the lines crosses to each other
|
|
* clockwise direction no clockwise direction
|
|
* 0 1
|
|
* .---. 1 .---. 2
|
|
* | | | |
|
|
* 3 .___. 0 .___.
|
|
* 2 3
|
|
*/
|
|
for(int i=0; i < 4; i++){
|
|
if(inc<0)
|
|
nCorners[i] = _getCrossPoint(lines[ i ], lines[ (i+1)%4 ]); // 01 12 23 30
|
|
else
|
|
nCorners[i] = _getCrossPoint(lines[ i ], lines[ (i+3)%4 ]); // 30 01 12 23
|
|
}
|
|
}
|
|
|
|
static inline void findCornerInPyrImage(const float scale_init, const int closest_pyr_image_idx,
|
|
const vector<Mat>& grey_pyramid, Mat corners,
|
|
const DetectorParameters& params) {
|
|
// scale them to the closest pyramid level
|
|
if (scale_init != 1.f)
|
|
corners *= scale_init; // scale_init * scale_pyr
|
|
for (int idx = closest_pyr_image_idx - 1; idx >= 0; --idx) {
|
|
// scale them to new pyramid level
|
|
corners *= 2.f; // *= scale_pyr;
|
|
// use larger win size for larger images
|
|
const int subpix_win_size = std::max(grey_pyramid[idx].cols, grey_pyramid[idx].rows) > 1080 ? 5 : 3;
|
|
cornerSubPix(grey_pyramid[idx], corners,
|
|
Size(subpix_win_size, subpix_win_size),
|
|
Size(-1, -1),
|
|
TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
|
|
params.cornerRefinementMaxIterations,
|
|
params.cornerRefinementMinAccuracy));
|
|
}
|
|
}
|
|
|
|
struct ArucoDetector::ArucoDetectorImpl {
|
|
/// dictionary indicates the type of markers that will be searched
|
|
Dictionary dictionary;
|
|
|
|
/// marker detection parameters, check DetectorParameters docs to see available settings
|
|
DetectorParameters detectorParams;
|
|
|
|
/// marker refine parameters
|
|
RefineParameters refineParams;
|
|
ArucoDetectorImpl() {}
|
|
|
|
ArucoDetectorImpl(const Dictionary &_dictionary, const DetectorParameters &_detectorParams,
|
|
const RefineParameters& _refineParams): dictionary(_dictionary),
|
|
detectorParams(_detectorParams), refineParams(_refineParams) {}
|
|
/**
|
|
* @brief Detect square candidates in the input image
|
|
*/
|
|
void detectCandidates(const Mat& grey, vector<vector<Point2f> >& candidates, vector<vector<Point> >& contours) {
|
|
/// 1. DETECT FIRST SET OF CANDIDATES
|
|
_detectInitialCandidates(grey, candidates, contours, detectorParams);
|
|
/// 2. SORT CORNERS
|
|
_reorderCandidatesCorners(candidates);
|
|
}
|
|
|
|
/**
|
|
* @brief FILTER OUT NEAR CANDIDATE PAIRS
|
|
*
|
|
* save the outter/inner border (i.e. potential candidates) to vector<MarkerCandidateTree>,
|
|
* clear candidates and contours
|
|
*/
|
|
vector<MarkerCandidateTree>
|
|
filterTooCloseCandidates(vector<vector<Point2f> > &candidates, vector<vector<Point> > &contours) {
|
|
CV_Assert(detectorParams.minMarkerDistanceRate >= 0.);
|
|
vector<MarkerCandidateTree> candidateTree(candidates.size());
|
|
for(size_t i = 0ull; i < candidates.size(); i++) {
|
|
candidateTree[i] = MarkerCandidateTree(std::move(candidates[i]), std::move(contours[i]));
|
|
}
|
|
candidates.clear();
|
|
contours.clear();
|
|
|
|
// sort candidates from big to small
|
|
std::stable_sort(candidateTree.begin(), candidateTree.end());
|
|
// group index for each candidate
|
|
vector<int> groupId(candidateTree.size(), -1);
|
|
vector<vector<size_t> > groupedCandidates;
|
|
vector<bool> isSelectedContours(candidateTree.size(), true);
|
|
|
|
size_t countSelectedContours = 0ull;
|
|
for (size_t i = 0ull; i < candidateTree.size(); i++) {
|
|
for (size_t j = i + 1ull; j < candidateTree.size(); j++) {
|
|
float minDist = getAverageDistance(candidateTree[i].corners, candidateTree[j].corners);
|
|
// if mean distance is too low, group markers
|
|
// the distance between the points of two independent markers should be more than half the side of the marker
|
|
// half the side of the marker = (perimeter / 4) * 0.5 = perimeter * 0.125
|
|
if(minDist < candidateTree[j].perimeter*(float)detectorParams.minMarkerDistanceRate) {
|
|
isSelectedContours[i] = false;
|
|
isSelectedContours[j] = false;
|
|
// i and j are not related to a group
|
|
if(groupId[i] < 0 && groupId[j] < 0){
|
|
// mark candidates with their corresponding group number
|
|
groupId[i] = groupId[j] = (int)groupedCandidates.size();
|
|
// create group
|
|
groupedCandidates.push_back({i, j});
|
|
}
|
|
// i is related to a group
|
|
else if(groupId[i] > -1 && groupId[j] == -1) {
|
|
int group = groupId[i];
|
|
groupId[j] = group;
|
|
// add to group
|
|
groupedCandidates[group].push_back(j);
|
|
}
|
|
// j is related to a group
|
|
else if(groupId[j] > -1 && groupId[i] == -1) {
|
|
int group = groupId[j];
|
|
groupId[i] = group;
|
|
// add to group
|
|
groupedCandidates[group].push_back(i);
|
|
}
|
|
}
|
|
}
|
|
countSelectedContours += isSelectedContours[i];
|
|
}
|
|
|
|
for (vector<size_t>& grouped : groupedCandidates) {
|
|
if (detectorParams.detectInvertedMarker) // if detectInvertedMarker choose smallest contours
|
|
std::stable_sort(grouped.begin(), grouped.end(), [](const size_t &a, const size_t &b) {
|
|
return a > b;
|
|
});
|
|
else // if detectInvertedMarker==false choose largest contours
|
|
std::stable_sort(grouped.begin(), grouped.end());
|
|
size_t currId = grouped[0];
|
|
isSelectedContours[currId] = true;
|
|
for (size_t i = 1ull; i < grouped.size(); i++) {
|
|
size_t id = grouped[i];
|
|
float dist = getAverageDistance(candidateTree[id].corners, candidateTree[currId].corners);
|
|
float moduleSize = getAverageModuleSize(candidateTree[id].corners, dictionary.markerSize, detectorParams.markerBorderBits);
|
|
if (dist > detectorParams.minGroupDistance*moduleSize) {
|
|
currId = id;
|
|
candidateTree[grouped[0]].closeContours.push_back(candidateTree[id]);
|
|
}
|
|
}
|
|
}
|
|
|
|
vector<MarkerCandidateTree> selectedCandidates(countSelectedContours + groupedCandidates.size());
|
|
countSelectedContours = 0ull;
|
|
for (size_t i = 0ull; i < candidateTree.size(); i++) {
|
|
if (isSelectedContours[i]) {
|
|
selectedCandidates[countSelectedContours] = std::move(candidateTree[i]);
|
|
countSelectedContours++;
|
|
}
|
|
}
|
|
|
|
// find hierarchy in the candidate tree
|
|
for (int i = (int)selectedCandidates.size()-1; i >= 0; i--) {
|
|
for (int j = i - 1; j >= 0; j--) {
|
|
if (checkMarker1InMarker2(selectedCandidates[i].corners, selectedCandidates[j].corners)) {
|
|
selectedCandidates[i].parent = j;
|
|
selectedCandidates[j].depth = max(selectedCandidates[j].depth, selectedCandidates[i].depth + 1);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return selectedCandidates;
|
|
}
|
|
|
|
/**
|
|
* @brief Identify square candidates according to a marker dictionary
|
|
*/
|
|
void identifyCandidates(const Mat& grey, const vector<Mat>& image_pyr, vector<MarkerCandidateTree>& selectedContours,
|
|
vector<vector<Point2f> >& accepted, vector<vector<Point> >& contours,
|
|
vector<int>& ids, OutputArrayOfArrays _rejected = noArray()) {
|
|
size_t ncandidates = selectedContours.size();
|
|
vector<vector<Point2f> > rejected;
|
|
|
|
vector<int> idsTmp(ncandidates, -1);
|
|
vector<int> rotated(ncandidates, 0);
|
|
vector<uint8_t> validCandidates(ncandidates, 0);
|
|
vector<uint8_t> was(ncandidates, false);
|
|
bool checkCloseContours = true;
|
|
|
|
int maxDepth = 0;
|
|
for (size_t i = 0ull; i < selectedContours.size(); i++)
|
|
maxDepth = max(selectedContours[i].depth, maxDepth);
|
|
vector<vector<size_t>> depths(maxDepth+1);
|
|
for (size_t i = 0ull; i < selectedContours.size(); i++) {
|
|
depths[selectedContours[i].depth].push_back(i);
|
|
}
|
|
|
|
//// Analyze each of the candidates
|
|
int depth = 0;
|
|
size_t counter = 0;
|
|
while (counter < ncandidates) {
|
|
parallel_for_(Range(0, (int)depths[depth].size()), [&](const Range& range) {
|
|
const int begin = range.start;
|
|
const int end = range.end;
|
|
for (int i = begin; i < end; i++) {
|
|
size_t v = depths[depth][i];
|
|
was[v] = true;
|
|
Mat img = grey;
|
|
// implements equation (4)
|
|
if (detectorParams.useAruco3Detection) {
|
|
const int minPerimeter = detectorParams.minSideLengthCanonicalImg * 4;
|
|
const size_t nearestImgId = _findOptPyrImageForCanonicalImg(image_pyr, grey.cols, static_cast<int>(selectedContours[v].contour.size()), minPerimeter);
|
|
img = image_pyr[nearestImgId];
|
|
}
|
|
const float scale = detectorParams.useAruco3Detection ? img.cols / static_cast<float>(grey.cols) : 1.f;
|
|
|
|
validCandidates[v] = _identifyOneCandidate(dictionary, img, selectedContours[v].corners, idsTmp[v], detectorParams, rotated[v], scale);
|
|
|
|
if (validCandidates[v] == 0 && checkCloseContours) {
|
|
for (const MarkerCandidate& closeMarkerCandidate: selectedContours[v].closeContours) {
|
|
validCandidates[v] = _identifyOneCandidate(dictionary, img, closeMarkerCandidate.corners, idsTmp[v], detectorParams, rotated[v], scale);
|
|
if (validCandidates[v] > 0) {
|
|
selectedContours[v].corners = closeMarkerCandidate.corners;
|
|
selectedContours[v].contour = closeMarkerCandidate.contour;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
});
|
|
|
|
// visit the parent vertices of the detected markers to skip identify parent contours
|
|
for(size_t v : depths[depth]) {
|
|
if(validCandidates[v] > 0) {
|
|
int parent = selectedContours[v].parent;
|
|
while (parent != -1) {
|
|
if (!was[parent]) {
|
|
was[parent] = true;
|
|
counter++;
|
|
}
|
|
parent = selectedContours[parent].parent;
|
|
}
|
|
}
|
|
counter++;
|
|
}
|
|
depth++;
|
|
}
|
|
|
|
for (size_t i = 0ull; i < selectedContours.size(); i++) {
|
|
if (validCandidates[i] > 0) {
|
|
// shift corner positions to the correct rotation
|
|
correctCornerPosition(selectedContours[i].corners, rotated[i]);
|
|
|
|
accepted.push_back(selectedContours[i].corners);
|
|
contours.push_back(selectedContours[i].contour);
|
|
ids.push_back(idsTmp[i]);
|
|
}
|
|
else {
|
|
rejected.push_back(selectedContours[i].corners);
|
|
}
|
|
}
|
|
|
|
// parse output
|
|
if(_rejected.needed()) {
|
|
_copyVector2Output(rejected, _rejected);
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
ArucoDetector::ArucoDetector(const Dictionary &_dictionary,
|
|
const DetectorParameters &_detectorParams,
|
|
const RefineParameters& _refineParams) {
|
|
arucoDetectorImpl = makePtr<ArucoDetectorImpl>(_dictionary, _detectorParams, _refineParams);
|
|
}
|
|
|
|
void ArucoDetector::detectMarkers(InputArray _image, OutputArrayOfArrays _corners, OutputArray _ids,
|
|
OutputArrayOfArrays _rejectedImgPoints) const {
|
|
CV_Assert(!_image.empty());
|
|
DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams;
|
|
const Dictionary& dictionary = arucoDetectorImpl->dictionary;
|
|
|
|
CV_Assert(detectorParams.markerBorderBits > 0);
|
|
// check that the parameters are set correctly if Aruco3 is used
|
|
CV_Assert(!(detectorParams.useAruco3Detection == true &&
|
|
detectorParams.minSideLengthCanonicalImg == 0 &&
|
|
detectorParams.minMarkerLengthRatioOriginalImg == 0.0));
|
|
|
|
Mat grey;
|
|
_convertToGrey(_image.getMat(), grey);
|
|
|
|
// Aruco3 functionality is the extension of Aruco.
|
|
// The description can be found in:
|
|
// [1] Speeded up detection of squared fiducial markers, 2018, FJ Romera-Ramirez et al.
|
|
// if Aruco3 functionality if not wanted
|
|
// change some parameters to be sure to turn it off
|
|
if (!detectorParams.useAruco3Detection) {
|
|
detectorParams.minMarkerLengthRatioOriginalImg = 0.0;
|
|
detectorParams.minSideLengthCanonicalImg = 0;
|
|
}
|
|
else {
|
|
// always turn on corner refinement in case of Aruco3, due to upsampling
|
|
detectorParams.cornerRefinementMethod = (int)CORNER_REFINE_SUBPIX;
|
|
// only CORNER_REFINE_SUBPIX implement correctly for useAruco3Detection
|
|
// Todo: update other CORNER_REFINE methods
|
|
}
|
|
|
|
/// Step 0: equation (2) from paper [1]
|
|
const float fxfy = (!detectorParams.useAruco3Detection ? 1.f : detectorParams.minSideLengthCanonicalImg /
|
|
(detectorParams.minSideLengthCanonicalImg + std::max(grey.cols, grey.rows)*
|
|
detectorParams.minMarkerLengthRatioOriginalImg));
|
|
|
|
/// Step 1: create image pyramid. Section 3.4. in [1]
|
|
vector<Mat> grey_pyramid;
|
|
int closest_pyr_image_idx = 0, num_levels = 0;
|
|
//// Step 1.1: resize image with equation (1) from paper [1]
|
|
if (detectorParams.useAruco3Detection) {
|
|
const float scale_pyr = 2.f;
|
|
const float img_area = static_cast<float>(grey.rows*grey.cols);
|
|
const float min_area_marker = static_cast<float>(detectorParams.minSideLengthCanonicalImg*
|
|
detectorParams.minSideLengthCanonicalImg);
|
|
// find max level
|
|
num_levels = static_cast<int>(log2(img_area / min_area_marker)/scale_pyr);
|
|
// the closest pyramid image to the downsampled segmentation image
|
|
// will later be used as start index for corner upsampling
|
|
const float scale_img_area = img_area * fxfy * fxfy;
|
|
closest_pyr_image_idx = cvRound(log2(img_area / scale_img_area)/scale_pyr);
|
|
}
|
|
buildPyramid(grey, grey_pyramid, num_levels);
|
|
|
|
// resize to segmentation image
|
|
// in this reduces size the contours will be detected
|
|
if (fxfy != 1.f)
|
|
resize(grey, grey, Size(cvRound(fxfy * grey.cols), cvRound(fxfy * grey.rows)));
|
|
|
|
/// STEP 2: Detect marker candidates
|
|
vector<vector<Point2f> > candidates;
|
|
vector<vector<Point> > contours;
|
|
vector<int> ids;
|
|
|
|
/// STEP 2.a Detect marker candidates :: using AprilTag
|
|
if(detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_APRILTAG){
|
|
_apriltag(grey, detectorParams, candidates, contours);
|
|
}
|
|
/// STEP 2.b Detect marker candidates :: traditional way
|
|
else {
|
|
arucoDetectorImpl->detectCandidates(grey, candidates, contours);
|
|
}
|
|
|
|
/// STEP 2.c FILTER OUT NEAR CANDIDATE PAIRS
|
|
auto selectedCandidates = arucoDetectorImpl->filterTooCloseCandidates(candidates, contours);
|
|
|
|
/// STEP 2: Check candidate codification (identify markers)
|
|
arucoDetectorImpl->identifyCandidates(grey, grey_pyramid, selectedCandidates, candidates, contours,
|
|
ids, _rejectedImgPoints);
|
|
|
|
/// STEP 3: Corner refinement :: use corner subpix
|
|
if (detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_SUBPIX) {
|
|
CV_Assert(detectorParams.cornerRefinementWinSize > 0 && detectorParams.cornerRefinementMaxIterations > 0 &&
|
|
detectorParams.cornerRefinementMinAccuracy > 0);
|
|
// Do subpixel estimation. In Aruco3 start on the lowest pyramid level and upscale the corners
|
|
parallel_for_(Range(0, (int)candidates.size()), [&](const Range& range) {
|
|
const int begin = range.start;
|
|
const int end = range.end;
|
|
|
|
for (int i = begin; i < end; i++) {
|
|
if (detectorParams.useAruco3Detection) {
|
|
const float scale_init = (float) grey_pyramid[closest_pyr_image_idx].cols / grey.cols;
|
|
findCornerInPyrImage(scale_init, closest_pyr_image_idx, grey_pyramid, Mat(candidates[i]), detectorParams);
|
|
}
|
|
else {
|
|
int cornerRefinementWinSize = std::max(1, cvRound(detectorParams.relativeCornerRefinmentWinSize*
|
|
getAverageModuleSize(candidates[i], dictionary.markerSize, detectorParams.markerBorderBits)));
|
|
cornerRefinementWinSize = min(cornerRefinementWinSize, detectorParams.cornerRefinementWinSize);
|
|
cornerSubPix(grey, Mat(candidates[i]), Size(cornerRefinementWinSize, cornerRefinementWinSize), Size(-1, -1),
|
|
TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
|
|
detectorParams.cornerRefinementMaxIterations,
|
|
detectorParams.cornerRefinementMinAccuracy));
|
|
}
|
|
}
|
|
});
|
|
}
|
|
|
|
/// STEP 3, Optional : Corner refinement :: use contour container
|
|
if (detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_CONTOUR){
|
|
|
|
if (!ids.empty()) {
|
|
|
|
// do corner refinement using the contours for each detected markers
|
|
parallel_for_(Range(0, (int)candidates.size()), [&](const Range& range) {
|
|
for (int i = range.start; i < range.end; i++) {
|
|
_refineCandidateLines(contours[i], candidates[i]);
|
|
}
|
|
});
|
|
}
|
|
}
|
|
|
|
if (detectorParams.cornerRefinementMethod != (int)CORNER_REFINE_SUBPIX && fxfy != 1.f) {
|
|
// only CORNER_REFINE_SUBPIX implement correctly for useAruco3Detection
|
|
// Todo: update other CORNER_REFINE methods
|
|
|
|
// scale to orignal size, this however will lead to inaccurate detections!
|
|
for (auto &vecPoints : candidates)
|
|
for (auto &point : vecPoints)
|
|
point *= 1.f/fxfy;
|
|
}
|
|
|
|
// copy to output arrays
|
|
_copyVector2Output(candidates, _corners);
|
|
Mat(ids).copyTo(_ids);
|
|
}
|
|
|
|
/**
|
|
* Project board markers that are not included in the list of detected markers
|
|
*/
|
|
static inline void _projectUndetectedMarkers(const Board &board, InputOutputArrayOfArrays detectedCorners,
|
|
InputOutputArray detectedIds, InputArray cameraMatrix, InputArray distCoeffs,
|
|
vector<vector<Point2f> >& undetectedMarkersProjectedCorners,
|
|
OutputArray undetectedMarkersIds) {
|
|
Mat rvec, tvec; // first estimate board pose with the current avaible markers
|
|
Mat objPoints, imgPoints; // object and image points for the solvePnP function
|
|
// To refine corners of ArUco markers the function refineDetectedMarkers() find an aruco markers pose from 3D-2D point correspondences.
|
|
// To find 3D-2D point correspondences uses matchImagePoints().
|
|
// The method matchImagePoints() works with ArUco corners (in Board/GridBoard cases) or with ChArUco corners (in CharucoBoard case).
|
|
// To refine corners of ArUco markers we need work with ArUco corners only in all boards.
|
|
// To call matchImagePoints() with ArUco corners for all boards we need to call matchImagePoints() from base class Board.
|
|
// The method matchImagePoints() implemented in Pimpl and we need to create temp Board object to call the base method.
|
|
Board(board.getObjPoints(), board.getDictionary(), board.getIds()).matchImagePoints(detectedCorners, detectedIds, objPoints, imgPoints);
|
|
if (objPoints.total() < 4ull) // at least one marker from board so rvec and tvec are valid
|
|
return;
|
|
solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec);
|
|
|
|
// search undetected markers and project them using the previous pose
|
|
vector<vector<Point2f> > undetectedCorners;
|
|
const std::vector<int>& ids = board.getIds();
|
|
vector<int> undetectedIds;
|
|
for(unsigned int i = 0; i < ids.size(); i++) {
|
|
int foundIdx = -1;
|
|
for(unsigned int j = 0; j < detectedIds.total(); j++) {
|
|
if(ids[i] == detectedIds.getMat().ptr<int>()[j]) {
|
|
foundIdx = j;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// not detected
|
|
if(foundIdx == -1) {
|
|
undetectedCorners.push_back(vector<Point2f>());
|
|
undetectedIds.push_back(ids[i]);
|
|
projectPoints(board.getObjPoints()[i], rvec, tvec, cameraMatrix, distCoeffs,
|
|
undetectedCorners.back());
|
|
}
|
|
}
|
|
// parse output
|
|
Mat(undetectedIds).copyTo(undetectedMarkersIds);
|
|
undetectedMarkersProjectedCorners = undetectedCorners;
|
|
}
|
|
|
|
/**
|
|
* Interpolate board markers that are not included in the list of detected markers using
|
|
* global homography
|
|
*/
|
|
static void _projectUndetectedMarkers(const Board &_board, InputOutputArrayOfArrays _detectedCorners,
|
|
InputOutputArray _detectedIds,
|
|
vector<vector<Point2f> >& _undetectedMarkersProjectedCorners,
|
|
OutputArray _undetectedMarkersIds) {
|
|
// check board points are in the same plane, if not, global homography cannot be applied
|
|
CV_Assert(_board.getObjPoints().size() > 0);
|
|
CV_Assert(_board.getObjPoints()[0].size() > 0);
|
|
float boardZ = _board.getObjPoints()[0][0].z;
|
|
for(unsigned int i = 0; i < _board.getObjPoints().size(); i++) {
|
|
for(unsigned int j = 0; j < _board.getObjPoints()[i].size(); j++)
|
|
CV_Assert(boardZ == _board.getObjPoints()[i][j].z);
|
|
}
|
|
|
|
vector<Point2f> detectedMarkersObj2DAll; // Object coordinates (without Z) of all the detected
|
|
// marker corners in a single vector
|
|
vector<Point2f> imageCornersAll; // Image corners of all detected markers in a single vector
|
|
vector<vector<Point2f> > undetectedMarkersObj2D; // Object coordinates (without Z) of all
|
|
// missing markers in different vectors
|
|
vector<int> undetectedMarkersIds; // ids of missing markers
|
|
// find markers included in board, and missing markers from board. Fill the previous vectors
|
|
for(unsigned int j = 0; j < _board.getIds().size(); j++) {
|
|
bool found = false;
|
|
for(unsigned int i = 0; i < _detectedIds.total(); i++) {
|
|
if(_detectedIds.getMat().ptr<int>()[i] == _board.getIds()[j]) {
|
|
for(int c = 0; c < 4; c++) {
|
|
imageCornersAll.push_back(_detectedCorners.getMat(i).ptr<Point2f>()[c]);
|
|
detectedMarkersObj2DAll.push_back(
|
|
Point2f(_board.getObjPoints()[j][c].x, _board.getObjPoints()[j][c].y));
|
|
}
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if(!found) {
|
|
undetectedMarkersObj2D.push_back(vector<Point2f>());
|
|
for(int c = 0; c < 4; c++) {
|
|
undetectedMarkersObj2D.back().push_back(
|
|
Point2f(_board.getObjPoints()[j][c].x, _board.getObjPoints()[j][c].y));
|
|
}
|
|
undetectedMarkersIds.push_back(_board.getIds()[j]);
|
|
}
|
|
}
|
|
if(imageCornersAll.size() == 0) return;
|
|
|
|
// get homography from detected markers
|
|
Mat transformation = findHomography(detectedMarkersObj2DAll, imageCornersAll);
|
|
|
|
_undetectedMarkersProjectedCorners.resize(undetectedMarkersIds.size());
|
|
|
|
// for each undetected marker, apply transformation
|
|
for(unsigned int i = 0; i < undetectedMarkersObj2D.size(); i++) {
|
|
perspectiveTransform(undetectedMarkersObj2D[i], _undetectedMarkersProjectedCorners[i], transformation);
|
|
}
|
|
Mat(undetectedMarkersIds).copyTo(_undetectedMarkersIds);
|
|
}
|
|
|
|
void ArucoDetector::refineDetectedMarkers(InputArray _image, const Board& _board,
|
|
InputOutputArrayOfArrays _detectedCorners, InputOutputArray _detectedIds,
|
|
InputOutputArrayOfArrays _rejectedCorners, InputArray _cameraMatrix,
|
|
InputArray _distCoeffs, OutputArray _recoveredIdxs) const {
|
|
DetectorParameters& detectorParams = arucoDetectorImpl->detectorParams;
|
|
const Dictionary& dictionary = arucoDetectorImpl->dictionary;
|
|
RefineParameters& refineParams = arucoDetectorImpl->refineParams;
|
|
CV_Assert(refineParams.minRepDistance > 0);
|
|
|
|
if(_detectedIds.total() == 0 || _rejectedCorners.total() == 0) return;
|
|
|
|
// get projections of missing markers in the board
|
|
vector<vector<Point2f> > undetectedMarkersCorners;
|
|
vector<int> undetectedMarkersIds;
|
|
if(_cameraMatrix.total() != 0) {
|
|
// reproject based on camera projection model
|
|
_projectUndetectedMarkers(_board, _detectedCorners, _detectedIds, _cameraMatrix, _distCoeffs,
|
|
undetectedMarkersCorners, undetectedMarkersIds);
|
|
|
|
} else {
|
|
// reproject based on global homography
|
|
_projectUndetectedMarkers(_board, _detectedCorners, _detectedIds, undetectedMarkersCorners,
|
|
undetectedMarkersIds);
|
|
}
|
|
|
|
// list of missing markers indicating if they have been assigned to a candidate
|
|
vector<bool > alreadyIdentified(_rejectedCorners.total(), false);
|
|
|
|
// maximum bits that can be corrected
|
|
int maxCorrectionRecalculated =
|
|
int(double(dictionary.maxCorrectionBits) * refineParams.errorCorrectionRate);
|
|
|
|
Mat grey;
|
|
_convertToGrey(_image, grey);
|
|
|
|
// vector of final detected marker corners and ids
|
|
vector<vector<Point2f> > finalAcceptedCorners;
|
|
vector<int> finalAcceptedIds;
|
|
// fill with the current markers
|
|
finalAcceptedCorners.resize(_detectedCorners.total());
|
|
finalAcceptedIds.resize(_detectedIds.total());
|
|
for(unsigned int i = 0; i < _detectedIds.total(); i++) {
|
|
finalAcceptedCorners[i] = _detectedCorners.getMat(i).clone();
|
|
finalAcceptedIds[i] = _detectedIds.getMat().ptr<int>()[i];
|
|
}
|
|
vector<int> recoveredIdxs; // original indexes of accepted markers in _rejectedCorners
|
|
|
|
// for each missing marker, try to find a correspondence
|
|
for(unsigned int i = 0; i < undetectedMarkersIds.size(); i++) {
|
|
|
|
// best match at the moment
|
|
int closestCandidateIdx = -1;
|
|
double closestCandidateDistance = refineParams.minRepDistance * refineParams.minRepDistance + 1;
|
|
Mat closestRotatedMarker;
|
|
|
|
for(unsigned int j = 0; j < _rejectedCorners.total(); j++) {
|
|
if(alreadyIdentified[j]) continue;
|
|
|
|
// check distance
|
|
double minDistance = closestCandidateDistance + 1;
|
|
bool valid = false;
|
|
int validRot = 0;
|
|
for(int c = 0; c < 4; c++) { // first corner in rejected candidate
|
|
double currentMaxDistance = 0;
|
|
for(int k = 0; k < 4; k++) {
|
|
Point2f rejCorner = _rejectedCorners.getMat(j).ptr<Point2f>()[(c + k) % 4];
|
|
Point2f distVector = undetectedMarkersCorners[i][k] - rejCorner;
|
|
double cornerDist = distVector.x * distVector.x + distVector.y * distVector.y;
|
|
currentMaxDistance = max(currentMaxDistance, cornerDist);
|
|
}
|
|
// if distance is better than current best distance
|
|
if(currentMaxDistance < closestCandidateDistance) {
|
|
valid = true;
|
|
validRot = c;
|
|
minDistance = currentMaxDistance;
|
|
}
|
|
if(!refineParams.checkAllOrders) break;
|
|
}
|
|
|
|
if(!valid) continue;
|
|
|
|
// apply rotation
|
|
Mat rotatedMarker;
|
|
if(refineParams.checkAllOrders) {
|
|
rotatedMarker = Mat(4, 1, CV_32FC2);
|
|
for(int c = 0; c < 4; c++)
|
|
rotatedMarker.ptr<Point2f>()[c] =
|
|
_rejectedCorners.getMat(j).ptr<Point2f>()[(c + 4 + validRot) % 4];
|
|
}
|
|
else rotatedMarker = _rejectedCorners.getMat(j);
|
|
|
|
// last filter, check if inner code is close enough to the assigned marker code
|
|
int codeDistance = 0;
|
|
// if errorCorrectionRate, dont check code
|
|
if(refineParams.errorCorrectionRate >= 0) {
|
|
|
|
// extract bits
|
|
Mat bits = _extractBits(
|
|
grey, rotatedMarker, dictionary.markerSize, detectorParams.markerBorderBits,
|
|
detectorParams.perspectiveRemovePixelPerCell,
|
|
detectorParams.perspectiveRemoveIgnoredMarginPerCell, detectorParams.minOtsuStdDev);
|
|
|
|
Mat onlyBits =
|
|
bits.rowRange(detectorParams.markerBorderBits, bits.rows - detectorParams.markerBorderBits)
|
|
.colRange(detectorParams.markerBorderBits, bits.rows - detectorParams.markerBorderBits);
|
|
|
|
codeDistance =
|
|
dictionary.getDistanceToId(onlyBits, undetectedMarkersIds[i], false);
|
|
}
|
|
|
|
// if everythin is ok, assign values to current best match
|
|
if(refineParams.errorCorrectionRate < 0 || codeDistance < maxCorrectionRecalculated) {
|
|
closestCandidateIdx = j;
|
|
closestCandidateDistance = minDistance;
|
|
closestRotatedMarker = rotatedMarker;
|
|
}
|
|
}
|
|
|
|
// if at least one good match, we have rescue the missing marker
|
|
if(closestCandidateIdx >= 0) {
|
|
|
|
// subpixel refinement
|
|
if(detectorParams.cornerRefinementMethod == (int)CORNER_REFINE_SUBPIX) {
|
|
CV_Assert(detectorParams.cornerRefinementWinSize > 0 &&
|
|
detectorParams.cornerRefinementMaxIterations > 0 &&
|
|
detectorParams.cornerRefinementMinAccuracy > 0);
|
|
|
|
std::vector<Point2f> marker(closestRotatedMarker.begin<Point2f>(), closestRotatedMarker.end<Point2f>());
|
|
int cornerRefinementWinSize = std::max(1, cvRound(detectorParams.relativeCornerRefinmentWinSize*
|
|
getAverageModuleSize(marker, dictionary.markerSize, detectorParams.markerBorderBits)));
|
|
cornerRefinementWinSize = min(cornerRefinementWinSize, detectorParams.cornerRefinementWinSize);
|
|
cornerSubPix(grey, closestRotatedMarker,
|
|
Size(cornerRefinementWinSize, cornerRefinementWinSize),
|
|
Size(-1, -1), TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,
|
|
detectorParams.cornerRefinementMaxIterations,
|
|
detectorParams.cornerRefinementMinAccuracy));
|
|
}
|
|
|
|
// remove from rejected
|
|
alreadyIdentified[closestCandidateIdx] = true;
|
|
|
|
// add to detected
|
|
finalAcceptedCorners.push_back(closestRotatedMarker);
|
|
finalAcceptedIds.push_back(undetectedMarkersIds[i]);
|
|
|
|
// add the original index of the candidate
|
|
recoveredIdxs.push_back(closestCandidateIdx);
|
|
}
|
|
}
|
|
|
|
// parse output
|
|
if(finalAcceptedIds.size() != _detectedIds.total()) {
|
|
// parse output
|
|
Mat(finalAcceptedIds).copyTo(_detectedIds);
|
|
_copyVector2Output(finalAcceptedCorners, _detectedCorners);
|
|
|
|
// recalculate _rejectedCorners based on alreadyIdentified
|
|
vector<vector<Point2f> > finalRejected;
|
|
for(unsigned int i = 0; i < alreadyIdentified.size(); i++) {
|
|
if(!alreadyIdentified[i]) {
|
|
finalRejected.push_back(_rejectedCorners.getMat(i).clone());
|
|
}
|
|
}
|
|
_copyVector2Output(finalRejected, _rejectedCorners);
|
|
|
|
if(_recoveredIdxs.needed()) {
|
|
Mat(recoveredIdxs).copyTo(_recoveredIdxs);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ArucoDetector::write(FileStorage &fs) const
|
|
{
|
|
arucoDetectorImpl->dictionary.writeDictionary(fs);
|
|
arucoDetectorImpl->detectorParams.writeDetectorParameters(fs);
|
|
arucoDetectorImpl->refineParams.writeRefineParameters(fs);
|
|
}
|
|
|
|
void ArucoDetector::read(const FileNode &fn) {
|
|
arucoDetectorImpl->dictionary.readDictionary(fn);
|
|
arucoDetectorImpl->detectorParams.readDetectorParameters(fn);
|
|
arucoDetectorImpl->refineParams.readRefineParameters(fn);
|
|
}
|
|
|
|
const Dictionary& ArucoDetector::getDictionary() const {
|
|
return arucoDetectorImpl->dictionary;
|
|
}
|
|
|
|
void ArucoDetector::setDictionary(const Dictionary& dictionary) {
|
|
arucoDetectorImpl->dictionary = dictionary;
|
|
}
|
|
|
|
const DetectorParameters& ArucoDetector::getDetectorParameters() const {
|
|
return arucoDetectorImpl->detectorParams;
|
|
}
|
|
|
|
void ArucoDetector::setDetectorParameters(const DetectorParameters& detectorParameters) {
|
|
arucoDetectorImpl->detectorParams = detectorParameters;
|
|
}
|
|
|
|
const RefineParameters& ArucoDetector::getRefineParameters() const {
|
|
return arucoDetectorImpl->refineParams;
|
|
}
|
|
|
|
void ArucoDetector::setRefineParameters(const RefineParameters& refineParameters) {
|
|
arucoDetectorImpl->refineParams = refineParameters;
|
|
}
|
|
|
|
void drawDetectedMarkers(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.channels() == 2);
|
|
if (currentMarker.type() != CV_32SC2)
|
|
currentMarker.convertTo(currentMarker, CV_32SC2);
|
|
|
|
// draw marker sides
|
|
for(int j = 0; j < 4; j++) {
|
|
Point p0, p1;
|
|
p0 = currentMarker.ptr<Point>(0)[j];
|
|
p1 = currentMarker.ptr<Point>(0)[(j + 1) % 4];
|
|
line(_image, p0, p1, borderColor, 1);
|
|
}
|
|
// draw first corner mark
|
|
rectangle(_image, currentMarker.ptr<Point>(0)[0] - Point(3, 3),
|
|
currentMarker.ptr<Point>(0)[0] + Point(3, 3), cornerColor, 1, LINE_AA);
|
|
|
|
// draw ID
|
|
if(_ids.total() != 0) {
|
|
Point cent(0, 0);
|
|
for(int p = 0; p < 4; p++)
|
|
cent += currentMarker.ptr<Point>(0)[p];
|
|
cent = cent / 4.;
|
|
stringstream s;
|
|
s << "id=" << _ids.getMat().ptr<int>(0)[i];
|
|
putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2);
|
|
}
|
|
}
|
|
}
|
|
|
|
void generateImageMarker(const Dictionary &dictionary, int id, int sidePixels, OutputArray _img, int borderBits) {
|
|
dictionary.generateImageMarker(id, sidePixels, _img, borderBits);
|
|
}
|
|
|
|
}
|
|
}
|