void ShapeDetector::detect( const Path2D &i_paths )
{
// split in segments
std::vector<std::vector<CGPoint>> segments;
splitInSegments( i_paths, segments );
// all different segements, the bool tells if it's a closed path or not
std::vector<std::pair<std::vector<CGPoint>,bool>> allSegments;
for ( auto segment : segments )
{
if ( segment.size() < 3 )
continue;
if ( aboutTheSame( segment.front(), segment.back(), kEpsilonForPoints ) )
{
// first and last are about the same, a closed segment
segment.back() = segment.front();
allSegments.push_back( std::make_pair( segment, true ) );
}
else
{
// try to merge it with a previous segments
auto it = allSegments.begin();
for ( ; it != allSegments.end(); ++it )
{
if ( it->second )
continue; // skip closed ones
if ( aboutTheSame( segment.front(), it->first.front(), kEpsilonForPoints ) )
{
it->first.insert( it->first.begin(), segment.rbegin(), segment.rend() );
break;
}
else if ( aboutTheSame( segment.front(), it->first.back(), kEpsilonForPoints ) )
{
it->first.insert( it->first.end(), segment.begin(), segment.end() );
break;
}
else if ( aboutTheSame( segment.back(), it->first.front(), kEpsilonForPoints ) )
{
it->first.insert( it->first.begin(), segment.begin(), segment.end() );
break;
}
else if ( aboutTheSame( segment.back(), it->first.back(), kEpsilonForPoints ) )
{
it->first.insert( it->first.end(), segment.rbegin(), segment.rend() );
break;
}
}
if ( it == allSegments.end() )
{
// new independant open segment
allSegments.push_back( std::make_pair( segment, false ) );
}
else
{
// check if the newly created combined segment is closed
if ( aboutTheSame( it->first.front(), it->first.back(), kEpsilonForPoints ) )
{
it->first.back() = it->first.front();
it->second = true;
}
}
}
}
for ( auto it : allSegments )
{
std::vector<size_t> corners;
std::vector<CGPoint> polyline;
cleanUpPointsInStraightLines( it.first, polyline, kEpsilonForStraightLines * kEpsilonForStraightLines );
if ( it.second )
polyline.push_back( polyline.front() );
CGPoint center;
CGFloat radius;
if ( it.second and looksLikeACircle( it.first, center, radius ) )
{
shapeDetected( "Circle", Path2D::circle( radius, center ) );
}
else
{
std::deque<CGPoint> resampled = uniformResample( polyline );
detectCorners( resampled, it.second, corners, 16 );
if ( corners.size() == 0 and not it.second and polyline.size() == 2 )
{
report( "line", polyline );
continue;
}
else if ( corners.size() == 3 and it.second and polyline.size() < 9 )
{
polyline.clear();
polyline.push_back( resampled[corners[0]] );
polyline.push_back( resampled[corners[1]] );
polyline.push_back( resampled[corners[2]] );
polyline.push_back( resampled[corners[0]] );
reportTriangle( polyline );
continue;
}
else if ( corners.size() == 3 and not it.second and polyline.size() < 12 )
{
// potential arrow
//! @todo: improve
CGFloat a1 = smallestAngle( resampled[0], resampled[corners[0]], resampled[corners[1]] );
CGFloat a2 = smallestAngle( resampled[corners[0]], resampled[corners[1]], resampled[corners[2]] );
CGFloat a3 = angle( resampled[corners[1]], resampled[corners[2]], resampled.back() );
if ( a1 < 80 and a2 < 10 and a3 < 100 )
{
//! @todo: cleanup path
report( "Arrow", it.first );
}
}
else if ( corners.size() == 4 and it.second and polyline.size() < 12 )
{
if ( not linesCross( resampled[corners[0]], resampled[corners[1]], resampled[corners[2]], resampled[corners[3]] ) and
not linesCross( resampled[corners[1]], resampled[corners[2]], resampled[corners[3]], resampled[corners[0]] ) )
{
polyline.clear();
polyline.push_back( resampled[corners[0]] );
polyline.push_back( resampled[corners[1]] );
polyline.push_back( resampled[corners[2]] );
polyline.push_back( resampled[corners[3]] );
polyline.push_back( resampled[corners[0]] );
reportQuad( polyline );
continue;
}
}
report( "unkown", it.first );
}
}
}
void FieldLineDetector::findTransformation(cv::Mat& src, cv::Mat& imgDst,
std::vector<cv::Point2f>& modelBots, cv::Mat& H)
{
this->botPosField = modelBots;
Mat imgBw;
blur(src, imgBw, Size(5, 5));
cvtColor(imgBw, imgBw, CV_BGR2GRAY);
Mat imgEdges;
Canny(imgBw, imgEdges, 50, 100, 3);
// imshow("bw", imgBw);
// imshow("edges", imgEdges);
std::vector<cv::Vec4i> lines;
HoughLinesP(imgEdges, lines, 1, CV_PI / 180, min_threshold + p_trackbar,
minLineLength, maxLineGap);
// Expand the lines little bit (by scaleFactor)
for (int i = 0; i < lines.size(); i++)
{
cv::Vec4i v = lines[i];
cv::Point2f p1 = Point2f(v[0], v[1]);
cv::Point2f p2 = Point2f(v[2], v[3]);
cv::Point2f p1p2 = p2 - p1;
float length = norm(p1p2);
cv::Point2f scaleP2 = p2 + p1p2 * (scaleFactor / 10.0f);
cv::Point2f scaleP1 = p1 - p1p2 * (scaleFactor / 10.0f);
lines[i][0] = scaleP1.x;
lines[i][1] = scaleP1.y;
lines[i][2] = scaleP2.x;
lines[i][3] = scaleP2.y;
}
createThresholdedImg(src);
// do line detection!
detectCorners(lines);
filterCorners();
findNeighbors(lines);
findCornerMapping(mappedEdges);
for (int i = 0; i < mappedEdges.size(); i++)
{
cout << (*mappedEdges[i]) << endl;
}
findFieldMatch(mappedEdges, H);
if (imgDst.cols > 0)
{
// Draw lines
for (int i = 0; i < lines.size(); i++)
{
cv::Vec4i v = lines[i];
cv::line(imgDst, cv::Point(v[0], v[1]), cv::Point(v[2], v[3]),
cv::Scalar(0, 255, 0), 2);
}
// draw corners
for (int i = 0; i < cornerBuffer.size(); i++)
{
cv::circle(imgDst, cornerBuffer.at(i), 1, cv::Scalar(255, 0, 0), 2);
}
// draw filtered corners
for (int i = 0; i < detectedCorners.size(); i++)
{
circle(imgDst, detectedCorners[i]->point, (int) 20,
Scalar(0, 255, 255), 1);
}
// draw detected corner coordinates
for (int i = 0; i < detectedCorners.size(); i++)
{
stringstream ss;
ss << detectedCorners[i]->point;
putText(imgDst, ss.str(),
detectedCorners[i]->point + Point2f(0, 10),
FONT_HERSHEY_PLAIN, 1, Scalar(250, 0, 0));
}
}
}
