[apriltag] Make AprilTagDetector.detect() use RawFrame instead of Ope…

…nCV Mat
wpilibsuite · Oct 10, 2024 · 34f6e98 · 34f6e98
1 parent f150b36
commit 34f6e98
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 9 deletions.
diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java b/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(), img.cols(), img.dataAddr());
+  public AprilTagDetection[] detect(RawFrame frame) {
+    return AprilTagJNI.detect(
+        m_native, frame.getWidth(), frame.getHeight(), frame.getWidth(), frame.getDataPtr());
   }
 
   private long m_native;

diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java
@@ -10,10 +10,13 @@
 
 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;
 import java.io.InputStream;
+import java.nio.ByteBuffer;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.BeforeEach;
@@ -143,8 +146,12 @@ void testDecodeAndPose() {
       fail(ex);
       return;
     }
+    var frame = new RawFrame();
+    var frameByteBuffer = ByteBuffer.allocateDirect(image.width() * image.height());
+    image.get(0, 0, frameByteBuffer.array());
+    frame.setData(frameByteBuffer, image.width(), image.height(), 1, PixelFormat.kGray);
     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());
@@ -176,8 +183,12 @@ void testPoseRotatedX() {
       fail(ex);
       return;
     }
+    var frame = new RawFrame();
+    var frameByteBuffer = ByteBuffer.allocateDirect(image.width() * image.height());
+    image.get(0, 0, frameByteBuffer.array());
+    frame.setData(frameByteBuffer, image.width(), image.height(), 1, PixelFormat.kGray);
     try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
 
       var estimator =
@@ -209,8 +220,12 @@ void testPoseRotatedY() {
       fail(ex);
       return;
     }
+    var frame = new RawFrame();
+    var frameByteBuffer = ByteBuffer.allocateDirect(image.width() * image.height());
+    image.get(0, 0, frameByteBuffer.array());
+    frame.setData(frameByteBuffer, image.width(), image.height(), 1, PixelFormat.kGray);
     try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
 
       var estimator =
@@ -239,8 +254,12 @@ void testPoseStraightOn() {
       fail(ex);
       return;
     }
+    var frame = new RawFrame();
+    var frameByteBuffer = ByteBuffer.allocateDirect(image.width() * image.height());
+    image.get(0, 0, frameByteBuffer.array());
+    frame.setData(frameByteBuffer, image.width(), image.height(), 1, PixelFormat.kGray);
     try {
-      AprilTagDetection[] results = detector.detect(image);
+      AprilTagDetection[] results = detector.detect(frame);
       assertEquals(1, results.length);
 
       var estimator =

diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/apriltagsvision/Robot.java
@@ -16,7 +16,10 @@
 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.nio.ByteBuffer;
 import java.util.ArrayList;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
@@ -65,6 +68,9 @@ void apriltagVisionThreadProc() {
     var mat = new Mat();
     var grayMat = new Mat();
 
+    var frame = new RawFrame();
+    var frameByteBuffer = ByteBuffer.allocateDirect(640 * 480);
+
     // Instantiate once
     ArrayList<Long> tags = new ArrayList<>();
     var outlineColor = new Scalar(0, 255, 0);
@@ -89,7 +95,10 @@ void apriltagVisionThreadProc() {
 
       Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY);
 
-      AprilTagDetection[] detections = detector.detect(grayMat);
+      grayMat.get(0, 0, frameByteBuffer.array());
+      frame.setData(frameByteBuffer, grayMat.width(), grayMat.height(), 1, PixelFormat.kGray);
+
+      AprilTagDetection[] detections = detector.detect(frame);
 
       // have not seen any tags yet
       tags.clear();