| 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 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. |
| 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(); |
| 95 | matOriginal = Imgcodecs.imread("imgs/test.png"); |
| 96 | |
| 97 | Imgproc.cvtColor(matOriginal, matHSV, Imgproc.COLOR_BGR2HSV); |
| 98 | |
| 99 | Core.inRange(matHSV, LOWER_BOUNDS, UPPER_BOUNDS, matThresh); |
| 100 | Imgcodecs.imwrite("output.png", matThresh); |
| 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 | |
| 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); |
| 136 | |
| 137 | System.out.println(distance + "m, " + azimuth + " degrees " + center.x + ", " + center.y); |
| 138 | |
| 139 | } |
| 140 | FrameCount++; |
| 141 | // Imgcodecs.imwrite("output.png", matThresh); |
| 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 | } |