Add 6328's implementation of PNP distance for Trig Solving to PhotonPoseEstimator (PhotonVision#1767)

JuliusZhou124 · samdev-7 · commit 199f0fa92868 · 2025-02-15T23:20:00.000-05:00
https://discord.com/channels/725836368059826228/725846784131203222/1334309604946874460 https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/85 Helps with ambiguous single tag estimates and produces more stability.
diff --git 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.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 enum PoseStrategy {
          * 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.
+         *
+         * <p>Yields a Pose2d in estimatedRobotPose (0 for z, roll, pitch)
+         *
+         * <p>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 enum PoseStrategy {
     protected double poseCacheTimestampSeconds = -1;
     private final Set<Integer> reportedErrors = new HashSet<>();
 
+    private final TimeInterpolatableBuffer<Rotation2d> headingBuffer =
+            TimeInterpolatableBuffer.createBuffer(1.0);
+
     /**
      * Create a new PhotonPoseEstimator.
      *
@@ -259,6 +277,30 @@ public void setLastPose(Pose2d lastPose) {
         setLastPose(new Pose3d(lastPose));
     }
 
+    /**
+     * Add robot heading data to buffer. Must be called periodically for the
+     * <b>PNP_DISTANCE_TRIG_SOLVE</b> 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
+     * <b>PNP_DISTANCE_TRIG_SOLVE</b> 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 @@ private Optional<EstimatedRobotPose> update(
                     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 @@ private Optional<EstimatedRobotPose> update(
         return estimatedPose;
     }
 
+    private Optional<EstimatedRobotPose> 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<EstimatedRobotPose> 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
@@ -42,6 +42,10 @@
 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 @@ void closestToLastPose() {
         assertEquals(1, pose.getZ(), .01);
     }
 
+    @Test
+    void pnpDistanceTrigSolve() {
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        PhotonCameraSim cameraOneSim =
+                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG());
+
+        List<VisionTargetSim> 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();
