Skip to content

Commit

Permalink
Changes sim to use 36h11 tags
Browse files Browse the repository at this point in the history
  • Loading branch information
Drew Williams authored and Drew Williams committed Dec 18, 2023
1 parent 796b8e7 commit 40fec21
Show file tree
Hide file tree
Showing 6 changed files with 59 additions and 92 deletions.
4 changes: 2 additions & 2 deletions photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -530,7 +530,7 @@ public PhotonPipelineResult process(
prop.getDistCoeffs(),
detectableTgts,
tagLayout,
TargetModel.kAprilTag16h5);
TargetModel.kAprilTag36h11);
multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
}

Expand Down
94 changes: 33 additions & 61 deletions photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<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;
Expand All @@ -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. */
Expand All @@ -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
Expand All @@ -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);
Expand Down Expand Up @@ -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) {
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
Expand Down
44 changes: 22 additions & 22 deletions photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,22 +51,22 @@ 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;
}

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;
Expand All @@ -81,27 +81,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) {
Expand All @@ -112,11 +112,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);
Expand All @@ -132,7 +132,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) {
Expand All @@ -142,7 +142,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;
}
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
5 changes: 0 additions & 5 deletions photonlib-cpp-examples/swervedriveposeestsim/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down

0 comments on commit 40fec21

Please sign in to comment.