Skip to content

Instantly share code, notes, and snippets.

@sp7412

sp7412/gist:9379327f772fde4e7ed31f83ead6fb0b

Created March 16, 2024 18:44
Show Gist options
  • Save sp7412/9379327f772fde4e7ed31f83ead6fb0b to your computer and use it in GitHub Desktop.
Save sp7412/9379327f772fde4e7ed31f83ead6fb0b to your computer and use it in GitHub Desktop.
Download ZIP
opencv snippet
Raw
gistfile1.txt
https://raw.githubusercontent.com/opencv/opencv/1eb061f89de0fb85c4c75a2deeb0f61a961a63ad/modules/objdetect/src/aruco/aruco_detector.cpp
// 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 &params, 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;
}
vector<vector<Point> > contours;
findContours(in, 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(in.cols, in.rows) * max(in.cols, in.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 > in.cols - 1 - minDistanceToBorder ||
approxCurve[j].y > in.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 &params) {
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, 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);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment