add tag model to photon estimator

PhotonVision · Oct 25, 2023 · 86197f0 · 86197f0
1 parent 9ab4e8b
commit 86197f0
Show file tree

Hide file tree

Showing 9 changed files with 38 additions and 14 deletions.
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/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,16 +107,18 @@ public enum TargetModel implements Releasable {
                             -Units.inchesToMeters(9.5) / 2,
                             -Units.inchesToMeters(9.5) / 2)),
             0),
-    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
-    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/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
@@ -519,7 +519,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);
@@ -537,7 +538,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
@@ -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;