challenge-bot.com Git - 3501/OpenCVShowImage/blob - GoalTracker.java

   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 }