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

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