[apriltag] Make AprilTagDetector.detect() use RawFrame instead of OpenCV Mat

Author: calcmogul

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java

@@ -5,7 +5,7 @@
 package edu.wpi.first.apriltag;
 
 import edu.wpi.first.apriltag.jni.AprilTagJNI;
-import org.opencv.core.Mat;
+import edu.wpi.first.util.RawFrame;
 
 /**
  * An AprilTag detector engine. This is expensive to set up and tear down, so most use cases should
@@ -293,11 +293,12 @@ public void clearFamilies() {
    *
    * <p>The image must be grayscale.
    *
-   * @param img 8-bit OpenCV Mat image
+   * @param frame The frame object containing an 8-bit image.
    * @return Results (array of AprilTagDetection)
    */
-  public AprilTagDetection[] detect(Mat img) {
-    return AprilTagJNI.detect(m_native, img.cols(), img.rows(), (int) img.step1(), img.dataAddr());
+  public AprilTagDetection[] detect(RawFrame frame) {
+    return AprilTagJNI.detect(
+        m_native, frame.getWidth(), frame.getHeight(), frame.getWidth(), frame.getDataPtr());
   }
 
   private long m_native;

apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java

@@ -10,6 +10,8 @@
 
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.PixelFormat;
+import edu.wpi.first.util.RawFrame;
 import edu.wpi.first.util.RuntimeLoader;
 import java.io.ByteArrayOutputStream;
 import java.io.IOException;
@@ -132,7 +134,7 @@ public Mat loadImage(String resource) throws IOException {
   }
 
   @Test
-  void testDecodeCropped() {
+  void testDecodeAndPose() {
     detector.addFamily("tag16h5");
     detector.addFamily("tag36h11");
 
@@ -144,35 +146,14 @@ void testDecodeCropped() {
       return;
     }
 
-    // Pre-knowledge -- the tag is within this ROI of this particular test image
-    var cropped = image.submat(100, 400, 220, 570);
-
-    try {
-      AprilTagDetection[] results = detector.detect(cropped);
-      assertEquals(1, results.length);
-      assertEquals("tag36h11", results[0].getFamily());
-      assertEquals(1, results[0].getId());
-      assertEquals(0, results[0].getHamming());
-    } finally {
-      cropped.release();
-      image.release();
-    }
-  }
+    var frameBytes = new byte[image.width() * image.height()];
+    image.get(0, 0, frameBytes);
 
-  @Test
-  void testDecodeAndPose() {
-    detector.addFamily("tag16h5");
-    detector.addFamily("tag36h11");
+    var frame = new RawFrame();
+    frame.setData(frameBytes, image.width(), image.height(), 1, PixelFormat.kGray);
 
-    Mat image;
     try {
-      image = loadImage("tag1_640_480.jpg");
-    } catch (IOException ex) {
-      fail(ex);
-      return;
-    }
-    try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
       assertEquals("tag36h11", results[0].getFamily());
       assertEquals(1, results[0].getId());
@@ -204,8 +185,15 @@ void testPoseRotatedX() {
       fail(ex);
       return;
     }
+
+    var frameBytes = new byte[image.width() * image.height()];
+    image.get(0, 0, frameBytes);
+
+    var frame = new RawFrame();
+    frame.setData(frameBytes, image.width(), image.height(), 1, PixelFormat.kGray);
+
     try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
 
       var estimator =
@@ -237,8 +225,15 @@ void testPoseRotatedY() {
       fail(ex);
       return;
     }
+
+    var frameBytes = new byte[image.width() * image.height()];
+    image.get(0, 0, frameBytes);
+
+    var frame = new RawFrame();
+    frame.setData(frameBytes, image.width(), image.height(), 1, PixelFormat.kGray);
+
     try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
 
       var estimator =
@@ -267,8 +262,15 @@ void testPoseStraightOn() {
       fail(ex);
       return;
     }
+
+    var frameBytes = new byte[image.width() * image.height()];
+    image.get(0, 0, frameBytes);
+
+    var frame = new RawFrame();
+    frame.setData(frameBytes, image.width(), image.height(), 1, PixelFormat.kGray);
+
     try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
 
       var estimator =

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java

@@ -16,6 +16,8 @@
 import edu.wpi.first.networktables.IntegerArrayPublisher;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.PixelFormat;
+import edu.wpi.first.util.RawFrame;
 import edu.wpi.first.wpilibj.TimedRobot;
 import java.util.ArrayList;
 import org.opencv.core.Mat;
@@ -65,6 +67,9 @@ void apriltagVisionThreadProc() {
     var mat = new Mat();
     var grayMat = new Mat();
 
+    var frameBytes = new byte[640 * 480];
+    var frame = new RawFrame();
+
     // Instantiate once
     ArrayList<Long> tags = new ArrayList<>();
     var outlineColor = new Scalar(0, 255, 0);
@@ -89,7 +94,10 @@ void apriltagVisionThreadProc() {
 
       Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY);
 
-      AprilTagDetection[] detections = detector.detect(grayMat);
+      grayMat.get(0, 0, frameBytes);
+      frame.setData(frameBytes, grayMat.width(), grayMat.height(), 1, PixelFormat.kGray);
+
+      AprilTagDetection[] detections = detector.detect(frame);
 
       // have not seen any tags yet
       tags.clear();

wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java

@@ -88,6 +88,22 @@ public void setData(ByteBuffer data, int width, int height, int stride, PixelFor
         m_nativeObj, data, data.limit(), width, height, stride, pixelFormat.getValue());
   }
 
+  /**
+   * Set frame data.
+   *
+   * @param data A byte array containing the frame data.
+   * @param width The width of the frame, in pixels
+   * @param height The height of the frame, in pixels
+   * @param stride The number of bytes in each row of image data
+   * @param pixelFormat The PixelFormat of the frame
+   */
+  public void setData(byte[] data, int width, int height, int stride, PixelFormat pixelFormat) {
+    var dataByteBuffer = ByteBuffer.allocateDirect(width * height * stride);
+    dataByteBuffer.put(data);
+
+    setData(dataByteBuffer, width, height, stride, pixelFormat);
+  }
+
   /**
    * Call to set frame information.
    *