void MarkerDetector::findContours(cv::Mat& thresholdImg,
ContoursVector& contours,
int minContourPointsAllowed)
{
ContoursVector allContours;
cv::findContours(thresholdImg, allContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
contours.clear();
for (size_t i=0; i<allContours.size(); i++)
{
int contourSize = allContours[i].size();
if (contourSize > minContourPointsAllowed)
{
contours.push_back(allContours[i]);
}
}
#ifdef SHOW_DEBUG_IMAGES
{
cv::Mat contoursImage(thresholdImg.size(), CV_8UC1);
contoursImage = cv::Scalar(0);
cv::drawContours(contoursImage, contours, -1, cv::Scalar(255), 2, CV_AA);
cv::imshow("Contours", contoursImage);
cv::waitKey();
}
#endif
}
void MarkerDetector::findCandidates
(
const ContoursVector& contours,
std::vector<Marker>& detectedMarkers
)
{
std::vector<cv::Point> approxCurve;
std::vector<Marker> possibleMarkers;
// For each contour, analyze if it is a parallelepiped likely to be the marker
for (size_t i=0; i<contours.size(); i++)
{
// Approximate to a polygon
double eps = contours[i].size() * 0.05;
cv::approxPolyDP(contours[i], approxCurve, eps, true);
// We interested only in polygons that contains only four points
if (approxCurve.size() != 4)
continue;
// And they have to be convex
if (!cv::isContourConvex(approxCurve))
continue;
// Ensure that the distance between consecutive points is large enough
float minDist = std::numeric_limits<float>::max();
for (int i = 0; i < 4; i++)
{
cv::Point side = approxCurve[i] - approxCurve[(i+1)%4];
float squaredSideLength = side.dot(side);
minDist = std::min(minDist, squaredSideLength);
}
// Check that distance is not very small
if (minDist < m_minContourLengthAllowed)
continue;
// All tests are passed. Save marker candidate:
Marker m;
for (int i = 0; i<4; i++)
m.points.push_back( cv::Point2f(approxCurve[i].x,approxCurve[i].y) );
// Sort the points in anti-clockwise order
// Trace a line between the first and second point.
// If the third point is at the right side, then the points are anti-clockwise
cv::Point v1 = m.points[1] - m.points[0];
cv::Point v2 = m.points[2] - m.points[0];
double o = (v1.x * v2.y) - (v1.y * v2.x);
if (o < 0.0) //if the third point is in the left side, then sort in anti-clockwise order
std::swap(m.points[1], m.points[3]);
possibleMarkers.push_back(m);
}
// Remove these elements which corners are too close to each other.
// First detect candidates for removal:
std::vector< std::pair<int,int> > tooNearCandidates;
for (size_t i=0;i<possibleMarkers.size();i++)
{
const Marker& m1 = possibleMarkers[i];
//calculate the average distance of each corner to the nearest corner of the other marker candidate
for (size_t j=i+1;j<possibleMarkers.size();j++)
{
const Marker& m2 = possibleMarkers[j];
float distSquared = 0;
for (int c = 0; c < 4; c++)
{
cv::Point v = m1.points[c] - m2.points[c];
distSquared += v.dot(v);
}
distSquared /= 4;
if (distSquared < 100)
{
tooNearCandidates.push_back(std::pair<int,int>(i,j));
}
}
}
// Mark for removal the element of the pair with smaller perimeter
std::vector<bool> removalMask (possibleMarkers.size(), false);
for (size_t i=0; i<tooNearCandidates.size(); i++)
{
float p1 = perimeter(possibleMarkers[tooNearCandidates[i].first ].points);
float p2 = perimeter(possibleMarkers[tooNearCandidates[i].second].points);
size_t removalIndex;
if (p1 > p2)
removalIndex = tooNearCandidates[i].second;
else
removalIndex = tooNearCandidates[i].first;
removalMask[removalIndex] = true;
}
// Return candidates
detectedMarkers.clear();
for (size_t i=0;i<possibleMarkers.size();i++)
{
if (!removalMask[i])
detectedMarkers.push_back(possibleMarkers[i]);
}
}
void MarkerDetector::findCandidates(cv::Mat src, std::vector<Marker>& detectedMarkers, int minPoints, int maxPoints)
{
cv::Mat thres2;
src.copyTo ( thres2 );
std::vector<cv::Vec4i> hierarchy2;
ContoursVector allContours;
std::vector<cv::Point> approxCurve;
std::vector<Marker> possibleMarkers;
cv::findContours(thres2, allContours,hierarchy2, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
for (size_t i=0; i<allContours.size(); i++)
{
int contourSize = allContours[i].size();
if ( minPoints< contourSize && contourSize<maxPoints )
{
//drawContours( src, allContours, i, cv::Scalar(0,0,255), CV_FILLED, 8);
cv::approxPolyDP(allContours[i], approxCurve, double(contourSize * 0.05), true);
// Selecting only those with 4 points (corners)
if (approxCurve.size() != 4)
continue;
// Selecting only convex polygons
if (!cv::isContourConvex(cv::Mat(approxCurve)))
continue;
// Ensure that the distance between consecutive points is large enough
float minDist = std::numeric_limits<float>::max();
for (int i = 0; i < 4; i++)
{
cv::Point side = approxCurve[i] - approxCurve[(i+1)%4];
//float squaredSideLength = side.dot(side);
float squaredSideLength= std::sqrt ( ( float ) ( approxCurve[i].x-approxCurve[ ( i+1 ) %4].x ) * ( approxCurve[i].x-approxCurve[ ( i+1 ) %4].x ) +
( approxCurve[i].y-approxCurve[ ( i+1 ) %4].y ) * ( approxCurve[i].y-approxCurve[ ( i+1 ) %4].y ) );
//std::cout<<squaredSideLength<< " " <<test<<std::endl;
//minDist = std::min(minDist, squaredSideLength);
if ( squaredSideLength<minDist ) minDist=squaredSideLength;
}
// Selecting those whose minimum side length is still greater than the limit
if (minDist <= 10)
continue;
// All tests are passed. Save marker candidate:
Marker m;
for (int i = 0; i<4; i++)
m.points.push_back( cv::Point2f(approxCurve[i].x,approxCurve[i].y) );
// Sort the points in anti-clockwise order
// Trace a line between the first and second point.
// If the third point is at the right side, then the points are anti-clockwise
cv::Point v1 = m.points[1] - m.points[0];
cv::Point v2 = m.points[2] - m.points[0];
double o = (v1.x * v2.y) - (v1.y * v2.x);
if (o < 0.0) //if the third point is in the left side, then sort in anti-clockwise order
std::swap(m.points[1], m.points[3]);
possibleMarkers.push_back(m);
}
}
// Remove these elements which corners are too close to each other.
// First detect candidates for removal:
std::vector< std::pair<int,int> > tooNearCandidates;
for (size_t i=0;i<possibleMarkers.size();i++)
{
const Marker& m1 = possibleMarkers[i];
//calculate the average distance of each corner to the nearest corner of the other marker candidate
for (size_t j=i+1;j<possibleMarkers.size();j++)
{
const Marker& m2 = possibleMarkers[j];
float distSquared = 0;
for (int c = 0; c < 4; c++)
{
/*
distSquared+= sqrt ( ( m1.points[c].x-m2.points[c].x ) * ( m1.points[c].x-m2.points[c].x ) +
( m1.points[c].y-m2.points[c].y ) * ( m1.points[c].y-m2.points[c].y ) );
*/
distSquared+= sqrt ( ( possibleMarkers[i].points[c].x-possibleMarkers[j].points[c].x ) *
( possibleMarkers[i].points[c].x-possibleMarkers[j].points[c].x ) +
( possibleMarkers[i].points[c].y-possibleMarkers[j].points[c].y ) *
( possibleMarkers[i].points[c].y-possibleMarkers[j].points[c].y ) );
//cv::Point v = m1.points[c] - m2.points[c];
//distSquared2 += v.dot(v);
}
distSquared /= 4;
if (distSquared < 10)
{
tooNearCandidates.push_back(std::pair<int,int>(i,j));
}
}
}
// Mark for removal the element of the pair with smaller perimeter
std::vector<bool> removalMask (possibleMarkers.size(), false);
for (size_t i=0; i<tooNearCandidates.size(); i++)
{
float p1 = getPerimeter(possibleMarkers[tooNearCandidates[i].first ].points);
float p2 = getPerimeter(possibleMarkers[tooNearCandidates[i].second].points);
size_t removalIndex;
if (p1 > p2)
removalIndex = tooNearCandidates[i].second;
else
removalIndex = tooNearCandidates[i].first;
removalMask[removalIndex] = true;
}
// Return candidates
detectedMarkers.clear();
for (size_t i=0;i<possibleMarkers.size();i++)
{
if (!removalMask[i])
detectedMarkers.push_back(possibleMarkers[i]);
}
}
