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

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

Browse Source 
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.
This commit is contained in:
Julius
2025-02-13 11:45:18 -08:00
committed by GitHub
parent a546ff0819
commit 01a3d31734
2 changed files with 178 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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