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
@@ -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

@@ -379,7 +379,10 @@ apriltag_detector_t *apriltag_detector_create(void)
 
     // NB: defer initialization of td->wp so that the user can
     // override td->nthreads.
-
+    td->roiX = 0;
+    td->roiY = 0;
+    td->roiWidth = 16;
+    td->roiHeight = 16;
     return td;
 }
 
@@ -1053,9 +1056,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/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java

@@ -145,16 +145,23 @@ void testDecodeCropped() {
     }
 
     // Pre-knowledge -- the tag is within this ROI of this particular test image
-    var cropped = image.submat(100, 400, 220, 570);
+
+    var configs = detector.getConfig();
+    configs.roiX = 100;
+    configs.roiY = 400;
+    configs.roiWidth = 220;
+    configs.roiHeight = 570;
+    detector.setConfig(configs);
+
 
     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();
     }
   }

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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] Remove GCC diagnostic pragmas on windows
 
 ---
  common/pthreads_cross.c | 3 ---

upstream_utils/apriltag_patches/0009-ROI-for-detection.patch

@@ -0,0 +1,39 @@
+From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
+From: GGreenix <[email protected]>
+Date: Tue, 31 Dec 2024 20:05:39 +0200
+Subject: [PATCH 09/12] ROI for detection
+
+---
+ apriltag.c | 4 ++--
+ apriltag.h | 3 +++
+ 2 files changed, 5 insertions(+), 2 deletions(-)
+
+diff --git a/apriltag.c b/apriltag.c
+index b7e24c4bc279643f478d810d494345216be991f1..3654ab376a62bf744d99bd89d45f97620a258495 100644
+--- a/apriltag.c
++++ b/apriltag.c
+@@ -1054,8 +1054,8 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
+                 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++) {
++                for (int y = td->roi_y_coor; y < td->roi_height; y++) {
++                    for (int x = td->roi_x_coor; x < td->roi_width; x++) {
+                         int vorig = orig->buf[y*orig->stride + x];
+                         int vblur = quad_im->buf[y*quad_im->stride + x];
+
+diff --git a/apriltag.h b/apriltag.h
+index 895b3459b8a84064989378fe533fd676964a1687..6ac15fae3c4700e25aa7a5f34831079e66abde5c 100644
+--- a/apriltag.h
++++ b/apriltag.h
+@@ -188,6 +188,9 @@ struct apriltag_detector
+
+     // Used for thread safety.
+     pthread_mutex_t mutex;
++
++    //ROI used for saving proccesing time
++    double roi_x_coor, roi_y_coor, roi_width, roi_height;
+ };
+
+ // Represents the detection of a tag. These are returned to the user