Vision-code/RobotVision.cpp at master · Robotics-Team-5268/Vision-code · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#include <vector>

#include "RobotVision.hpp"
#include "Contour.h"
#include "GripPipeline.h"
#include "UDPSender.h"

static void calculateContourAngle( Contour& aContour, std::vector<grip::Line>& filterLinesOutput );

void RobotVision::drawHWCA(cv::Mat &frame, std::vector<shape> &filterContoursOutput,std::vector<grip::Line> &filterLinesOutput, UDPSender *udpsender)
{
	std::vector<Contour> contours;

	if( filterContoursOutput.size() == 0 )
	{
		std::cout << "no contours";
	}

	for(shape shapes : filterContoursOutput)
	{
		Contour contour;
		contour.mBoundingBox = cv::boundingRect( shapes );
		contour.mCenterX = contour.mBoundingBox.x + contour.mBoundingBox.width / 2;
		contour.mCenterY = contour.mBoundingBox.y + contour.mBoundingBox.height / 2;
		calculateContourAngle( contour, filterLinesOutput );
		contours.push_back( contour );
	}

	udpsender->sendContours( contours );

}

static void calculateContourAngle
	(
	Contour& aContour, // Input/Output parameter
	std::vector<grip::Line>& filterLinesOutput
	)
{
	std::vector<grip::Line> contourLines;

	// Generate a slightly larger bounding box to incorporate lines on the edge
	cv::Rect inflatedRect( aContour.mBoundingBox );
	cv::Size inflationSize( 10, 20 );
	cv::Point offset( 5, 10 );
	inflatedRect += inflationSize;
	inflatedRect -= offset;
	//std::cout << "bounding box ";
	//std::cout << aContour.mBoundingBox.x << "-->" << inflatedRect.x << " " ;
	//std::cout << aContour.mBoundingBox.y << "-->" << inflatedRect.y << " ";
	//std::cout << aContour.mBoundingBox.width << "-->" << inflatedRect.width << " ";
	//std::cout << aContour.mBoundingBox.height << "-->" << inflatedRect.height << std::endl;

	for( const grip::Line line : filterLinesOutput )
	{
		if( inflatedRect.contains( cv::Point( line.x1, line.y1 ) ) &&
			inflatedRect.contains( cv::Point( line.x2, line.y2 ) ) )
		{
			contourLines.push_back( line );
		}
	}

	float angle = 0;
	for( grip::Line line : contourLines )
	{
		if( line.angle() >= 0 )
		{
			angle += line.angle();
		}
		else
		{
			angle += line.angle() + 180.0f;
		}
		//std::cout << "ang=" << line.angle();
	}

	// Average the angles of the lines that represent the contour.  They *should* all be roughly equivalent
	// TODO: Should there be something more complex for outliers?
	if( contourLines.size() > 0 )
	{
	    aContour.mAngle = angle / contourLines.size();
	}

}