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 <noleetarrr@gmail.com>
2026-06-25 01:41:40 +00:00 · 2023-10-29 23:02:16 -04:00
parent d61225eba3
commit 0898dfe2f7
19 changed files with 98 additions and 74 deletions
--- 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.ArrayList;
 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,7 +71,8 @@ public class MultiTargetPNPPipe
                        params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
                        params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
                        TrackedTarget.simpleFromTrackedTargets(targetList),
-                        params.atfl);
+                        params.atfl,
+                        params.targetModel);

        return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
    }
@@ -78,11 +80,15 @@ public class MultiTargetPNPPipe
    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;
        }
    }
 }
--- 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.ArrayList;
 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 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
        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 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
                // TODO global state ew
                var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
                multiTagPNPPipe.setParams(
-                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
            }
        }
    }
--- 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 class AprilTagPipelineSettings extends AdvancedPipelineSettings {
        super();
        pipelineType = PipelineType.AprilTag;
        outputShowMultipleTargets = true;
-        targetModel = TargetModel.k6in_16h5;
+        targetModel = TargetModel.kAprilTag6in_16h5;
        cameraExposure = 20;
        cameraAutoExposure = false;
        ledMode = false;
--- 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.imgproc.Imgproc;
 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 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
        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 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
            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));
            }
        }
    }
--- 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 class ArucoPipelineSettings extends AdvancedPipelineSettings {
        super();
        pipelineType = PipelineType.Aruco;
        outputShowMultipleTargets = true;
-        targetModel = TargetModel.k6in_16h5;
+        targetModel = TargetModel.kAprilTag6in_16h5;
        cameraExposure = -1;
        cameraAutoExposure = true;
        ledMode = false;
--- 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 @@ public class OutputStreamPipeline {
                new Draw3dArucoPipe.Draw3dArucoParams(
                        settings.outputShouldDraw,
                        frameStaticProperties.cameraCalibration,
-                        TargetModel.k6in_16h5,
+                        TargetModel.kAprilTag6in_16h5,
                        settings.streamingFrameDivisor);
        draw3dArucoPipe.setParams(draw3dArucoParams);

--- 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),
--- 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 class AprilTagTest {
        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 class AprilTagTest {
        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 =
--- 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 class ArucoPipelineTest {
        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 =