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

[3501/OpenCVShowImage] / GoalTracker.java

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;
	}
}