Aruco/Multitag 36h11 support (#981)

- Aruco pipeline now infers tag width from tag family like the AprilTag pipeline - Removes unused Aruco and 200mm AprilTag models - `VisionEstimation.estimateCamPosePNP()` now requires a target model instead of assuming 16h5 - Multitarget pipeline similarly infers target model of tag family now - `PhotonPoseEstimator` can have target model set for on-rio multitarget --------- Co-authored-by: amquake <[email protected]>
PhotonVision · Oct 30, 2023 · 0898dfe · 0898dfe
1 parent d61225e
commit 0898dfe
Show file tree

Hide file tree

Showing 19 changed files with 98 additions and 74 deletions.
diff --git a/photon-client/src/components/dashboard/tabs/PnPTab.vue b/photon-client/src/components/dashboard/tabs/PnPTab.vue
@@ -26,11 +26,8 @@ const interactiveCols = computed(
         { 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: '200mm AprilTag', value: TargetModel.Apriltag_200mm },
-        { name: '6in (16h5) Aruco', value: TargetModel.Aruco6in_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 }
+        { name: '2023 AprilTag 6in (16h5)', value: TargetModel.AprilTag6in_16h5 },
+        { name: '2024 AprilTag 6.5in (36h11)', 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
@@ -26,11 +26,8 @@ export enum TargetModel {
   InfiniteRechargeHighGoalOuter = 2,
   CircularPowerCell7in = 3,
   RapidReactCircularCargoBall = 4,
-  Apriltag_200mm = 5,
-  Aruco6in_16h5 = 6,
-  Apriltag6in_16h5 = 7,
-  Aruco6p5in_36h11 = 8,
-  Apriltag6p5in_36h11 = 9
+  AprilTag6in_16h5 = 5,
+  AprilTag6p5in_36h11 = 6
 }
 
 export interface PipelineSettings {
@@ -225,7 +222,7 @@ export type ConfigurableAprilTagPipelineSettings = Partial<
 export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
   ...DefaultPipelineSettings,
   cameraGain: 75,
-  targetModel: TargetModel.Apriltag6in_16h5,
+  targetModel: TargetModel.AprilTag6in_16h5,
   ledMode: false,
   outputShowMultipleTargets: true,
   cameraExposure: 20,
@@ -268,7 +265,7 @@ export type ConfigurableArucoPipelineSettings = Partial<Omit<ArucoPipelineSettin
 export const DefaultArucoPipelineSettings: ArucoPipelineSettings = {
   ...DefaultPipelineSettings,
   outputShowMultipleTargets: true,
-  targetModel: TargetModel.Aruco6in_16h5,
+  targetModel: TargetModel.AprilTag6in_16h5,
   cameraExposure: -1,
   cameraAutoExposure: true,
   ledMode: false,

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
@@ -22,6 +22,7 @@
 import java.util.List;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -70,19 +71,24 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
                         params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
                         params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
                         TrackedTarget.simpleFromTrackedTargets(targetList),
-                        params.atfl);
+                        params.atfl,
+                        params.targetModel);
 
         return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
     }
 
     public static class MultiTargetPNPPipeParams {
         private final CameraCalibrationCoefficients cameraCoefficients;
         private final AprilTagFieldLayout atfl;
+        private final TargetModel targetModel;
 
         public MultiTargetPNPPipeParams(
-                CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) {
+                CameraCalibrationCoefficients cameraCoefficients,
+                AprilTagFieldLayout atfl,
+                TargetModel targetModel) {
             this.cameraCoefficients = cameraCoefficients;
             this.atfl = atfl;
+            this.targetModel = targetModel;
         }
     }
 }
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
@@ -30,6 +30,7 @@
 import java.util.List;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.estimation.TargetModel;
 import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.frame.Frame;
@@ -71,13 +72,13 @@ protected void setPipeParamsImpl() {
         settings.threads = Math.max(1, settings.threads);
 
         // for now, hard code tag width based on enum value
-        double tagWidth;
+        // 2023/other: best guess is 6in
+        double tagWidth = Units.inchesToMeters(6);
+        TargetModel tagModel = TargetModel.kAprilTag16h5;
         if (settings.tagFamily == AprilTagFamily.kTag36h11) {
-            // 2024 tag, guess 6.5in
+            // 2024 tag, 6.5in
             tagWidth = Units.inchesToMeters(6.5);
-        } else {
-            // 2023/other: best guess is 6in
-            tagWidth = Units.inchesToMeters(6);
+            tagModel = TargetModel.kAprilTag36h11;
         }
 
         var config = new AprilTagDetector.Config();
@@ -104,7 +105,7 @@ protected void setPipeParamsImpl() {
                 // TODO global state ew
                 var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
                 multiTagPNPPipe.setParams(
-                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
             }
         }
     }

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
@@ -41,7 +41,7 @@ public AprilTagPipelineSettings() {
         super();
         pipelineType = PipelineType.AprilTag;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.k6in_16h5;
+        targetModel = TargetModel.kAprilTag6in_16h5;
         cameraExposure = 20;
         cameraAutoExposure = false;
         ledMode = false;

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
@@ -46,6 +46,7 @@
 import org.opencv.objdetect.Objdetect;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.estimation.TargetModel;
 import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.Frame;
@@ -79,9 +80,16 @@ protected void setPipeParamsImpl() {
         var params = new ArucoDetectionPipeParams();
         // sanitize and record settings
 
+        // for now, hard code tag width based on enum value
+        // 2023/other: best guess is 6in
+        double tagWidth = Units.inchesToMeters(6);
+        TargetModel tagModel = TargetModel.kAprilTag16h5;
         switch (settings.tagFamily) {
             case kTag36h11:
+                // 2024 tag, 6.5in
                 params.tagFamily = Objdetect.DICT_APRILTAG_36h11;
+                tagWidth = Units.inchesToMeters(6.5);
+                tagModel = TargetModel.kAprilTag36h11;
                 break;
             case kTag25h9:
                 params.tagFamily = Objdetect.DICT_APRILTAG_25h9;
@@ -113,14 +121,13 @@ protected void setPipeParamsImpl() {
             var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
             if (cameraMatrix != null && cameraMatrix.rows() > 0) {
                 var estimatorParams =
-                        new ArucoPoseEstimatorPipeParams(
-                                frameStaticProperties.cameraCalibration, Units.inchesToMeters(6));
+                        new ArucoPoseEstimatorPipeParams(frameStaticProperties.cameraCalibration, tagWidth);
                 singleTagPoseEstimatorPipe.setParams(estimatorParams);
 
                 // TODO global state ew
                 var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
                 multiTagPNPPipe.setParams(
-                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
             }
         }
     }

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java
@@ -46,7 +46,7 @@ public ArucoPipelineSettings() {
         super();
         pipelineType = PipelineType.Aruco;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.k6in_16h5;
+        targetModel = TargetModel.kAprilTag6in_16h5;
         cameraExposure = -1;
         cameraAutoExposure = true;
         ledMode = false;

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
@@ -107,7 +107,7 @@ protected void setPipeParams(
                 new Draw3dArucoPipe.Draw3dArucoParams(
                         settings.outputShouldDraw,
                         frameStaticProperties.cameraCalibration,
-                        TargetModel.k6in_16h5,
+                        TargetModel.kAprilTag6in_16h5,
                         settings.streamingFrameDivisor);
         draw3dArucoPipe.setParams(draw3dArucoParams);
 

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
@@ -107,30 +107,18 @@ public enum TargetModel implements Releasable {
                             -Units.inchesToMeters(9.5) / 2,
                             -Units.inchesToMeters(9.5) / 2)),
             0),
-    k200mmAprilTag( // Corners of the tag's inner black square (excluding white border)
-            List.of(
-                    new Point3(Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
-                    new Point3(-Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
-                    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)),
-    k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
-            // do not
+    // 2023 AprilTag, with 6 inch marker width (inner black square).
+    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)),
-    // 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(
+    // 2024 AprilTag, with 6.5 inch marker width (inner black square).
+    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),

diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
@@ -47,7 +47,7 @@ public void testApriltagFacingCamera() {
         pipeline.getSettings().solvePNPEnabled = true;
         pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
         pipeline.getSettings().cornerDetectionUseConvexHulls = true;
-        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().targetModel = TargetModel.kAprilTag6p5in_36h11;
         pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
 
         var frameProvider =
@@ -112,7 +112,7 @@ public void testApriltagDistorted() {
         pipeline.getSettings().solvePNPEnabled = true;
         pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
         pipeline.getSettings().cornerDetectionUseConvexHulls = true;
-        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().targetModel = TargetModel.kAprilTag6p5in_36h11;
         pipeline.getSettings().tagFamily = AprilTagFamily.kTag16h5;
 
         var frameProvider =

diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java
@@ -47,7 +47,7 @@ public void testApriltagFacingCamera() {
         pipeline.getSettings().solvePNPEnabled = true;
         pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
         pipeline.getSettings().cornerDetectionUseConvexHulls = true;
-        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().targetModel = TargetModel.kAprilTag6p5in_36h11;
         pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
 
         var frameProvider =

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -41,6 +41,7 @@
 import java.util.List;
 import java.util.Optional;
 import java.util.Set;
+import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
@@ -82,6 +83,7 @@ public enum PoseStrategy {
     }
 
     private AprilTagFieldLayout fieldTags;
+    private TargetModel tagModel = TargetModel.kAprilTag16h5;
     private PoseStrategy primaryStrategy;
     private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
     private final PhotonCamera camera;
@@ -155,6 +157,24 @@ public void setFieldTags(AprilTagFieldLayout fieldTags) {
         this.fieldTags = fieldTags;
     }
 
+    /**
+     * Get the TargetModel representing the tags being detected. This is used for on-rio multitag.
+     *
+     * <p>By default, this is {@link TargetModel#kAprilTag16h5}.
+     */
+    public TargetModel getTagModel() {
+        return tagModel;
+    }
+
+    /**
+     * Set the TargetModel representing the tags being detected. This is used for on-rio multitag.
+     *
+     * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}.
+     */
+    public void setTagModel(TargetModel tagModel) {
+        this.tagModel = tagModel;
+    }
+
     /**
      * Get the Position Estimation Strategy being used by the Position Estimator.
      *
@@ -419,7 +439,7 @@ private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
 
         var pnpResults =
                 VisionEstimation.estimateCamPosePNP(
-                        cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags);
+                        cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
         // try fallback strategy if solvePNP fails for some reason
         if (!pnpResults.isPresent)
             return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);

diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -531,7 +531,11 @@ public PhotonPipelineResult process(
                     visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
             var pnpResults =
                     VisionEstimation.estimateCamPosePNP(
-                            prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout);
+                            prop.getIntrinsics(),
+                            prop.getDistCoeffs(),
+                            detectableTgts,
+                            tagLayout,
+                            TargetModel.kAprilTag16h5);
             multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs);
         }
 

diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
@@ -257,7 +257,7 @@ public void addAprilTags(AprilTagFieldLayout tagLayout) {
                     "apriltag",
                     new VisionTargetSim(
                             tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
-                            TargetModel.kTag16h5,
+                            TargetModel.kAprilTag16h5,
                             tag.ID));
         }
     }

diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java
@@ -137,7 +137,7 @@ public void testRotConvert() {
     public void testProjection() {
         var target =
                 new VisionTargetSim(
-                        new Pose3d(1, 0, 0, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0);
+                        new Pose3d(1, 0, 0, new Rotation3d(0, 0, Math.PI)), TargetModel.kAprilTag16h5, 0);
         var cameraPose = new Pose3d(0, 0, 0, new Rotation3d());
         var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
         var imagePoints =
@@ -193,7 +193,7 @@ public void testSolvePNP_SQUARE() {
         // square AprilTag target
         var target =
                 new VisionTargetSim(
-                        new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0);
+                        new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), TargetModel.kAprilTag16h5, 0);
         var cameraPose = new Pose3d(0, 0, 0, new Rotation3d());
         var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
         // target relative to camera