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);

66
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -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<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();