AprilTag detection

Author: devmanso

src/main/java/frc/robot/subsystems/vision/CamRIO.java

@@ -5,13 +5,20 @@
 package frc.robot.subsystems.vision;
 
 import java.io.Console;
+import java.util.HashSet;
 
 import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
 
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagDetector.QuadThresholdParameters;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.CvSink;
 import edu.wpi.first.cscore.CvSource;
 import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
@@ -50,40 +57,99 @@ public void camInit() {
     //init visionThread
     visionThread = new Thread(
       () -> {
-        // Get the USB camera from CameraServer
-        UsbCamera camera = CameraServer.startAutomaticCapture();
-
-        //set resolution/viewport
-        //lower resolutions == more fps, better RIO perfromance
-
-        camera.setResolution(640, 480);
-
-        //get the CvSink, this caputres/records video
-        CvSink cvSink = CameraServer.getVideo();
-
-        CvSource videoStream = CameraServer.putVideo(cameraVision.cameraName, 640, 480);
-
-        // We should re-use one mat instead of 2, beacuse mats are memory heavy
-        // 2 mats, one for regular images, one for grayscale (AprilTags)
-        Mat mat = new Mat();
-        Mat greymap = new Mat();
-
-        while (!Thread.interrupted()) {
-          // tell sink to grab video frames from the camera and put it into the source mat
-          if (cvSink.grabFrame(mat) == 0) {
-            videoStream.notifyError("There was an error grabbing a frame from: ");
-            videoStream.notifyError(cameraVision.cameraName);
-            videoStream.notifyError(" sink err");
-            videoStream.notifyError(cvSink.getError());
-            continue;
-          }
-          videoStream.putFrame(mat);
-        }
-
-        
-      });
-      visionThread.setDaemon(true);
-      visionThread.start();
+        var camera = CameraServer.startAutomaticCapture();
+
+              var cameraWidth = 640;
+              var cameraHeight = 480;
+
+              camera.setResolution(cameraWidth, cameraHeight);
+
+              var cvSink = CameraServer.getVideo();
+              var outputStream = CameraServer.putVideo("RioApriltags", cameraWidth, cameraHeight);
+
+              var mat = new Mat();
+              var grayMat = new Mat();
+
+              var pt0 = new Point();
+              var pt1 = new Point();
+              var pt2 = new Point();
+              var pt3 = new Point();
+              var center = new Point();
+              var red = new Scalar(0, 0, 255);
+              var green = new Scalar(0, 255, 0);
+
+              var aprilTagDetector = new AprilTagDetector();
+
+              var config = aprilTagDetector.getConfig();
+              config.quadSigma = 0.8f;
+              aprilTagDetector.setConfig(config);
+
+              var quadThreshParams = aprilTagDetector.getQuadThresholdParameters();
+              quadThreshParams.minClusterPixels = 250;
+              quadThreshParams.criticalAngle *= 5; // default is 10
+              quadThreshParams.maxLineFitMSE *= 1.5;
+              aprilTagDetector.setQuadThresholdParameters(quadThreshParams);
+
+              aprilTagDetector.addFamily("tag16h5");
+
+              var timer = new Timer();
+              timer.start();
+              var count = 0;
+
+              while (!Thread.interrupted()) {
+                if (cvSink.grabFrame(mat) == 0) {
+                  outputStream.notifyError(cvSink.getError());
+                  continue;
+                }
+
+                Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY);
+
+                var results = aprilTagDetector.detect(grayMat);
+
+                var set = new HashSet<>();
+
+                for (var result: results) {
+                  count += 1;
+                  pt0.x = result.getCornerX(0);
+                  pt1.x = result.getCornerX(1);
+                  pt2.x = result.getCornerX(2);
+                  pt3.x = result.getCornerX(3);
+
+                  pt0.y = result.getCornerY(0);
+                  pt1.y = result.getCornerY(1);
+                  pt2.y = result.getCornerY(2);
+                  pt3.y = result.getCornerY(3);
+
+                  center.x = result.getCenterX();
+                  center.y = result.getCenterY();
+
+                  set.add(result.getId());
+
+                  Imgproc.line(mat, pt0, pt1, red, 5);
+                  Imgproc.line(mat, pt1, pt2, red, 5);
+                  Imgproc.line(mat, pt2, pt3, red, 5);
+                  Imgproc.line(mat, pt3, pt0, red, 5);
+
+                  Imgproc.circle(mat, center, 4, green);
+                  Imgproc.putText(mat, String.valueOf(result.getId()), pt2, Imgproc.FONT_HERSHEY_SIMPLEX, 2, green, 7);
+
+                };
+
+                for (var id : set){
+                  System.out.println("Tag: " + String.valueOf(id));
+                }
+
+                if (timer.advanceIfElapsed(1.0)){
+                  System.out.println("detections per second: " + String.valueOf(count));
+                  count = 0;
+                }
+
+                outputStream.putFrame(mat);
+              }
+              aprilTagDetector.close();
+            });
+    visionThread.setDaemon(true);
+    visionThread.start();
   }
 
   @Override