Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Changes sim to use 36h11 tags (#1056)

Browse Source 
Fixes #1041

---------

Co-authored-by: Drew Williams <DrewW@iARx.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Drew Williams
2024-01-02 10:09:32 -05:00
committed by GitHub
parent b9c2d839f4
commit 96de176ba2
43 changed files with 116 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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);
}

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

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

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

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