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 c242344a9..c8831e944 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 08d231b73..860c87e75 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 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 94eb96343..2690034db 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 class ArucoPipelineSettings extends AdvancedPipelineSettings { 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 0474b6b56..238333ee6 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 @@ public class OutputStreamPipeline { 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 3e8a967ba..1cd357938 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 a970b1e3f..24bffd6d5 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 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 = 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 105858e85..8a7a4af46 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 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 = diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index ed29a9c26..1c73f2cab 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.HashSet; 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 class PhotonPoseEstimator { } 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 class PhotonPoseEstimator { 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 @@ public class PhotonPoseEstimator { 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 b2c7317c3..4880adaa8 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 class PhotonCameraSim implements AutoCloseable { 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 ed0207096..573ad77e9 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 class VisionSystemSim { "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 3cab3d496..52b61f573 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 class OpenCVTest { 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 class OpenCVTest { // 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 d9ed86d8e..c668621fd 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 @@ class VisionSystemSimTest { 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 @@ class VisionSystemSimTest { 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 @@ class VisionSystemSimTest { 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 @@ class VisionSystemSimTest { 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 @@ class VisionSystemSimTest { 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 db8787208..f01e431d3 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 @@ public class Main { 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 4a4c22449..f945d2c80 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 387176d57..9ad6e0fc4 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 class VisionEstimation { 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 class VisionEstimation { // 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 class VisionEstimation { // 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(