diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java index 0cf8ad1eb..084380ec0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java @@ -69,97 +69,28 @@ public enum TargetModel implements Releasable { new Point3(Units.inchesToMeters(-19.625), Units.inchesToMeters(-8.5), 0), new Point3(Units.inchesToMeters(19.625), Units.inchesToMeters(-8.5), 0)), Units.inchesToMeters(12)), - kCircularPowerCell7in( - List.of( - new Point3( - -Units.inchesToMeters(7) / 2, - -Units.inchesToMeters(7) / 2, - -Units.inchesToMeters(7) / 2), - new Point3( - -Units.inchesToMeters(7) / 2, - Units.inchesToMeters(7) / 2, - -Units.inchesToMeters(7) / 2), - new Point3( - Units.inchesToMeters(7) / 2, - Units.inchesToMeters(7) / 2, - -Units.inchesToMeters(7) / 2), - new Point3( - Units.inchesToMeters(7) / 2, - -Units.inchesToMeters(7) / 2, - -Units.inchesToMeters(7) / 2)), - 0), - k2022CircularCargoBall( - List.of( - new Point3( - -Units.inchesToMeters(9.5) / 2, - -Units.inchesToMeters(9.5) / 2, - -Units.inchesToMeters(9.5) / 2), - new Point3( - -Units.inchesToMeters(9.5) / 2, - Units.inchesToMeters(9.5) / 2, - -Units.inchesToMeters(9.5) / 2), - new Point3( - Units.inchesToMeters(9.5) / 2, - Units.inchesToMeters(9.5) / 2, - -Units.inchesToMeters(9.5) / 2), - new Point3( - Units.inchesToMeters(9.5) / 2, - -Units.inchesToMeters(9.5) / 2, - -Units.inchesToMeters(9.5) / 2)), - 0), - k2025Algae( - List.of( - new Point3( - -Units.inchesToMeters(16.25) / 2, - -Units.inchesToMeters(16.25) / 2, - -Units.inchesToMeters(16.25) / 2), - new Point3( - -Units.inchesToMeters(16.25) / 2, - Units.inchesToMeters(16.25) / 2, - -Units.inchesToMeters(16.25) / 2), - new Point3( - Units.inchesToMeters(16.25) / 2, - Units.inchesToMeters(16.25) / 2, - -Units.inchesToMeters(16.25) / 2), - new Point3( - Units.inchesToMeters(16.25) / 2, - -Units.inchesToMeters(16.25) / 2, - -Units.inchesToMeters(16.25) / 2)), - 0), + kCircularPowerCell7in(circleTargetCorners(Units.inchesToMeters(7)), 0), + k2022CircularCargoBall(circleTargetCorners(Units.inchesToMeters(9.5)), 0), + k2025Algae(circleTargetCorners(Units.inchesToMeters(16.25)), 0), // 2023 AprilTag, with 6 inch marker width (inner black square). // MIGRATION: 2023 @Json.Alias({"k6in_16h5"}) kAprilTag6in_16h5( // Corners of the tag's inner black square (excluding white border) - List.of( - new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0), - new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0), - new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), - new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), - Units.inchesToMeters(3 * 2)), + squareTargetCorners(Units.inchesToMeters(6)), Units.inchesToMeters(6)), // 2024 AprilTag, with 6.5 inch marker width (inner black square). // MIGRATION: 2023 @Json.Alias({"k6p5in_36h11", "k200mmAprilTag", "kAruco6p5in_36h11"}) kAprilTag6p5in_36h11( // Corners of the tag's inner black square (excluding white border) - List.of( - new Point3(-Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), - new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), - new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0), - new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)), - Units.inchesToMeters(6.5)); + squareTargetCorners(Units.inchesToMeters(6.5)), Units.inchesToMeters(6.5)); @Json.Ignore private final MatOfPoint3f realWorldTargetCoordinates; @Json.Ignore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); @Json.Ignore private final MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); - private List realWorldCoordinatesArray; - private double boxHeight; - TargetModel(MatOfPoint3f realWorldTargetCoordinates, double boxHeight) { this.realWorldTargetCoordinates = realWorldTargetCoordinates; - this.realWorldCoordinatesArray = realWorldTargetCoordinates.toList(); - this.boxHeight = boxHeight; var bottomList = realWorldTargetCoordinates.toList(); var topList = new ArrayList(); @@ -175,22 +106,6 @@ public enum TargetModel implements Releasable { this(listToMat(realWorldCoordinatesArray), boxHeight); } - public List getRealWorldCoordinatesArray() { - return this.realWorldCoordinatesArray; - } - - public double getBoxHeight() { - return boxHeight; - } - - public void setRealWorldCoordinatesArray(List realWorldCoordinatesArray) { - this.realWorldCoordinatesArray = realWorldCoordinatesArray; - } - - public void setBoxHeight(double boxHeight) { - this.boxHeight = boxHeight; - } - private static MatOfPoint3f listToMat(List points) { var mat = new MatOfPoint3f(); mat.fromList(points); @@ -209,15 +124,23 @@ public enum TargetModel implements Releasable { return visualizationBoxTop; } - // public static TargetModel getCircleTarget(double Units.inchesToMeters(7)) { - // var corners = - // List.of( - // new Point3(-Units.inchesToMeters(7) / 2, -radius / 2, -radius / 2), - // new Point3(-Units.inchesToMeters(7) / 2, radius / 2, -radius / 2), - // new Point3(Units.inchesToMeters(7) / 2, radius / 2, -radius / 2), - // new Point3(Units.inchesToMeters(7) / 2, -radius / 2, -radius / 2)); - // return new TargetModel(corners, 0); - // } + private static List circleTargetCorners(double diameter) { + double radius = diameter / 2; + return List.of( + new Point3(-radius, -radius, -radius), + new Point3(-radius, radius, -radius), + new Point3(radius, radius, -radius), + new Point3(radius, -radius, -radius)); + } + + private static List squareTargetCorners(double edgeLength) { + double radius = edgeLength / 2; + return List.of( + new Point3(-radius, -radius, 0), + new Point3(-radius, radius, 0), + new Point3(radius, radius, 0), + new Point3(radius, -radius, 0)); + } @Json.Value @Override diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index c8b314be8..7e3d6319c 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java @@ -62,7 +62,7 @@ public class TargetCalculationsTest { null); @BeforeAll - public static void setup() { + public static void init() { LoadJNI.loadLibraries(); } diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetModelTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetModelTest.java new file mode 100644 index 000000000..f10101a28 --- /dev/null +++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetModelTest.java @@ -0,0 +1,79 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.target; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.List; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.opencv.core.Point3; +import org.photonvision.common.LoadJNI; +import org.wpilib.math.util.Units; + +public class TargetModelTest { + @BeforeAll + public static void init() { + LoadJNI.loadLibraries(); + } + + @Test + void testCircleTargetGeneration() { + assertApproxEquals( + List.of( + new Point3( + -Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2), + new Point3( + -Units.inchesToMeters(7) / 2, + Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2), + new Point3( + Units.inchesToMeters(7) / 2, + Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2), + new Point3( + Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2, + -Units.inchesToMeters(7) / 2)), + TargetModel.kCircularPowerCell7in.getRealWorldTargetCoordinates().toList(), + 1E-6); + } + + @Test + void testSquareTargetGeneration() { + assertApproxEquals( + List.of( + new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0), + new Point3(-Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), + new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), + new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)), + TargetModel.kAprilTag6p5in_36h11.getRealWorldTargetCoordinates().toList(), + 1E-6); + } + + static void assertApproxEquals(List expected, List actual, double delta) { + assertEquals(expected.size(), actual.size()); + for (int i = 0; i < actual.size(); i++) { + assertEquals(expected.get(i).x, actual.get(i).x, delta, "Bad x for point %d".formatted(i)); + assertEquals(expected.get(i).y, actual.get(i).y, delta, "Bad y for point %d".formatted(i)); + assertEquals(expected.get(i).z, actual.get(i).z, delta, "Bad z for point %d".formatted(i)); + } + } +} diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TrackedTargetTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TrackedTargetTest.java index c373ef88f..24c7dd6cf 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TrackedTargetTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/target/TrackedTargetTest.java @@ -20,7 +20,7 @@ package org.photonvision.vision.target; import static org.junit.jupiter.api.Assertions.assertEquals; import java.util.List; -import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.opencv.core.MatOfPoint; import org.opencv.core.Point; @@ -30,8 +30,8 @@ import org.photonvision.vision.opencv.Contour; import org.photonvision.vision.opencv.DualOffsetValues; public class TrackedTargetTest { - @BeforeEach - public void Init() { + @BeforeAll + public static void init() { LoadJNI.loadLibraries(); }