mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-24 01:31:44 +00:00
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>
This commit is contained in:
Sriman Achanta
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -257,7 +257,7 @@ public class VisionSystemSim {
|
||||
"apriltag",
|
||||
new VisionTargetSim(
|
||||
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
|
||||
TargetModel.kTag16h5,
|
||||
TargetModel.kAprilTag16h5,
|
||||
tag.ID));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user