add cindy's goaltracker.java class that filters out tape

diff --git a/GoalTracker.java b/GoalTracker.java
new file mode 100644 (file)
index 0000000..ec627d0
--- /dev/null
+++ b/GoalTracker.java
@@ -0,0 +1,161 @@
+import java.util.ArrayList;
+import java.util.Iterator;
+
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ *
+ */
+
+/**
+ *
+ * @author Elijah Kaufman
+ * @version 1.0
+ * @description Uses opencv and network table 3.0 to detect the vision targets
+ *
+ */
+public class GoalTracker {
+
+       /**
+        * static method to load opencv and networkTables
+        */
+       static {
+               System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+       }
+
+       // constants for the color rbg values
+       public static final Scalar BLACK = new Scalar(0, 0, 0), LOWER_BOUNDS = new Scalar(58, 0, 109),
+                       UPPER_BOUNDS = new Scalar(93, 255, 255);
+
+       public static int[] BLACKINT = { 0, 0, 0 };
+       public static int[] WHITEINT = { 255, 255, 255 };
+
+       // LOWER_BOUNDS = new Scalar(46, 122, 46),
+       // UPPER_BOUNDS = new Scalar(139, 180, 105);
+
+       // the size for resing the image
+       public static final Size resize = new Size(320, 240);
+
+       // ignore these
+       public static Mat matOriginal, matHSV, matThresh, clusters, matHeirarchy;
+
+       // Constants for known variables
+       // the height to the top of the target in first stronghold is 97 inches
+       public static final int TOP_TARGET_HEIGHT = 97;
+       // the physical height of the camera lens
+       public static final int TOP_CAMERA_HEIGHT = 32;
+
+       // camera details, can usually be found on the datasheets of the camera
+       public static final double VERTICAL_FOV = 51;
+       public static final double HORIZONTAL_FOV = 67;
+       public static final double CAMERA_ANGLE = 10;
+
+       public static boolean shouldRun = true;
+
+       /**
+        *
+        * @param args
+        *            command line arguments just the main loop for the program and
+        *            the entry points
+        */
+       public static void main(String[] args) {
+               // TODO Auto-generated method stub
+               matOriginal = new Mat();
+               matHSV = new Mat();
+               matThresh = new Mat();
+               clusters = new Mat();
+               matHeirarchy = new Mat();
+               processImage();
+       }
+
+       /**
+        *
+        * reads an image from a live image capture and outputs information to the
+        * SmartDashboard or a file
+        */
+       public static void processImage() {
+               ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>();
+               double x, y, targetX, targetY, distance, azimuth;
+               int FrameCount = 0;
+               long before = System.currentTimeMillis();
+
+               while (FrameCount < 1) {
+                       contours.clear();
+                       matOriginal = Imgcodecs.imread("goal.jpg");
+
+                       Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV);
+
+                       Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh);
+                       Imgcodecs.imwrite("output.png", matThresh);
+
+                       Imgproc.findContours(matThresh, contours, matHeirarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);
+
+                       for (Iterator<MatOfPoint> iterator = contours.iterator(); iterator.hasNext();) {
+                               MatOfPoint matOfPoint = iterator.next();
+                               Rect rec = Imgproc.boundingRect(matOfPoint);
+                               if (rec.height < 25 || rec.width < 25) {
+                                       iterator.remove();
+                                       continue;
+                               }
+                       }
+
+                       for (MatOfPoint mop : contours) {
+                               Rect rec = Imgproc.boundingRect(mop);
+                               Imgproc.rectangle(matOriginal, rec.br(), rec.tl(), BLACK, 5);
+                       }
+
+                       // if there is only 1 target, then we have found the target we want
+
+                       if (contours.size() == 1) {
+                               Rect rec = Imgproc.boundingRect(contours.get(0));
+                               y = rec.br().y + rec.height / 2;
+                               y = -((2 * (y / matOriginal.height())) - 1);
+                               distance = (TOP_TARGET_HEIGHT - TOP_CAMERA_HEIGHT)
+                                               / Math.tan((y * VERTICAL_FOV / 2.0 + CAMERA_ANGLE) * Math.PI / 180);
+                               // angle to target...would not rely on this
+                               targetX = rec.tl().x + rec.width / 2;
+                               targetX = (2 * (targetX / matOriginal.width())) - 1;
+                               azimuth = normalize360(targetX * HORIZONTAL_FOV / 2.0 + 0);
+
+                               Point center = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y - rec.height / 2);
+                               Point centerw = new Point(rec.br().x - rec.width / 2 - 15, rec.br().y - rec.height / 2 - 20);
+
+                               // Imgproc.putText(matOriginal, "hello" + (int) distance,
+                               // center,
+                               // Core.FONT_HERSHEY_PLAIN, 20, BLACK);
+                               // Imgproc.putText(matOriginal, "hello2" + (int) azimuth,
+                               // centerw,
+                               // Core.FONT_HERSHEY_PLAIN, 20, BLACK);
+
+                               System.out.println(distance + "m, " + azimuth + " degrees     " + center.x + ", " + center.y);
+
+                       }
+                       FrameCount++;
+                       // Imgcodecs.imwrite("output.png", matOriginal);
+               }
+               shouldRun = false;
+       }
+
+       /**
+        * @param angle
+        *            a nonnormalized angle
+        */
+
+       public static double normalize360(double angle) {
+               while (angle >= 360.0) {
+                       angle -= 360.0;
+               }
+               while (angle < 0.0) {
+                       angle += 360.0;
+               }
+               return angle;
+       }
+}