Roi for tags

apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java

@@ -8,12 +8,12 @@ public final class DevMain {
   /** Main entry point. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
-    AprilTagDetector detector = new AprilTagDetector();
-    detector.addFamily("tag16h5");
-    AprilTagDetector.Config config = new AprilTagDetector.Config();
-    config.refineEdges = false;
-    detector.setConfig(config);
-    detector.close();
+    // AprilTagDetector detector = new AprilTagDetector();
+    // detector.addFamily("tag16h5");
+    // AprilTagDetector.Config config = new AprilTagDetector.Config();
+    // config.refineEdges = false;
+    // detector.setConfig(config);
+    // detector.close();
   }
 
   private DevMain() {}

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

@@ -57,6 +57,12 @@ public static class Config {
      */
     public boolean debug;
 
+    //ROI used for saving proccesing time
+    public int roiX = 0;
+    public int roiY = 0;
+    public int roiWidth = 16;
+    public int roiHeight = 16;
+
     /** Default constructor. */
     public Config() {}
 
@@ -71,20 +77,33 @@ public Config() {}
      *     gradients nearby.
      * @param decodeSharpening How much sharpening should be done to decoded images.
      * @param debug Debug mode.
+     * @param roiX X coordinate of the ROI for proccesing.
+     * @param roiY Y coordinate of the ROI for proccesing.
+     * @param roiWidth Width of the ROI for proccesing.
+     * @param roiHeight Height of the ROI for proccesing.
      */
     Config(
         int numThreads,
         float quadDecimate,
         float quadSigma,
         boolean refineEdges,
         double decodeSharpening,
-        boolean debug) {
+        boolean debug,
+        int roiX,
+        int roiY,
+        int roiWidth,
+        int roiHeight
+        ) {
       this.numThreads = numThreads;
       this.quadDecimate = quadDecimate;
       this.quadSigma = quadSigma;
       this.refineEdges = refineEdges;
       this.decodeSharpening = decodeSharpening;
       this.debug = debug;
+      // this.roiX = roiX;
+      // this.roiY = roiY;
+      // this.roiWidth = roiWidth;
+      // this.roiHeight = roiHeight;
     }
 
     @Override
@@ -94,7 +113,11 @@ public int hashCode() {
           + Float.hashCode(quadSigma)
           + Boolean.hashCode(refineEdges)
           + Double.hashCode(decodeSharpening)
-          + Boolean.hashCode(debug);
+          + Boolean.hashCode(debug)
+          + roiX
+          + roiY
+          + roiWidth
+          + roiHeight;
     }
 
     @Override
@@ -105,7 +128,11 @@ public boolean equals(Object obj) {
           && quadSigma == other.quadSigma
           && refineEdges == other.refineEdges
           && decodeSharpening == other.decodeSharpening
-          && debug == other.debug;
+          && debug == other.debug
+          && roiX == other.roiX
+          && roiY == other.roiY
+          && roiWidth == other.roiWidth
+          && roiHeight == other.roiHeight;
     }
   }
 

apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java

@@ -219,6 +219,7 @@ public static native Transform3d estimatePose(
    */
   public static native void generate36h11AprilTagImage(RawFrame frameObj, long frame, int id);
 
+
   /** Utility class. */
   private AprilTagJNI() {}
 }

apriltag/src/main/native/cpp/AprilTagDetector.cpp

@@ -62,8 +62,13 @@ void AprilTagDetector::SetConfig(const Config& config) {
   impl.refine_edges = config.refineEdges;
   impl.decode_sharpening = config.decodeSharpening;
   impl.debug = config.debug;
+  impl.roiX = config.roiX;
+  impl.roiY = config.roiY;
+  impl.roiWidth = config.roiWidth;
+  impl.roiHeight = config.roiHeight;
 }
 
+
 AprilTagDetector::Config AprilTagDetector::GetConfig() const {
   auto& impl = *static_cast<apriltag_detector_t*>(m_impl);
   return {
@@ -73,6 +78,10 @@ AprilTagDetector::Config AprilTagDetector::GetConfig() const {
       .refineEdges = impl.refine_edges,
       .decodeSharpening = impl.decode_sharpening,
       .debug = impl.debug,
+      .roiX = impl.roiX, 
+      .roiY = impl.roiY,
+      .roiWidth = impl.roiWidth,
+      .roiHeight = impl.roiHeight
   };
 }
 
@@ -86,6 +95,7 @@ void AprilTagDetector::SetQuadThresholdParameters(
   qtp.max_line_fit_mse = params.maxLineFitMSE;
   qtp.min_white_black_diff = params.minWhiteBlackDiff;
   qtp.deglitch = params.deglitch;
+
 
   m_qtpCriticalAngle = params.criticalAngle;
 }

apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp

@@ -117,6 +117,10 @@ static AprilTagDetector::Config FromJavaDetectorConfig(JNIEnv* env,
   FIELD(refineEdges, "Z");
   FIELD(decodeSharpening, "D");
   FIELD(debug, "Z");
+  FIELD(roiX, "I");
+  FIELD(roiY, "I");
+  FIELD(roiWidth, "I");
+  FIELD(roiHeight, "I");
 
 #undef FIELD
 
@@ -130,6 +134,10 @@ static AprilTagDetector::Config FromJavaDetectorConfig(JNIEnv* env,
       FIELD(bool, Boolean, refineEdges),
       FIELD(double, Double, decodeSharpening),
       FIELD(bool, Boolean, debug),
+      FIELD(int, Int, roiX),
+      FIELD(int, Int, roiY),
+      FIELD(int, Int, roiWidth),
+      FIELD(int, Int, roiHeight)
   };
 
 #undef GET
@@ -225,7 +233,7 @@ static jobjectArray MakeJObject(JNIEnv* env,
 static jobject MakeJObject(JNIEnv* env,
                            const AprilTagDetector::Config& config) {
   static jmethodID constructor =
-      env->GetMethodID(detectorConfigCls, "<init>", "(IFFZDZ)V");
+      env->GetMethodID(detectorConfigCls, "<init>", "(IFFZDZIIII)V");
   if (!constructor) {
     return nullptr;
   }
@@ -236,7 +244,12 @@ static jobject MakeJObject(JNIEnv* env,
                         static_cast<jfloat>(config.quadSigma),
                         static_cast<jboolean>(config.refineEdges),
                         static_cast<jdouble>(config.decodeSharpening),
-                        static_cast<jboolean>(config.debug));
+                        static_cast<jboolean>(config.debug),
+                        static_cast<jint>(config.roiX),
+                        static_cast<jint>(config.roiY),
+                        static_cast<jint>(config.roiWidth),
+                        static_cast<jint>(config.roiHeight)
+                        );
 }
 
 static jobject MakeJObject(
@@ -631,4 +644,5 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage
   wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
 }
 
+
 }  // extern "C"

apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h

@@ -76,6 +76,12 @@ class WPILIB_DLLEXPORT AprilTagDetector {
      * such as the RoboRIO. Default is disabled (false).
      */
     bool debug = false;
+
+    //ROI used for saving proccesing time
+    int roiX = 0;
+    int roiY = 0;
+    int roiWidth = 16;
+    int roiHeight = 16;
   };
 
   /** Quad threshold parameters. */

apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h

@@ -75,6 +75,8 @@ class WPILIB_DLLEXPORT AprilTagPoseEstimator {
    * @return Pose estimate
    */
   Transform3d EstimateHomography(std::span<const double, 9> homography) const;
+
+
 
   /**
    * Estimates the pose of the tag. This returns one or two possible poses for

apriltag/src/main/native/thirdparty/apriltag/include/apriltag.h

@@ -188,6 +188,9 @@ struct apriltag_detector
 
     // Used for thread safety.
     pthread_mutex_t mutex;
+
+    //ROI used for saving proccesing time
+    int roiX, roiY, roiWidth, roiHeight;
 };
 
 // Represents the detection of a tag. These are returned to the user

apriltag/src/main/native/thirdparty/apriltag/src/apriltag.c

@@ -1053,9 +1053,10 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
                 // SHARPEN the image by subtracting the low frequency components.
                 image_u8_t *orig = image_u8_copy(quad_im);
                 image_u8_gaussian_blur(quad_im, sigma, ksz);
-
-                for (int y = 0; y < orig->height; y++) {
-                    for (int x = 0; x < orig->width; x++) {
+
+                //TO-DO add check of roiX and roiY
+                for (int y = td->roiY; y < td->roiHeight; y++) {
+                    for (int x = td->roiX; x < td->roiWidth; x++) {
                         int vorig = orig->buf[y*orig->stride + x];
                         int vblur = quad_im->buf[y*quad_im->stride + x];
 

apriltag/src/main/native/thirdparty/apriltag/src/apriltag_quad_thresh.c

@@ -798,7 +798,7 @@ int fit_quad(
         }
     }
 
-    if ((xmax - xmin)*(ymax - ymin) < tag_width) {
+    if ((xmax - xmin)*(ymax - ymin) < tag_width || xmin < td->roiX || ymin < td->roiY || ymax > td->roiY + td->roiHeight || xmax > td->roiX + td->roiWidth) {
         return 0;
     }
 

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

@@ -145,16 +145,21 @@ void testDecodeCropped() {
     }
 
     // Pre-knowledge -- the tag is within this ROI of this particular test image
-    var cropped = image.submat(100, 400, 220, 570);
+
+    var config = detector.getConfig();
+      config.roiHeight = image.rows();
+      config.roiWidth = image.cols();
+      detector.setConfig(config);
+
 
     try {
-      AprilTagDetection[] results = detector.detect(cropped);
+      AprilTagDetection[] results = detector.detect(image);
       assertEquals(1, results.length);
       assertEquals("tag36h11", results[0].getFamily());
       assertEquals(1, results[0].getId());
       assertEquals(0, results[0].getHamming());
     } finally {
-      cropped.release();
+
       image.release();
     }
   }
@@ -172,6 +177,11 @@ void testDecodeAndPose() {
       return;
     }
     try {
+      var config = detector.getConfig();
+      config.roiHeight = image.rows();
+      config.roiWidth = image.cols();
+      detector.setConfig(config);
+
       AprilTagDetection[] results = detector.detect(image);
       assertEquals(1, results.length);
       assertEquals("tag36h11", results[0].getFamily());
@@ -205,6 +215,12 @@ void testPoseRotatedX() {
       return;
     }
     try {
+
+      var config = detector.getConfig();
+      config.roiHeight = image.rows();
+      config.roiWidth = image.cols();
+      detector.setConfig(config);
+
       AprilTagDetection[] results = detector.detect(image);
       assertEquals(1, results.length);
 
@@ -238,6 +254,11 @@ void testPoseRotatedY() {
       return;
     }
     try {
+      var config = detector.getConfig();
+      config.roiHeight = image.rows();
+      config.roiWidth = image.cols();
+      detector.setConfig(config);
+
       AprilTagDetection[] results = detector.detect(image);
       assertEquals(1, results.length);
 
@@ -268,6 +289,11 @@ void testPoseStraightOn() {
       return;
     }
     try {
+      var config = detector.getConfig();
+      config.roiHeight = image.rows();
+      config.roiWidth = image.cols();
+      detector.setConfig(config);
+
       AprilTagDetection[] results = detector.detect(image);
       assertEquals(1, results.length);
 

upstream_utils/apriltag_patches/0001-apriltag_pose.c-Set-NULL-when-second-solution-could-.patch

@@ -1,8 +1,8 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <[email protected]>
 Date: Sun, 4 Dec 2022 11:01:56 -0800
-Subject: [PATCH 1/8] apriltag_pose.c: Set NULL when second solution could not
- be determined
+Subject: [PATCH 01/17] apriltag_pose.c: Set NULL when second solution could
+ not be determined
 
 ---
  apriltag_pose.c | 1 +

upstream_utils/apriltag_patches/0002-Avoid-unused-variable-warnings-in-release-builds.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <[email protected]>
 Date: Sun, 4 Dec 2022 11:42:13 -0800
-Subject: [PATCH 2/8] Avoid unused variable warnings in release builds
+Subject: [PATCH 02/17] Avoid unused variable warnings in release builds
 
 ---
  common/matd.c        | 4 +++-

upstream_utils/apriltag_patches/0003-Make-orthogonal_iteration-exit-early-upon-convergenc.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <[email protected]>
 Date: Tue, 10 Jan 2023 18:36:36 -0800
-Subject: [PATCH 3/8] Make orthogonal_iteration() exit early upon convergence
+Subject: [PATCH 03/17] Make orthogonal_iteration() exit early upon convergence
 
 The current approach wastes iterations doing no work. Exiting early can
 give lower latencies and higher FPS.

upstream_utils/apriltag_patches/0004-Fix-signed-left-shift-warning.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <[email protected]>
 Date: Wed, 19 Jul 2023 20:48:21 -0700
-Subject: [PATCH 4/8] Fix signed left shift warning
+Subject: [PATCH 04/17] Fix signed left shift warning
 
 ---
  common/pjpeg.c | 4 ++--

upstream_utils/apriltag_patches/0005-Avoid-incompatible-pointer-warning.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <[email protected]>
 Date: Wed, 19 Jul 2023 21:28:43 -0700
-Subject: [PATCH 5/8] Avoid incompatible pointer warning
+Subject: [PATCH 05/17] Avoid incompatible pointer warning
 
 ---
  common/getopt.c | 3 ++-

upstream_utils/apriltag_patches/0006-Remove-calls-to-postscript_image.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Tyler Veness <[email protected]>
 Date: Fri, 19 Jul 2024 21:45:29 -0700
-Subject: [PATCH 6/8] Remove calls to postscript_image()
+Subject: [PATCH 06/17] Remove calls to postscript_image()
 
 ---
  apriltag.c             | 5 -----

upstream_utils/apriltag_patches/0007-Fix-clang-16-warnings.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Peter Johnson <[email protected]>
 Date: Thu, 29 Jun 2023 22:14:05 -0700
-Subject: [PATCH 7/8] Fix clang 16 warnings
+Subject: [PATCH 07/17] Fix clang 16 warnings
 
 ---
  apriltag.c              | 2 +-

upstream_utils/apriltag_patches/0008-Remove-GCC-diagnostic-pragmas-on-windows.patch

@@ -1,7 +1,7 @@
 From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
 From: Ryan Blue <[email protected]>
 Date: Fri, 23 Aug 2024 02:50:24 -0400
-Subject: [PATCH 8/8] Remove GCC diagnostic pragmas on windows
+Subject: [PATCH 08/17] Remove GCC diagnostic pragmas on windows
 
 ---
  common/pthreads_cross.c | 3 ---