From: EvanYap <evanyap.14@gmail.com>
Date: Tue, 20 Sep 2016 03:57:19 +0000 (-0700)
Subject: add cindy's goaltracker.java class that filters out tape
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2FOpenCVShowImage;a=commitdiff_plain;h=abfe39d4275b5b5357571ab246ba73862db5a9d1

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

diff --git a/GoalTracker.java b/GoalTracker.java
new file mode 100644
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;
+	}
+}