From 0898dfe2f7253fb526af3506f6fd907e81c28ebf Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 29 Oct 2023 23:02:16 -0400 Subject: [PATCH] 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 --- .../src/components/dashboard/tabs/PnPTab.vue | 7 ++-- photon-client/src/types/PipelineTypes.ts | 11 +++--- .../vision/pipe/impl/MultiTargetPNPPipe.java | 10 ++++-- .../vision/pipeline/AprilTagPipeline.java | 13 +++---- .../pipeline/AprilTagPipelineSettings.java | 2 +- .../vision/pipeline/ArucoPipeline.java | 13 +++++-- .../pipeline/ArucoPipelineSettings.java | 2 +- .../vision/pipeline/OutputStreamPipeline.java | 2 +- .../vision/target/TargetModel.java | 24 ++++--------- .../vision/pipeline/AprilTagTest.java | 4 +-- .../vision/pipeline/ArucoPipelineTest.java | 2 +- .../org/photonvision/PhotonPoseEstimator.java | 22 +++++++++++- .../simulation/PhotonCameraSim.java | 6 +++- .../simulation/VisionSystemSim.java | 2 +- .../java/org/photonvision/OpenCVTest.java | 4 +-- .../org/photonvision/VisionSystemSimTest.java | 34 ++++++++++--------- .../src/main/java/org/photonvision/Main.java | 2 +- .../photonvision/estimation/TargetModel.java | 4 ++- .../estimation/VisionEstimation.java | 8 ++--- 19 files changed, 98 insertions(+), 74 deletions(-) diff --git a/photon-client/src/components/dashboard/tabs/PnPTab.vue b/photon-client/src/components/dashboard/tabs/PnPTab.vue index c242344a92..c8831e944b 100644 --- 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 index 08d231b732..860c87e757 100644 --- 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 targetL 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 @@ private MultiTargetPNPResults calculateCameraInField(List targetL 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 index 4a17673c94..1eb040616a 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 @@ -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 index 6db514d005..31da770127 100644 --- 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 index 4d80029cbd..c463cb14e4 100644 --- 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 index 94eb963435..2690034db4 100644 --- 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 index 0474b6b562..238333ee6a 100644 --- 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 index 3e8a967ba2..1cd3579382 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 @@ -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 index a970b1e3f7..24bffd6d55 100644 --- 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 index 105858e85b..8a7a4af464 100644 --- 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 index ed29a9c265..1c73f2cab2 100644 --- 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. + * + *

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 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 index b2c7317c32..4880adaa85 100644 --- 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 index ed0207096e..573ad77e91 100644 --- 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 index 3cab3d496e..52b61f5739 100644 --- 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 diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index d9ed86d8ed..c668621fdb 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -415,67 +415,67 @@ public void testMultipleTargets() { new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 1)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 2)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 3)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 4)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 5)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 6)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 7)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 8)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 9)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 10)); visionSysSim.addVisionTargets( new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())), - TargetModel.kTag16h5, + TargetModel.kAprilTag16h5, 11)); var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25)); @@ -506,7 +506,7 @@ public void testPoseEstimation() { Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5)); visionSysSim.addVisionTargets( - new VisionTargetSim(tagList.get(0).pose, TargetModel.kTag16h5, 0)); + new VisionTargetSim(tagList.get(0).pose, TargetModel.kAprilTag16h5, 0)); visionSysSim.update(robotPose); var results = @@ -514,7 +514,8 @@ public void testPoseEstimation() { camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), - layout); + layout, + TargetModel.kAprilTag16h5); Pose3d pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); @@ -522,9 +523,9 @@ public void testPoseEstimation() { assertEquals(Math.toRadians(5), pose.getRotation().getZ(), 0.01); visionSysSim.addVisionTargets( - new VisionTargetSim(tagList.get(1).pose, TargetModel.kTag16h5, 1)); + new VisionTargetSim(tagList.get(1).pose, TargetModel.kAprilTag16h5, 1)); visionSysSim.addVisionTargets( - new VisionTargetSim(tagList.get(2).pose, TargetModel.kTag16h5, 2)); + new VisionTargetSim(tagList.get(2).pose, TargetModel.kAprilTag16h5, 2)); visionSysSim.update(robotPose); results = @@ -532,7 +533,8 @@ public void testPoseEstimation() { camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), camera.getLatestResult().getTargets(), - layout); + layout, + TargetModel.kAprilTag16h5); pose = new Pose3d().plus(results.best); assertEquals(5, pose.getX(), .01); assertEquals(1, pose.getY(), .01); diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index db8787208d..f01e431d30 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -251,7 +251,7 @@ private static void addTestModeSources() { var pipeline2023 = new AprilTagPipelineSettings(); var path_split = Path.of(camConf2023.path).getFileName().toString(); pipeline2023.pipelineNickname = path_split.replace(".png", ""); - pipeline2023.targetModel = TargetModel.k6in_16h5; + pipeline2023.targetModel = TargetModel.kAprilTag6in_16h5; pipeline2023.inputShouldShow = true; pipeline2023.solvePNPEnabled = true; diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index 4a4c22449d..f945d2c806 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -37,8 +37,10 @@ public class TargetModel { public final boolean isPlanar; public final boolean isSpherical; - public static final TargetModel kTag16h5 = + public static final TargetModel kAprilTag16h5 = new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6)); + public static final TargetModel kAprilTag36h11 = + new TargetModel(Units.inchesToMeters(6.5), Units.inchesToMeters(6.5)); /** * Creates a rectangular, planar target model given the width and height. The model has four diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 387176d57b..9ad6e0fc4e 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -70,7 +70,8 @@ public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, - AprilTagFieldLayout tagLayout) { + AprilTagFieldLayout tagLayout, + TargetModel tagModel) { if (tagLayout == null || visTags == null || tagLayout.getTags().size() == 0 @@ -99,8 +100,7 @@ public static PNPResults estimateCamPosePNP( // single-tag pnp if (knownTags.size() == 1) { var camToTag = - OpenCVHelp.solvePNP_SQUARE( - cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, points); + OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points); if (!camToTag.isPresent) return new PNPResults(); var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); var altPose = new Pose3d(); @@ -118,7 +118,7 @@ public static PNPResults estimateCamPosePNP( // multi-tag pnp else { var objectTrls = new ArrayList(); - for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose)); + for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose)); var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); if (!camToOrigin.isPresent) return new PNPResults(); return new PNPResults(