diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 2cb8b6e21..8f3272b69 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -31,9 +31,13 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N8; @@ -83,7 +87,18 @@ public class PhotonPoseEstimator { * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can * take a lot of time. */ - MULTI_TAG_PNP_ON_RIO + MULTI_TAG_PNP_ON_RIO, + + /** + * Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order + * to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading + * data is up-to-date. + * + *

Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch) + * + *

https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98 + */ + PNP_DISTANCE_TRIG_SOLVE } private AprilTagFieldLayout fieldTags; @@ -97,6 +112,9 @@ public class PhotonPoseEstimator { protected double poseCacheTimestampSeconds = -1; private final Set reportedErrors = new HashSet<>(); + private final TimeInterpolatableBuffer headingBuffer = + TimeInterpolatableBuffer.createBuffer(1.0); + /** * Create a new PhotonPoseEstimator. * @@ -259,6 +277,30 @@ public class PhotonPoseEstimator { setLastPose(new Pose3d(lastPose)); } + /** + * Add robot heading data to buffer. Must be called periodically for the + * PNP_DISTANCE_TRIG_SOLVE strategy. + * + * @param timestampSeconds timestamp of the robot heading data. + * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field + * coordinates. + */ + public void addHeadingData(double timestampSeconds, Rotation3d heading) { + addHeadingData(timestampSeconds, heading.toRotation2d()); + } + + /** + * Add robot heading data to buffer. Must be called periodically for the + * PNP_DISTANCE_TRIG_SOLVE strategy. + * + * @param timestampSeconds timestamp of the robot heading data. + * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field + * coordinates. + */ + public void addHeadingData(double timestampSeconds, Rotation2d heading) { + headingBuffer.addSample(timestampSeconds, heading); + } + /** * @return The current transform from the center of the robot to the camera mount position */ @@ -374,6 +416,7 @@ public class PhotonPoseEstimator { case MULTI_TAG_PNP_ON_RIO -> multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult); + case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult); }; if (estimatedPose.isPresent()) { @@ -383,6 +426,74 @@ public class PhotonPoseEstimator { return estimatedPose; } + private Optional pnpDistanceTrigSolveStrategy(PhotonPipelineResult result) { + PhotonTrackedTarget bestTarget = result.getBestTarget(); + + if (bestTarget == null) return Optional.empty(); + + Translation2d camToTagTranslation = + new Pose3d( + Translation3d.kZero, + new Rotation3d( + 0, + -Math.toRadians(bestTarget.getPitch()), + -Math.toRadians(bestTarget.getYaw()))) + .transformBy( + new Transform3d( + new Translation3d( + bestTarget.getBestCameraToTarget().getTranslation().getNorm(), 0, 0), + Rotation3d.kZero)) + .getTranslation() + .rotateBy( + new Rotation3d( + getRobotToCameraTransform().getRotation().getX(), + getRobotToCameraTransform().getRotation().getY(), + 0)) + .toTranslation2d(); + + if (headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) { + return Optional.empty(); + } + + Rotation2d headingSample = headingBuffer.getSample(result.getTimestampSeconds()).get(); + + Rotation2d camToTagRotation = + headingSample.plus( + getRobotToCameraTransform() + .getRotation() + .toRotation2d() + .plus(camToTagTranslation.getAngle())); + + if (fieldTags.getTagPose(bestTarget.getFiducialId()).isEmpty()) return Optional.empty(); + var tagPose2d = fieldTags.getTagPose(bestTarget.getFiducialId()).get().toPose2d(); + + Translation2d fieldToCameraTranslation = + new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) + .transformBy(new Transform2d(camToTagTranslation.getNorm(), 0, Rotation2d.kZero)) + .getTranslation(); + + Pose2d robotPose = + new Pose2d( + fieldToCameraTranslation, + headingSample.plus(getRobotToCameraTransform().getRotation().toRotation2d())) + .transformBy( + new Transform2d( + new Pose3d( + getRobotToCameraTransform().getTranslation(), + getRobotToCameraTransform().getRotation()) + .toPose2d(), + Pose2d.kZero)); + + robotPose = new Pose2d(robotPose.getTranslation(), headingSample); + + return Optional.of( + new EstimatedRobotPose( + new Pose3d(robotPose), + result.getTimestampSeconds(), + result.getTargets(), + PoseStrategy.PNP_DISTANCE_TRIG_SOLVE)); + } + private Optional multiTagOnCoprocStrategy(PhotonPipelineResult result) { if (result.getMultiTagResult().isEmpty()) { return update(result, this.multiTagFallbackStrategy); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 9cab92dda..61c9a4538 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -42,6 +42,10 @@ import java.util.Optional; import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionTargetSim; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -489,6 +493,68 @@ class PhotonPoseEstimatorTest { assertEquals(1, pose.getZ(), .01); } + @Test + void pnpDistanceTrigSolve() { + PhotonCameraInjector cameraOne = new PhotonCameraInjector(); + PhotonCameraSim cameraOneSim = + new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG()); + + List simTargets = + aprilTags.getTags().stream() + .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID)) + .toList(); + + /* Compound Rolled + Pitched + Yaw */ + + Transform3d compoundTestTransform = + new Transform3d( + -Units.inchesToMeters(12), + -Units.inchesToMeters(11), + 3, + new Rotation3d( + Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60))); + + var estimator = + new PhotonPoseEstimator( + aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); + + /* this is the real pose of the robot base we test against */ + var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); + PhotonPipelineResult result = + cameraOneSim.process( + 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + + estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); + + var estimatedPose = estimator.update(result); + var pose = estimatedPose.get().estimatedPose; + + assertEquals(realPose.getX(), pose.getX(), .01); + assertEquals(realPose.getY(), pose.getY(), .01); + assertEquals(0.0, pose.getZ(), .01); + + /* Straight on */ + + Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, new Rotation3d(0, 0, 0)); + + estimator.setRobotToCameraTransform(straightOnTestTransform); + + /* Pose to compare with */ + realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); + result = + cameraOneSim.process( + 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + + estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); + + estimatedPose = estimator.update(result); + pose = estimatedPose.get().estimatedPose; + + assertEquals(realPose.getX(), pose.getX(), .01); + assertEquals(realPose.getY(), pose.getY(), .01); + assertEquals(0.0, pose.getZ(), .01); + } + @Test void cacheIsInvalidated() { PhotonCameraInjector cameraOne = new PhotonCameraInjector();