Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Aruco/Multitag 36h11 support (#981)

Browse Source 
- 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>
This commit is contained in:
Sriman Achanta
2023-10-29 23:02:16 -04:00
committed by GitHub
parent d61225eba3
commit 0898dfe2f7
19 changed files with 98 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
photon-client/src/components/dashboard/tabs/PnPTab.vue
View File

@@ -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)"

11
photon-client/src/types/PipelineTypes.ts
View File

@@ -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,

10
photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
View File

@@ -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;
}
}
}

13
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -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));
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
View File

@@ -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;

13
photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
View File

@@ -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));
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java
View File

@@ -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;

2
photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
View File

@@ -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);

24
photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
View File

@@ -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),

4
photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
View File

@@ -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 =

2
photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java
View File

@@ -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 =

22
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -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.
*
* <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 @@ 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);

6
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -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);
}

2
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -257,7 +257,7 @@ public class VisionSystemSim {
"apriltag",
new VisionTargetSim(
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
TargetModel.kTag16h5,
TargetModel.kAprilTag16h5,
tag.ID));
}
}

4
photon-lib/src/test/java/org/photonvision/OpenCVTest.java
View File

@@ -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

34
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

@@ -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);

2
photon-server/src/main/java/org/photonvision/Main.java
View File

@@ -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;

4
photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
View File

@@ -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

8
photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -70,7 +70,8 @@ public class VisionEstimation {
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<PhotonTrackedTarget> 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<Translation3d>();
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(