Commit | Line | Data |
---|---|---|
abfe39d4 E |
1 | import java.util.ArrayList; |
2 | import java.util.Iterator; | |
3 | ||
4 | import org.opencv.core.Core; | |
5 | import org.opencv.core.Mat; | |
6 | import org.opencv.core.MatOfPoint; | |
7 | import org.opencv.core.Point; | |
8 | import org.opencv.core.Rect; | |
9 | import org.opencv.core.Scalar; | |
10 | import org.opencv.core.Size; | |
11 | import org.opencv.imgcodecs.Imgcodecs; | |
12 | import org.opencv.imgproc.Imgproc; | |
13 | ||
14 | /** | |
15 | * | |
16 | */ | |
17 | ||
18 | /** | |
19 | * | |
20 | * @author Elijah Kaufman | |
21 | * @version 1.0 | |
22 | * @description Uses opencv and network table 3.0 to detect the vision targets | |
23 | * | |
24 | */ | |
25 | public class GoalTracker { | |
26 | ||
27 | /** | |
28 | * static method to load opencv and networkTables | |
29 | */ | |
30 | static { | |
31 | System.loadLibrary(Core.NATIVE_LIBRARY_NAME); | |
32 | } | |
33 | ||
34 | // constants for the color rbg values | |
35 | public static final Scalar BLACK = new Scalar(0, 0, 0), LOWER_BOUNDS = new Scalar(58, 0, 109), | |
36 | UPPER_BOUNDS = new Scalar(93, 255, 255); | |
37 | ||
38 | public static int[] BLACKINT = { 0, 0, 0 }; | |
39 | public static int[] WHITEINT = { 255, 255, 255 }; | |
40 | ||
41 | // LOWER_BOUNDS = new Scalar(46, 122, 46), | |
42 | // UPPER_BOUNDS = new Scalar(139, 180, 105); | |
43 | ||
44 | // the size for resing the image | |
45 | public static final Size resize = new Size(320, 240); | |
46 | ||
47 | // ignore these | |
48 | public static Mat matOriginal, matHSV, matThresh, clusters, matHeirarchy; | |
49 | ||
50 | // Constants for known variables | |
51 | // the height to the top of the target in first stronghold is 97 inches | |
52 | public static final int TOP_TARGET_HEIGHT = 97; | |
53 | // the physical height of the camera lens | |
54 | public static final int TOP_CAMERA_HEIGHT = 32; | |
55 | ||
56 | // camera details, can usually be found on the datasheets of the camera | |
57 | public static final double VERTICAL_FOV = 51; | |
58 | public static final double HORIZONTAL_FOV = 67; | |
59 | public static final double CAMERA_ANGLE = 10; | |
60 | ||
61 | public static boolean shouldRun = true; | |
62 | ||
63 | /** | |
64 | * | |
65 | * @param args | |
66 | * command line arguments just the main loop for the program and | |
67 | * the entry points | |
68 | */ | |
69 | public static void main(String[] args) { | |
70 | // TODO Auto-generated method stub | |
71 | matOriginal = new Mat(); | |
72 | matHSV = new Mat(); | |
73 | matThresh = new Mat(); | |
74 | clusters = new Mat(); | |
75 | matHeirarchy = new Mat(); | |
76 | processImage(); | |
77 | } | |
78 | ||
79 | /** | |
80 | * | |
81 | * reads an image from a live image capture and outputs information to the | |
82 | * SmartDashboard or a file | |
83 | */ | |
84 | public static void processImage() { | |
85 | ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>(); | |
86 | double x, y, targetX, targetY, distance, azimuth; | |
87 | int FrameCount = 0; | |
88 | long before = System.currentTimeMillis(); | |
89 | ||
90 | while (FrameCount < 1) { | |
91 | contours.clear(); | |
92 | matOriginal = Imgcodecs.imread("goal.jpg"); | |
93 | ||
94 | Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV); | |
95 | ||
96 | Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh); | |
97 | Imgcodecs.imwrite("output.png", matThresh); | |
98 | ||
99 | Imgproc.findContours(matThresh, contours, matHeirarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); | |
100 | ||
101 | for (Iterator<MatOfPoint> iterator = contours.iterator(); iterator.hasNext();) { | |
102 | MatOfPoint matOfPoint = iterator.next(); | |
103 | Rect rec = Imgproc.boundingRect(matOfPoint); | |
104 | if (rec.height < 25 || rec.width < 25) { | |
105 | iterator.remove(); | |
106 | continue; | |
107 | } | |
108 | } | |
109 | ||
110 | for (MatOfPoint mop : contours) { | |
111 | Rect rec = Imgproc.boundingRect(mop); | |
112 | Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK, 5); | |
113 | } | |
114 | ||
115 | // if there is only 1 target, then we have found the target we want | |
116 | ||
117 | if (contours.size() == 1) { | |
118 | Rect rec = Imgproc.boundingRect(contours.get(0)); | |
119 | y = rec.br().y + rec.height / 2; | |
120 | y = -((2 * (y / matOriginal.height())) - 1); | |
121 | distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) | |
122 | / Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180); | |
123 | // angle to target...would not rely on this | |
124 | targetX = rec.tl().x + rec.width / 2; | |
125 | targetX = (2 * (targetX / matOriginal.width())) - 1; | |
126 | azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0); | |
127 | ||
128 | Point center = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y - rec.height / 2); | |
129 | Point centerw = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y - rec.height / 2 - 20); | |
130 | ||
131 | // Imgproc.putText(matOriginal, "hello" + (int) distance, | |
132 | // center, | |
133 | // Core.FONT_HERSHEY_PLAIN, 20, BLACK); | |
134 | // Imgproc.putText(matOriginal, "hello2" + (int) azimuth, | |
135 | // centerw, | |
136 | // Core.FONT_HERSHEY_PLAIN, 20, BLACK); | |
137 | ||
138 | System.out.println(distance + "m, " + azimuth + " degrees " + center.x + ", " + center.y); | |
139 | ||
140 | } | |
141 | FrameCount++; | |
142 | // Imgcodecs.imwrite("output.png", matOriginal); | |
143 | } | |
144 | shouldRun = false; | |
145 | } | |
146 | ||
147 | /** | |
148 | * @param angle | |
149 | * a nonnormalized angle | |
150 | */ | |
151 | ||
152 | public static double normalize360(double angle) { | |
153 | while (angle >= 360.0) { | |
154 | angle -= 360.0; | |
155 | } | |
156 | while (angle < 0.0) { | |
157 | angle += 360.0; | |
158 | } | |
159 | return angle; | |
160 | } | |
161 | } |