Changes sim to use 36h11 tags (#1056)

Fixes #1041

---------

Co-authored-by: Drew Williams <DrewW@iARx.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -28,11 +28,11 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.CvSource;
-import edu.wpi.first.cscore.VideoMode.PixelFormat;
 import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.util.PixelFormat;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.ArrayList;
 import java.util.List;
@@ -464,7 +464,7 @@ public class PhotonCameraSim implements AutoCloseable {
                 var corn = pair.getSecond();
 
                 if (tgt.fiducialID >= 0) { // apriltags
-                    VideoSimUtil.warp16h5TagImage(tgt.fiducialID, corn, true, videoSimFrameRaw);
+                    VideoSimUtil.warp36h11TagImage(tgt.fiducialID, corn, true, videoSimFrameRaw);
                 } else if (!tgt.getModel().isSpherical) { // non-spherical targets
                     var contour = corn;
                     if (!tgt.getModel()
@@ -529,7 +529,7 @@ public class PhotonCameraSim implements AutoCloseable {
                             prop.getDistCoeffs(),
                             detectableTgts,
                             tagLayout,
-                            TargetModel.kAprilTag16h5);
+                            TargetModel.kAprilTag36h11);
             multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
         }
 

photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java

@@ -24,18 +24,17 @@
 
 package org.photonvision.simulation;
 
+import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.cscore.CvSource;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
-import java.awt.image.BufferedImage;
-import java.io.IOException;
+import edu.wpi.first.util.RawFrame;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
-import javax.imageio.ImageIO;
 import org.opencv.core.Core;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
@@ -45,21 +44,17 @@ import org.opencv.core.Point;
 import org.opencv.core.Rect;
 import org.opencv.core.Scalar;
 import org.opencv.core.Size;
-import org.opencv.imgcodecs.Imgcodecs;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.RotTrlTransform3d;
 
 public class VideoSimUtil {
-    public static final String kLocalTagImagesPath = "./src/main/resources/images/apriltags/";
-    public static final String kResourceTagImagesPath = "/images/apriltags/";
-    public static final String kTag16h5ImageName = "tag16_05_00000";
-    public static final int kNumTags16h5 = 30;
+    public static final int kNumTags36h11 = 30;
 
-    // All 16h5 tag images
-    private static final Map<Integer, Mat> kTag16h5Images = new HashMap<>();
-    // Points corresponding to marker(black square) corners of 8x8 16h5 tag images
-    public static final Point[] kTag16h5MarkerPts;
+    // All 36h11 tag images
+    private static final Map<Integer, Mat> kTag36h11Images = new HashMap<>();
+    // Points corresponding to marker(black square) corners of 10x10 36h11 tag images
+    public static final Point[] kTag36h11MarkerPts;
 
     // field dimensions for wireframe
     private static double fieldLength = 16.54175;
@@ -68,13 +63,13 @@ public class VideoSimUtil {
     static {
         OpenCVHelp.forceLoadOpenCV();
 
-        // create Mats of 8x8 apriltag images
-        for (int i = 0; i < VideoSimUtil.kNumTags16h5; i++) {
-            Mat tagImage = VideoSimUtil.get16h5TagImage(i);
-            kTag16h5Images.put(i, tagImage);
+        // create Mats of 10x10 apriltag images
+        for (int i = 0; i < VideoSimUtil.kNumTags36h11; i++) {
+            Mat tagImage = VideoSimUtil.get36h11TagImage(i);
+            kTag36h11Images.put(i, tagImage);
         }
 
-        kTag16h5MarkerPts = get16h5MarkerPts();
+        kTag36h11MarkerPts = get36h11MarkerPts();
     }
 
     /** Updates the properties of this CvSource video stream with the given camera properties. */
@@ -100,69 +95,43 @@ public class VideoSimUtil {
     }
 
     /**
-     * Gets the 8x8 (grayscale) image of a specific 16h5 AprilTag.
+     * Gets the 10x10 (grayscale) image of a specific 36h11 AprilTag.
      *
      * @param id The fiducial id of the desired tag
      */
-    public static Mat get16h5TagImage(int id) {
-        String name = kTag16h5ImageName;
-        String idString = String.valueOf(id);
-        name = name.substring(0, name.length() - idString.length()) + idString;
-
-        var resource = VideoSimUtil.class.getResource(kResourceTagImagesPath + name + ".png");
-
-        Mat result = new Mat();
-        // reading jar file
-        if (resource != null && resource.getPath().startsWith("file")) {
-            BufferedImage buf;
-            try {
-                buf = ImageIO.read(resource);
-            } catch (IOException e) {
-                System.err.println("Couldn't read tag image!");
-                return result;
-            }
-
-            result = new Mat(buf.getHeight(), buf.getWidth(), CvType.CV_8UC1);
-
-            byte[] px = new byte[1];
-            for (int y = 0; y < result.height(); y++) {
-                for (int x = 0; x < result.width(); x++) {
-                    px[0] = (byte) (buf.getRGB(x, y) & 0xFF);
-                    result.put(y, x, px);
-                }
-            }
-        }
-        // local IDE tests
-        else result = Imgcodecs.imread(kLocalTagImagesPath + name + ".png", Imgcodecs.IMREAD_GRAYSCALE);
+    public static Mat get36h11TagImage(int id) {
+        RawFrame frame = AprilTag.generate36h11AprilTagImage(id);
+        Mat result = new Mat(10, 10, CvType.CV_8UC1, frame.getData(), frame.getStride()).clone();
+        frame.close();
         return result;
     }
 
     /** Gets the points representing the marker(black square) corners. */
-    public static Point[] get16h5MarkerPts() {
-        return get16h5MarkerPts(1);
+    public static Point[] get36h11MarkerPts() {
+        return get36h11MarkerPts(1);
     }
 
     /**
      * Gets the points representing the marker(black square) corners.
      *
-     * @param scale The scale of the tag image (8*scale x 8*scale image)
+     * @param scale The scale of the tag image (10*scale x 10*scale image)
      */
-    public static Point[] get16h5MarkerPts(int scale) {
-        var roi16h5 = new Rect(new Point(1, 1), new Size(6, 6));
-        roi16h5.x *= scale;
-        roi16h5.y *= scale;
-        roi16h5.width *= scale;
-        roi16h5.height *= scale;
-        var pts = getImageCorners(roi16h5.size());
+    public static Point[] get36h11MarkerPts(int scale) {
+        var roi36h11 = new Rect(new Point(1, 1), new Size(8, 8));
+        roi36h11.x *= scale;
+        roi36h11.y *= scale;
+        roi36h11.width *= scale;
+        roi36h11.height *= scale;
+        var pts = getImageCorners(roi36h11.size());
         for (int i = 0; i < pts.length; i++) {
             var pt = pts[i];
-            pts[i] = new Point(roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y);
+            pts[i] = new Point(roi36h11.tl().x + pt.x, roi36h11.tl().y + pt.y);
         }
         return pts;
     }
 
     /**
-     * Warps the image of a specific 16h5 AprilTag onto the destination image at the given points.
+     * Warps the image of a specific 36h11 AprilTag onto the destination image at the given points.
      *
      * @param tagId The id of the specific tag to warp onto the destination image
      * @param dstPoints Points(4) in destination image where the tag marker(black square) corners
@@ -172,11 +141,11 @@ public class VideoSimUtil {
      *     is desired or target detection is being done on the stream, but can hurt performance.
      * @param destination The destination image to place the warped tag image onto.
      */
-    public static void warp16h5TagImage(
+    public static void warp36h11TagImage(
             int tagId, Point[] dstPoints, boolean antialiasing, Mat destination) {
-        Mat tagImage = kTag16h5Images.get(tagId);
+        Mat tagImage = kTag36h11Images.get(tagId);
         if (tagImage == null || tagImage.empty()) return;
-        var tagPoints = new MatOfPoint2f(kTag16h5MarkerPts);
+        var tagPoints = new MatOfPoint2f(kTag36h11MarkerPts);
         // points of tag image corners
         var tagImageCorners = new MatOfPoint2f(getImageCorners(tagImage.size()));
         var dstPointMat = new MatOfPoint2f(dstPoints);
@@ -206,7 +175,7 @@ public class VideoSimUtil {
         */
         int supersampling = 6;
         supersampling = (int) Math.ceil(supersampling / warpedTagUpscale);
-        supersampling = Math.max(Math.min(supersampling, 8), 1);
+        supersampling = Math.max(Math.min(supersampling, 10), 1);
 
         Mat scaledTagImage = new Mat();
         if (warpedTagUpscale > 2.0) {
@@ -216,7 +185,7 @@ public class VideoSimUtil {
             scaleFactor *= supersampling;
             Imgproc.resize(
                     tagImage, scaledTagImage, new Size(), scaleFactor, scaleFactor, Imgproc.INTER_NEAREST);
-            tagPoints.fromArray(get16h5MarkerPts(scaleFactor));
+            tagPoints.fromArray(get36h11MarkerPts(scaleFactor));
         } else tagImage.assignTo(scaledTagImage);
 
         // constrain the bounding rect inside of the destination image

photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java

@@ -256,7 +256,7 @@ public class VisionSystemSim {
                     "apriltag",
                     new VisionTargetSim(
                             tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
-                            TargetModel.kAprilTag16h5,
+                            TargetModel.kAprilTag36h11,
                             tag.ID));
         }
     }

photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h

@@ -51,22 +51,24 @@ int sgn(T val) {
 
 namespace photon {
 namespace VideoSimUtil {
-static constexpr int kNumTags16h5 = 30;
+static constexpr int kNumTags36h11 = 30;
 
 static constexpr units::meter_t fieldLength{16.54175_m};
 static constexpr units::meter_t fieldWidth{8.0137_m};
 
-static cv::Mat Get16h5TagImage(int id) {
-  wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id);
-  cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data};
-  cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone();
+static cv::Mat Get36h11TagImage(int id) {
+  wpi::RawFrame frame;
+  frc::AprilTag::Generate36h11AprilTagImage(&frame, id);
+  cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data,
+                      static_cast<size_t>(frame.stride)};
+  cv::Mat markerClone = markerImage.clone();
   return markerClone;
 }
 
 static std::unordered_map<int, cv::Mat> LoadAprilTagImages() {
   std::unordered_map<int, cv::Mat> retVal{};
-  for (int i = 0; i < kNumTags16h5; i++) {
-    cv::Mat tagImage = Get16h5TagImage(i);
+  for (int i = 0; i < kNumTags36h11; i++) {
+    cv::Mat tagImage = Get36h11TagImage(i);
     retVal[i] = tagImage;
   }
   return retVal;
@@ -81,27 +83,27 @@ static std::vector<cv::Point2f> GetImageCorners(const cv::Size& size) {
   return retVal;
 }
 
-static std::vector<cv::Point2f> Get16h5MarkerPts(int scale) {
-  cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}};
-  roi16h5.x *= scale;
-  roi16h5.y *= scale;
-  roi16h5.width *= scale;
-  roi16h5.height *= scale;
-  std::vector<cv::Point2f> pts = GetImageCorners(roi16h5.size());
+static std::vector<cv::Point2f> Get36h11MarkerPts(int scale) {
+  cv::Rect2f roi36h11{cv::Point2f{1, 1}, cv::Point2f{8, 8}};
+  roi36h11.x *= scale;
+  roi36h11.y *= scale;
+  roi36h11.width *= scale;
+  roi36h11.height *= scale;
+  std::vector<cv::Point2f> pts = GetImageCorners(roi36h11.size());
   for (size_t i = 0; i < pts.size(); i++) {
     cv::Point2f pt = pts[i];
-    pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y};
+    pts[i] = cv::Point2f{roi36h11.tl().x + pt.x, roi36h11.tl().y + pt.y};
   }
   return pts;
 }
 
-static std::vector<cv::Point2f> Get16h5MarkerPts() {
-  return Get16h5MarkerPts(1);
+static std::vector<cv::Point2f> Get36h11MarkerPts() {
+  return Get36h11MarkerPts(1);
 }
 
-static const std::unordered_map<int, cv::Mat> kTag16h5Images =
+static const std::unordered_map<int, cv::Mat> kTag36h11Images =
     LoadAprilTagImages();
-static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
+static const std::vector<cv::Point2f> kTag36h11MarkPts = Get36h11MarkerPts();
 
 [[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video,
                                              const SimCameraProperties& prop) {
@@ -112,11 +114,11 @@ static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
 [[maybe_unused]] static void Warp165h5TagImage(
     int tagId, const std::vector<cv::Point2f>& dstPoints, bool antialiasing,
     cv::Mat& destination) {
-  if (!kTag16h5Images.contains(tagId)) {
+  if (!kTag36h11Images.contains(tagId)) {
     return;
   }
-  cv::Mat tagImage = kTag16h5Images.at(tagId);
-  std::vector<cv::Point2f> tagPoints{kTag16h5MarkPts};
+  cv::Mat tagImage = kTag36h11Images.at(tagId);
+  std::vector<cv::Point2f> tagPoints{kTag36h11MarkPts};
   std::vector<cv::Point2f> tagImageCorners{GetImageCorners(tagImage.size())};
   std::vector<cv::Point2f> dstPointMat = dstPoints;
   cv::Rect boundingRect = cv::boundingRect(dstPointMat);
@@ -132,7 +134,7 @@ static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
 
   int supersampling = 6;
   supersampling = static_cast<int>(std::ceil(supersampling / warpedTagUpscale));
-  supersampling = std::max(std::min(supersampling, 8), 1);
+  supersampling = std::max(std::min(supersampling, 10), 1);
 
   cv::Mat scaledTagImage{};
   if (warpedTagUpscale > 2.0) {
@@ -142,7 +144,7 @@ static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
     scaleFactor *= supersampling;
     cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor,
                cv::INTER_NEAREST);
-    tagPoints = Get16h5MarkerPts(scaleFactor);
+    tagPoints = Get36h11MarkerPts(scaleFactor);
   } else {
     scaledTagImage = tagImage;
   }

photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h

@@ -173,7 +173,7 @@ class VisionSystemSim {
     std::vector<VisionTargetSim> targets;
     for (const frc::AprilTag& tag : layout.GetTags()) {
       targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(),
-                                           photon::kAprilTag16h5, tag.ID});
+                                           photon::kAprilTag36h11, tag.ID});
     }
     AddVisionTargets("apriltag", targets);
   }