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

Open up pose estimator strategy methods (#2252)

Browse Source
This commit is contained in:
Gold856
2026-01-11 13:58:53 -05:00
committed by GitHub
parent 8e9fe38477
commit a5dc9ec0d6
17 changed files with 2493 additions and 1052 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,11 +24,8 @@
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.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
@@ -39,13 +36,11 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
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.util.Units;
import edu.wpi.first.util.RuntimeLoader;
@@ -57,8 +52,6 @@ import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
@@ -170,10 +163,10 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateLowestAmbiguityPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
@@ -257,11 +250,10 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToCameraHeightPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
@@ -345,12 +337,11 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToReferencePose(
cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
@@ -434,13 +425,11 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateClosestToReferencePose(
cameraOne.result, new Pose3d(1, 1, 1, new Rotation3d()));
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
@@ -514,7 +503,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
estimatedPose = estimator.update(cameraOne.result);
estimatedPose = estimator.estimateClosestToReferencePose(cameraOne.result, pose);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
@@ -542,9 +531,7 @@ class PhotonPoseEstimatorTest {
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
var estimator = new PhotonPoseEstimator(aprilTags, 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));
@@ -556,7 +543,7 @@ class PhotonPoseEstimatorTest {
assertEquals(0, bestTarget.fiducialId);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
var estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
@@ -575,7 +562,7 @@ class PhotonPoseEstimatorTest {
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
@@ -584,82 +571,6 @@ class PhotonPoseEstimatorTest {
}
}
@Test
void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
20_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Set new field layout -- right after, the pose cache timestamp should be -1
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
cameraOne.result =
@@ -735,11 +646,10 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateAverageBestTargetsPose(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
@@ -749,8 +659,9 @@ class PhotonPoseEstimatorTest {
}
@Test
void testMultiTagOnRioFallback() {
cameraOne.result =
void testMultiTagOnCoprocFallback() {
PhotonCameraInjector camera = new PhotonCameraInjector();
camera.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
@@ -799,18 +710,21 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, Transform3d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.estimateCoprocMultiTagPose(camera.result);
assertTrue(estimatedPose.isEmpty());
estimatedPose = estimator.estimateLowestAmbiguityPose(camera.result);
assertTrue(estimatedPose.isPresent());
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), 1e-9);
assertEquals(3, pose.getY(), 1e-9);
assertEquals(2, pose.getZ(), 1e-9);
}
@Test
@@ -888,34 +802,17 @@ class PhotonPoseEstimatorTest {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), kRobotToCam);
var multiTagEstimate = estimator.estimateCoprocMultiTagPose(result);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
estimator.estimateConstrainedSolvepnpPose(
result, cameraMat, distortion, multiTagEstimate.get().estimatedPose, true, 0);
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");