From 84dfe891887cfcbb54b1ab1fbe7ec7e0454d5b87 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Feb 2025 15:44:07 -0800 Subject: [PATCH 1/5] Basic cleanup --- .../org/photonvision/PhotonPoseEstimator.java | 28 ++++++++----------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 8f3272b693..4bdbd60e03 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -446,26 +446,24 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline .getTranslation() .rotateBy( new Rotation3d( - getRobotToCameraTransform().getRotation().getX(), - getRobotToCameraTransform().getRotation().getY(), - 0)) + robotToCamera.getRotation().getX(), robotToCamera.getRotation().getY(), 0)) .toTranslation2d(); - if (headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) { + var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds()); + if (headingSampleOpt.isEmpty()) { return Optional.empty(); } - - Rotation2d headingSample = headingBuffer.getSample(result.getTimestampSeconds()).get(); + Rotation2d headingSample = headingSampleOpt.get(); Rotation2d camToTagRotation = headingSample.plus( - getRobotToCameraTransform() - .getRotation() - .toRotation2d() - .plus(camToTagTranslation.getAngle())); + robotToCamera.getRotation().toRotation2d().plus(camToTagTranslation.getAngle())); - if (fieldTags.getTagPose(bestTarget.getFiducialId()).isEmpty()) return Optional.empty(); - var tagPose2d = fieldTags.getTagPose(bestTarget.getFiducialId()).get().toPose2d(); + var tagPoseOpt = fieldTags.getTagPose(bestTarget.getFiducialId()); + if (tagPoseOpt.isEmpty()) { + return Optional.empty(); + } + var tagPose2d = tagPoseOpt.get().toPose2d(); Translation2d fieldToCameraTranslation = new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) @@ -475,12 +473,10 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline Pose2d robotPose = new Pose2d( fieldToCameraTranslation, - headingSample.plus(getRobotToCameraTransform().getRotation().toRotation2d())) + headingSample.plus(robotToCamera.getRotation().toRotation2d())) .transformBy( new Transform2d( - new Pose3d( - getRobotToCameraTransform().getTranslation(), - getRobotToCameraTransform().getRotation()) + new Pose3d(robotToCamera.getTranslation(), robotToCamera.getRotation()) .toPose2d(), Pose2d.kZero)); From 7207903dff212e2ef3d5b4d6c0a96bdd8495e516 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Feb 2025 16:13:08 -0800 Subject: [PATCH 2/5] Clean up transforms --- .../org/photonvision/PhotonPoseEstimator.java | 24 ++++++++----------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 4bdbd60e03..617e03c43e 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -432,18 +432,12 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline if (bestTarget == null) return Optional.empty(); Translation2d camToTagTranslation = - new Pose3d( - Translation3d.kZero, + new Translation3d( + bestTarget.getBestCameraToTarget().getTranslation().getNorm(), 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( robotToCamera.getRotation().getX(), robotToCamera.getRotation().getY(), 0)) @@ -466,9 +460,11 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline var tagPose2d = tagPoseOpt.get().toPose2d(); Translation2d fieldToCameraTranslation = - new Pose2d(tagPose2d.getTranslation(), camToTagRotation.plus(Rotation2d.kPi)) - .transformBy(new Transform2d(camToTagTranslation.getNorm(), 0, Rotation2d.kZero)) - .getTranslation(); + tagPose2d + .getTranslation() + .plus( + new Translation2d( + camToTagTranslation.getNorm(), camToTagRotation.plus(Rotation2d.kPi))); Pose2d robotPose = new Pose2d( @@ -476,9 +472,9 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline headingSample.plus(robotToCamera.getRotation().toRotation2d())) .transformBy( new Transform2d( - new Pose3d(robotToCamera.getTranslation(), robotToCamera.getRotation()) - .toPose2d(), - Pose2d.kZero)); + robotToCamera.getTranslation().toTranslation2d(), + robotToCamera.getRotation().toRotation2d()) + .inverse()); robotPose = new Pose2d(robotPose.getTranslation(), headingSample); From eaa70a951ac702845653f9da1b4b35ad5675f0ca Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Feb 2025 16:20:45 -0800 Subject: [PATCH 3/5] Make camToTagTranslation be in the field frame --- .../org/photonvision/PhotonPoseEstimator.java | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 617e03c43e..9d9be35de3 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -431,6 +431,12 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline if (bestTarget == null) return Optional.empty(); + var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds()); + if (headingSampleOpt.isEmpty()) { + return Optional.empty(); + } + Rotation2d headingSample = headingSampleOpt.get(); + Translation2d camToTagTranslation = new Translation3d( bestTarget.getBestCameraToTarget().getTranslation().getNorm(), @@ -438,20 +444,11 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline 0, -Math.toRadians(bestTarget.getPitch()), -Math.toRadians(bestTarget.getYaw()))) - .rotateBy( - new Rotation3d( - robotToCamera.getRotation().getX(), robotToCamera.getRotation().getY(), 0)) - .toTranslation2d(); - - var headingSampleOpt = headingBuffer.getSample(result.getTimestampSeconds()); - if (headingSampleOpt.isEmpty()) { - return Optional.empty(); - } - Rotation2d headingSample = headingSampleOpt.get(); + .rotateBy(robotToCamera.getRotation()) + .toTranslation2d() + .rotateBy(headingSample); - Rotation2d camToTagRotation = - headingSample.plus( - robotToCamera.getRotation().toRotation2d().plus(camToTagTranslation.getAngle())); + Rotation2d camToTagRotation = camToTagTranslation.getAngle(); var tagPoseOpt = fieldTags.getTagPose(bestTarget.getFiducialId()); if (tagPoseOpt.isEmpty()) { From f3ec072cad83e6d69beedb0f8ba8a010488ed365 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Feb 2025 16:31:20 -0800 Subject: [PATCH 4/5] Clean up camToTagTranslation usage --- .../main/java/org/photonvision/PhotonPoseEstimator.java | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 9d9be35de3..927a6acde3 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -448,8 +448,6 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline .toTranslation2d() .rotateBy(headingSample); - Rotation2d camToTagRotation = camToTagTranslation.getAngle(); - var tagPoseOpt = fieldTags.getTagPose(bestTarget.getFiducialId()); if (tagPoseOpt.isEmpty()) { return Optional.empty(); @@ -457,11 +455,7 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline var tagPose2d = tagPoseOpt.get().toPose2d(); Translation2d fieldToCameraTranslation = - tagPose2d - .getTranslation() - .plus( - new Translation2d( - camToTagTranslation.getNorm(), camToTagRotation.plus(Rotation2d.kPi))); + tagPose2d.getTranslation().plus(camToTagTranslation.unaryMinus()); Pose2d robotPose = new Pose2d( From 1f5851d304abce2a444d7168c8335b8db8983948 Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Fri, 14 Feb 2025 16:43:41 -0800 Subject: [PATCH 5/5] Refactor robotPose calculation Use translation operations instead of transforms --- .../org/photonvision/PhotonPoseEstimator.java | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 927a6acde3..28506db79c 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -33,7 +33,6 @@ 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; @@ -457,17 +456,11 @@ private Optional pnpDistanceTrigSolveStrategy(PhotonPipeline Translation2d fieldToCameraTranslation = tagPose2d.getTranslation().plus(camToTagTranslation.unaryMinus()); + Translation2d camToRobotTranslation = + robotToCamera.getTranslation().toTranslation2d().unaryMinus().rotateBy(headingSample); + Pose2d robotPose = - new Pose2d( - fieldToCameraTranslation, - headingSample.plus(robotToCamera.getRotation().toRotation2d())) - .transformBy( - new Transform2d( - robotToCamera.getTranslation().toTranslation2d(), - robotToCamera.getRotation().toRotation2d()) - .inverse()); - - robotPose = new Pose2d(robotPose.getTranslation(), headingSample); + new Pose2d(fieldToCameraTranslation.plus(camToRobotTranslation), headingSample); return Optional.of( new EstimatedRobotPose(