Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

[photon-lib] Python support for PNP_DISTANCE_TRIG_SOLVE (#2015)

Browse Source 
This adds support for PNP_DISTANCE_TRIG_SOLVE in the the python
PhotonPoseEstimator, mirroring the implementation in the Java
PhotonPoseEstimator.

Changes:
- Add PoseStrategy.PNP_DISTANCE_TRIG_SOLVE
- Add addHeadingData() and resetHeadingData() to PhotonPoseEstimator
- Fix PhotonCameraSim.process() to set ntReceiveTimestampMicros in the
result
- Minor readability improvements to PhotonPipelineResult
- Minor test improvements to PhotonPoseEstimatorTest
- Add .vscode/settings.json (to make running python tests in VSCode
easier)

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [ ] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2024.3.1
- [ ] If this PR touches pipeline settings or anything related to data
exchange, the frontend typing is updated
- [ ] If this PR addresses a bug, a regression test for it is added

---------

Co-authored-by: Sam948-byte <samf.236@proton.me>
This commit is contained in:
Kevin Cooney
2025-08-01 11:04:01 -07:00
committed by GitHub
parent cefaa313df
commit 29e24bbac2
7 changed files with 250 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,6 +27,7 @@ package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
@@ -526,63 +527,63 @@ class PhotonPoseEstimatorTest {
@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();
try (PhotonCameraSim cameraOneSim =
new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
/* 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)));
/* Compound Rolled + Pitched + Yaw */
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
Transform3d compoundTestTransform =
new Transform3d(
-Units.inchesToMeters(12),
-Units.inchesToMeters(11),
3,
new Rotation3d(
Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60)));
/* 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);
var bestTarget = result.getBestTarget();
assertNotNull(bestTarget);
assertEquals(0, bestTarget.fiducialId);
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
/* 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);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
/* Straight on */
Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);
var estimatedPose = estimator.update(result);
var pose = estimatedPose.get().estimatedPose;
estimator.setRobotToCameraTransform(straightOnTestTransform);
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
/* 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);
/* Straight on */
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
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);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
}
}
@Test