From cb401e1c7a3c1bf92ff12976a1cf613cb521386b Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 24 Oct 2023 23:04:52 -0400
Subject: [PATCH] Add 36h11 tag model for 2024 (#951)

* Add 36h11 model

* Formatting fixes

* Match frontend TargetModel enum with backend

* fix target model merge

* Update TargetModel.java

---------

Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
---
 .../components/dashboard/tabs/AprilTagTab.vue |  2 +-
 .../src/components/dashboard/tabs/PnPTab.vue  |  9 +++---
 photon-client/src/types/PipelineTypes.ts      | 15 +++++-----
 .../vision/pipeline/AprilTagPipeline.java     | 29 ++++++-------------
 .../vision/target/TargetModel.java            | 17 ++++++++++-
 5 files changed, 39 insertions(+), 33 deletions(-)

diff --git a/photon-client/src/components/dashboard/tabs/AprilTagTab.vue b/photon-client/src/components/dashboard/tabs/AprilTagTab.vue
index fa56016a5..25ef85b49 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(
     <pv-select
       v-model="currentPipelineSettings.tagFamily"
       label="Target family"
-      :items="['AprilTag Family 36h11', 'AprilTag Family 25h9', 'AprilTag Family 16h5']"
+      :items="['AprilTag 36h11 (6.5in)', 'AprilTag 25h9 (6in)', 'AprilTag 16h5 (6in)']"
       :select-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ tagFamily: value }, false)"
     />
diff --git a/photon-client/src/components/dashboard/tabs/PnPTab.vue b/photon-client/src/components/dashboard/tabs/PnPTab.vue
index dddf4b072..c242344a9 100644
--- a/photon-client/src/components/dashboard/tabs/PnPTab.vue
+++ b/photon-client/src/components/dashboard/tabs/PnPTab.vue
@@ -21,15 +21,16 @@ const interactiveCols = computed(
       v-model="useCameraSettingsStore().currentPipelineSettings.targetModel"
       label="Target Model"
       :items="[
-        { name: '2020 High Goal Outer', value: TargetModel.InfiniteRechargeHighGoalOuter },
-        { name: '2020 High Goal Inner', value: TargetModel.InfiniteRechargeHighGoalInner },
+        { name: '2016 High Goal', value: TargetModel.StrongholdHighGoal },
         { name: '2019 Dual Target', value: TargetModel.DeepSpaceDualTarget },
+        { name: '2020 High Goal Outer', value: TargetModel.InfiniteRechargeHighGoalOuter },
         { name: '2020 Power Cell (7in)', value: TargetModel.CircularPowerCell7in },
         { name: '2022 Cargo Ball (9.5in)', value: TargetModel.RapidReactCircularCargoBall },
-        { 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 d6debea90..08d231b73 100644
--- a/photon-client/src/types/PipelineTypes.ts
+++ b/photon-client/src/types/PipelineTypes.ts
@@ -21,15 +21,16 @@ export enum RobotOffsetPointMode {
 }
 
 export enum TargetModel {
-  InfiniteRechargeHighGoalOuter = 0,
-  InfiniteRechargeHighGoalInner = 1,
-  DeepSpaceDualTarget = 2,
+  StrongholdHighGoal = 0,
+  DeepSpaceDualTarget = 1,
+  InfiniteRechargeHighGoalOuter = 2,
   CircularPowerCell7in = 3,
   RapidReactCircularCargoBall = 4,
-  StrongholdHighGoal = 5,
-  Apriltag_200mm = 6,
-  Aruco6in_16h5 = 7,
-  Apriltag6in_16h5 = 8
+  Apriltag_200mm = 5,
+  Aruco6in_16h5 = 6,
+  Apriltag6in_16h5 = 7,
+  Aruco6p5in_36h11 = 8,
+  Apriltag6p5in_36h11 = 9
 }
 
 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 97ec1f2f3..f49aaab5d 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
@@ -31,6 +31,7 @@ import java.util.List;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.targeting.MultiTargetPNPResults;
+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;
@@ -69,27 +70,15 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         // 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 de6635b58..3e8a967ba 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
@@ -121,7 +121,22 @@ public enum TargetModel implements Releasable {
                     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));
+            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(6.5)),
+    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(6.5));
 
     @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;
     @JsonIgnore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();