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 | * | |
fd7b42ee E |
81 | * reads an image from a live image capture and outputs information to the a |
82 | * file. It currently spits out an image that has contours of white objects, | |
83 | * or filters white objects to be brighter. | |
84 | * | |
85 | * MatThresh = contours, MatOriginal = white filter. | |
abfe39d4 E |
86 | */ |
87 | public static void processImage() { | |
88 | ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>(); | |
89 | double x, y, targetX, targetY, distance, azimuth; | |
90 | int FrameCount = 0; | |
91 | long before = System.currentTimeMillis(); | |
92 | ||
93 | while (FrameCount < 1) { | |
94 | contours.clear(); | |
fd7b42ee | 95 | matOriginal = Imgcodecs.imread("imgs/test.png"); |
abfe39d4 E |
96 | |
97 | Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV); | |
98 | ||
99 | Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh); | |
b0441b26 | 100 | Imgcodecs.imwrite("output.png", matThresh); |
abfe39d4 E |
101 | |
102 | Imgproc.findContours(matThresh, contours, matHeirarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); | |
103 | ||
104 | for (Iterator<MatOfPoint> iterator = contours.iterator(); iterator.hasNext();) { | |
105 | MatOfPoint matOfPoint = iterator.next(); | |
106 | Rect rec = Imgproc.boundingRect(matOfPoint); | |
107 | if (rec.height < 25 || rec.width < 25) { | |
108 | iterator.remove(); | |
109 | continue; | |
110 | } | |
111 | } | |
112 | ||
113 | for (MatOfPoint mop : contours) { | |
114 | Rect rec = Imgproc.boundingRect(mop); | |
115 | Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK, 5); | |
116 | } | |
117 | ||
118 | // if there is only 1 target, then we have found the target we want | |
119 | ||
120 | if (contours.size() == 1) { | |
121 | Rect rec = Imgproc.boundingRect(contours.get(0)); | |
122 | y = rec.br().y + rec.height / 2; | |
123 | y = -((2 * (y / matOriginal.height())) - 1); | |
124 | distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT) | |
125 | / Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180); | |
126 | // angle to target...would not rely on this | |
127 | targetX = rec.tl().x + rec.width / 2; | |
128 | targetX = (2 * (targetX / matOriginal.width())) - 1; | |
129 | azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0); | |
130 | ||
131 | Point center = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y - rec.height / 2); | |
132 | Point centerw = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y - rec.height / 2 - 20); | |
133 | ||
b0441b26 E |
134 | Imgproc.putText(matOriginal, "hello" + (int) distance, center, Core.FONT_HERSHEY_PLAIN, 20, BLACK); |
135 | Imgproc.putText(matOriginal, "hello2" + (int) azimuth, centerw, Core.FONT_HERSHEY_PLAIN, 20, BLACK); | |
abfe39d4 E |
136 | |
137 | System.out.println(distance + "m, " + azimuth + " degrees " + center.x + ", " + center.y); | |
138 | ||
139 | } | |
140 | FrameCount++; | |
b0441b26 | 141 | // Imgcodecs.imwrite("output.png", matThresh); |
abfe39d4 E |
142 | } |
143 | shouldRun = false; | |
144 | } | |
145 | ||
146 | /** | |
147 | * @param angle | |
148 | * a nonnormalized angle | |
149 | */ | |
150 | ||
151 | public static double normalize360(double angle) { | |
152 | while (angle >= 360.0) { | |
153 | angle -= 360.0; | |
154 | } | |
155 | while (angle < 0.0) { | |
156 | angle += 360.0; | |
157 | } | |
158 | return angle; | |
159 | } | |
160 | } |