1 import java
.util
.ArrayList
;
2 import java
.util
.Iterator
;
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
;
20 * @author Elijah Kaufman
22 * @description Uses opencv and network table 3.0 to detect the vision targets
25 public class GoalTracker
{
28 * static method to load opencv and networkTables
31 System
.loadLibrary(Core
.NATIVE_LIBRARY_NAME
);
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);
38 public static int[] BLACKINT
= { 0, 0, 0 };
39 public static int[] WHITEINT
= { 255, 255, 255 };
41 // LOWER_BOUNDS = new Scalar(46, 122, 46),
42 // UPPER_BOUNDS = new Scalar(139, 180, 105);
44 // the size for resing the image
45 public static final Size resize
= new Size(320, 240);
48 public static Mat matOriginal
, matHSV
, matThresh
, clusters
, matHeirarchy
;
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;
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;
61 public static boolean shouldRun
= true;
66 * command line arguments just the main loop for the program and
69 public static void main(String
[] args
) {
70 // TODO Auto-generated method stub
71 matOriginal
= new Mat();
73 matThresh
= new Mat();
75 matHeirarchy
= new Mat();
81 * reads an image from a live image capture and outputs information to the
82 * SmartDashboard or a file
84 public static void processImage() {
85 ArrayList
<MatOfPoint
> contours
= new ArrayList
<MatOfPoint
>();
86 double x
, y
, targetX
, targetY
, distance
, azimuth
;
88 long before
= System
.currentTimeMillis();
90 while (FrameCount
< 1) {
92 matOriginal
= Imgcodecs
.imread("goal.jpg");
94 Imgproc
.cvtColor(matOriginal
, matHSV
, Imgproc
.COLOR_BGR2HSV
);
96 Core
.inRange(matHSV
, LOWER_BOUNDS
, UPPER_BOUNDS
, matThresh
);
97 Imgcodecs
.imwrite("output.png", matThresh
);
99 Imgproc
.findContours(matThresh
, contours
, matHeirarchy
, Imgproc
.RETR_EXTERNAL
, Imgproc
.CHAIN_APPROX_SIMPLE
);
101 for (Iterator
<MatOfPoint
> iterator
= contours
.iterator(); iterator
.hasNext();) {
102 MatOfPoint matOfPoint
= iterator
.next();
103 Rect rec
= Imgproc
.boundingRect(matOfPoint
);
104 if (rec
.height
< 25 || rec
.width
< 25) {
110 for (MatOfPoint mop
: contours
) {
111 Rect rec
= Imgproc
.boundingRect(mop
);
112 Imgproc
.rectangle(matOriginal
, rec
.br(), rec
.tl(), BLACK
, 5);
115 // if there is only 1 target, then we have found the target we want
117 if (contours
.size() == 1) {
118 Rect rec
= Imgproc
.boundingRect(contours
.get(0));
119 y
= rec
.br().y
+ rec
.height
/ 2;
120 y
= -((2 * (y
/ matOriginal
.height())) - 1);
121 distance
= (TOP_TARGET_HEIGHT
- TOP_CAMERA_HEIGHT
)
122 / Math
.tan((y
* VERTICAL_FOV
/ 2.0 + CAMERA_ANGLE
) * Math
.PI
/ 180);
123 // angle to target...would not rely on this
124 targetX
= rec
.tl().x
+ rec
.width
/ 2;
125 targetX
= (2 * (targetX
/ matOriginal
.width())) - 1;
126 azimuth
= normalize360(targetX
* HORIZONTAL_FOV
/ 2.0 + 0);
128 Point center
= new Point(rec
.br().x
- rec
.width
/ 2 - 15, rec
.br().y
- rec
.height
/ 2);
129 Point centerw
= new Point(rec
.br().x
- rec
.width
/ 2 - 15, rec
.br().y
- rec
.height
/ 2 - 20);
131 // Imgproc.putText(matOriginal, "hello" + (int) distance,
133 // Core.FONT_HERSHEY_PLAIN, 20, BLACK);
134 // Imgproc.putText(matOriginal, "hello2" + (int) azimuth,
136 // Core.FONT_HERSHEY_PLAIN, 20, BLACK);
138 System
.out
.println(distance
+ "m, " + azimuth
+ " degrees " + center
.x
+ ", " + center
.y
);
142 // Imgcodecs.imwrite("output.png", matOriginal);
149 * a nonnormalized angle
152 public static double normalize360(double angle
) {
153 while (angle
>= 360.0) {
156 while (angle
< 0.0) {