From 40fec2124d5b8c031adc81a2a7262e143a12a1e7 Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Mon, 18 Dec 2023 15:14:25 -0500 Subject: [PATCH] Changes sim to use 36h11 tags --- .../simulation/PhotonCameraSim.java | 4 +- .../photonvision/simulation/VideoSimUtil.java | 94 +++++++------------ .../simulation/VisionSystemSim.java | 2 +- .../include/photon/simulation/VideoSimUtil.h | 44 ++++----- .../photon/simulation/VisionSystemSim.h | 2 +- .../swervedriveposeestsim/build.gradle | 5 - 6 files changed, 59 insertions(+), 92 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index b602b0194d..d37e41fd8e 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -465,7 +465,7 @@ public PhotonPipelineResult process( 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() @@ -530,7 +530,7 @@ public PhotonPipelineResult process( prop.getDistCoeffs(), detectableTgts, tagLayout, - TargetModel.kAprilTag16h5); + TargetModel.kAprilTag36h11); multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs); } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index 8b5c787b78..9119659e4b 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -24,10 +24,12 @@ 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 edu.wpi.first.util.RawFrame; import java.awt.image.BufferedImage; import java.io.IOException; import java.util.ArrayList; @@ -51,15 +53,12 @@ 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 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 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 +67,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 +99,42 @@ public static Point[] getImageCorners(Size size) { } /** - * 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.getDataByteBuffer()); 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 +144,11 @@ public static Point[] get16h5MarkerPts(int scale) { * 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 +178,7 @@ public static void warp16h5TagImage( */ 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 +188,7 @@ public static void warp16h5TagImage( 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 diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index eb4720a21a..b61b939465 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -256,7 +256,7 @@ public void addAprilTags(AprilTagFieldLayout tagLayout) { "apriltag", new VisionTargetSim( tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation - TargetModel.kAprilTag16h5, + TargetModel.kAprilTag36h11, tag.ID)); } } diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h index bb2c453a88..60ed42ee56 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -51,13 +51,13 @@ 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); +static cv::Mat Get36h11TagImage(int id) { + wpi::RawFrame frame = frc::AprilTag::Generate36h11AprilTagImage(id); cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data}; cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone(); return markerClone; @@ -65,8 +65,8 @@ static cv::Mat Get16h5TagImage(int id) { static std::unordered_map LoadAprilTagImages() { std::unordered_map 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 +81,27 @@ static std::vector GetImageCorners(const cv::Size& size) { return retVal; } -static std::vector 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 pts = GetImageCorners(roi16h5.size()); +static std::vector 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 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 Get16h5MarkerPts() { - return Get16h5MarkerPts(1); +static std::vector Get36h11MarkerPts() { + return Get36h11MarkerPts(1); } -static const std::unordered_map kTag16h5Images = +static const std::unordered_map kTag36h11Images = LoadAprilTagImages(); -static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); +static const std::vector kTag36h11MarkPts = Get36h11MarkerPts(); [[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, const SimCameraProperties& prop) { @@ -112,11 +112,11 @@ static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); [[maybe_unused]] static void Warp165h5TagImage( int tagId, const std::vector& dstPoints, bool antialiasing, cv::Mat& destination) { - if (!kTag16h5Images.contains(tagId)) { + if (!kTag36h11Images.contains(tagId)) { return; } - cv::Mat tagImage = kTag16h5Images.at(tagId); - std::vector tagPoints{kTag16h5MarkPts}; + cv::Mat tagImage = kTag36h11Images.at(tagId); + std::vector tagPoints{kTag36h11MarkPts}; std::vector tagImageCorners{GetImageCorners(tagImage.size())}; std::vector dstPointMat = dstPoints; cv::Rect boundingRect = cv::boundingRect(dstPointMat); @@ -132,7 +132,7 @@ static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); int supersampling = 6; supersampling = static_cast(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 +142,7 @@ static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); scaleFactor *= supersampling; cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor, cv::INTER_NEAREST); - tagPoints = Get16h5MarkerPts(scaleFactor); + tagPoints = Get36h11MarkerPts(scaleFactor); } else { scaledTagImage = tagImage; } diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h index 55988bc32f..aca9e3d91b 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -173,7 +173,7 @@ class VisionSystemSim { std::vector 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); } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle index 8eeef20126..c1c32d0284 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle +++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle @@ -6,11 +6,6 @@ plugins { id "com.dorongold.task-tree" version "2.1.0" } -wpi.maven.useLocal = false -wpi.maven.useDevelopment = true -wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015' -wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015' - repositories { mavenLocal() jcenter()