From 3340e74719bb3858f489d47318a635581fc8b1de Mon Sep 17 00:00:00 2001 From: Matthew Morley Date: Sun, 15 Oct 2023 12:50:15 -0400 Subject: [PATCH] Add 36h11 model --- .../components/dashboard/tabs/AprilTagTab.vue | 2 +- .../src/components/dashboard/tabs/PnPTab.vue | 4 ++- photon-client/src/types/PipelineTypes.ts | 4 ++- .../vision/pipeline/AprilTagPipeline.java | 29 ++++++------------- .../vision/target/TargetModel.java | 22 +++++++++++--- 5 files changed, 34 insertions(+), 27 deletions(-) diff --git a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue index 5da6d959ff..d84d3ac66b 100644 --- a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue +++ b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue @@ -25,7 +25,7 @@ const interactiveCols = computed( diff --git a/photon-client/src/components/dashboard/tabs/PnPTab.vue b/photon-client/src/components/dashboard/tabs/PnPTab.vue index e129759b8f..cb424a6080 100644 --- a/photon-client/src/components/dashboard/tabs/PnPTab.vue +++ b/photon-client/src/components/dashboard/tabs/PnPTab.vue @@ -29,7 +29,9 @@ const interactiveCols = computed( { name: '2016 High Goal', value: TargetModel.StrongholdHighGoal }, { name: '200mm AprilTag', value: TargetModel.Apriltag_200mm }, { name: '6in (16h5) Aruco', value: TargetModel.Aruco6in_16h5 }, - { name: '6in (16h5) AprilTag', value: TargetModel.Apriltag6in_16h5 } + { name: '6in (16h5) AprilTag', value: TargetModel.Apriltag6in_16h5 }, + { name: '6.5in (36h11) Aruco', value: TargetModel.Aruco6p5in_36h11 }, + { name: '6.5in (36h11) AprilTag', value: TargetModel.Apriltag6p5in_36h11 } ]" :select-cols="interactiveCols" @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ targetModel: value }, false)" diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts index 2f2a090316..f882f803b3 100644 --- a/photon-client/src/types/PipelineTypes.ts +++ b/photon-client/src/types/PipelineTypes.ts @@ -29,7 +29,9 @@ export enum TargetModel { StrongholdHighGoal = 5, Apriltag_200mm = 6, Aruco6in_16h5 = 7, - Apriltag6in_16h5 = 8 + Apriltag6in_16h5 = 8, + Aruco6p5in_36h11 = 9, + Apriltag6p5in_36h11 = 10 } export interface PipelineSettings { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index af1c5145d4..14ad427031 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -26,6 +26,7 @@ import java.util.ArrayList; import java.util.List; import org.photonvision.common.util.math.MathUtils; +import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.pipe.CVPipe.CVPipeResult; @@ -60,27 +61,15 @@ protected void setPipeParamsImpl() { // Sanitize thread count - not supported to have fewer than 1 threads settings.threads = Math.max(1, settings.threads); - // if (cameraQuirks.hasQuirk(CameraQuirk.PiCam) && LibCameraJNI.isSupported()) { - // // TODO: Picam grayscale - // LibCameraJNI.setRotation(settings.inputImageRotationMode.value); - // // LibCameraJNI.setShouldCopyColor(true); // need the color image to grayscale - // } - - // TODO (HACK): tag width is Fun because it really belongs in the "target model" - // We need the tag width for the JNI to figure out target pose, but we need a - // target model for the draw 3d targets pipeline to work... - // for now, hard code tag width based on enum value - double tagWidth = Units.inchesToMeters(3 * 2); // for 6in 16h5 tag. - - // AprilTagDetectorParams aprilTagDetectionParams = - // new AprilTagDetectorParams( - // settings.tagFamily, - // settings.decimate, - // settings.blur, - // settings.threads, - // settings.debug, - // settings.refineEdges); + double tagWidth; + if (settings.tagFamily == AprilTagFamily.kTag36h11) { + // 2024 tag, guess 6.5in + tagWidth = Units.inchesToMeters(6.5); + } else { + // 2023/other: best guess is 6in + tagWidth = Units.inchesToMeters(6); + } var config = new AprilTagDetector.Config(); config.numThreads = settings.threads; 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 ea06638af8..444a84d5e7 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 @@ -109,21 +109,35 @@ public enum TargetModel implements Releasable { new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0), new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)), Units.inchesToMeters(3.25 * 2)), - kAruco6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners - // do not + kAruco6in_16h5( // Corners used are only the internal tag black corners, not the full tag size + // (which would include the white boarder) 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)), - k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners - // do not + k6in_16h5( 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)), + // 2024 FRC tag. 6.5in inner tag, 8.125 overall + kAruco6p5in_36h11( + 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(3 * 2)), + k6p5in_36h11( + 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(3 * 2)); @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;