mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Return named type from PhotonPoseEstimator (#734)
Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator
This commit is contained in:
Andrew Gasser
@@ -45,12 +45,12 @@ import java.util.List;
|
||||
import java.util.Optional;
|
||||
import org.junit.jupiter.api.BeforeAll;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.RobotPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
import org.photonvision.targeting.TargetCorner;
|
||||
|
||||
class RobotPoseEstimatorTest {
|
||||
class PhotonPoseEstimatorTest {
|
||||
static AprilTagFieldLayout aprilTags;
|
||||
|
||||
@BeforeAll
|
||||
@@ -62,24 +62,22 @@ class RobotPoseEstimatorTest {
|
||||
|
||||
try {
|
||||
CombinedRuntimeLoader.loadLibraries(
|
||||
RobotPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
|
||||
PhotonPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
|
||||
} catch (IOException e) {
|
||||
// TODO Auto-generated catch block
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
List<AprilTag> atList = new ArrayList<AprilTag>(2);
|
||||
atList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
|
||||
atList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
|
||||
var fl = Units.feetToMeters(54.0);
|
||||
var fw = Units.feetToMeters(27.0);
|
||||
aprilTags = new AprilTagFieldLayout(atList, fl, fw);
|
||||
List<AprilTag> tagList = new ArrayList<AprilTag>(2);
|
||||
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
|
||||
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
|
||||
double fieldLength = Units.feetToMeters(54.0);
|
||||
double fieldWidth = Units.feetToMeters(27.0);
|
||||
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testLowestAmbiguityStrategy() {
|
||||
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
|
||||
|
||||
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
||||
cameraOne.result =
|
||||
new PhotonPipelineResult(
|
||||
@@ -122,12 +120,7 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(1, 2),
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
|
||||
cameraTwo.result =
|
||||
new PhotonPipelineResult(
|
||||
4,
|
||||
List.of(
|
||||
new TargetCorner(7, 8))),
|
||||
new PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
@@ -147,17 +140,16 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setTimestampSeconds(11);
|
||||
|
||||
cameras.add(Pair.of(cameraOne, new Transform3d()));
|
||||
cameras.add(Pair.of(cameraTwo, new Transform3d()));
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());
|
||||
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
|
||||
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().estimatedPose;
|
||||
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
|
||||
assertEquals(2, estimatedPose.get().getSecond());
|
||||
assertEquals(11, estimatedPose.get().timestampSeconds);
|
||||
assertEquals(1, pose.getX(), .01);
|
||||
assertEquals(3, pose.getY(), .01);
|
||||
assertEquals(2, pose.getZ(), .01);
|
||||
@@ -209,12 +201,7 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(1, 2),
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
|
||||
cameraTwo.result =
|
||||
new PhotonPipelineResult(
|
||||
4,
|
||||
List.of(
|
||||
new TargetCorner(7, 8))),
|
||||
new PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
@@ -235,16 +222,19 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
|
||||
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())));
|
||||
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 2), new Rotation3d())));
|
||||
cameraOne.result.setTimestampSeconds(4);
|
||||
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
aprilTags,
|
||||
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
|
||||
cameraOne,
|
||||
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
|
||||
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().estimatedPose;
|
||||
|
||||
assertEquals(2, estimatedPose.get().getSecond());
|
||||
assertEquals(4, estimatedPose.get().timestampSeconds);
|
||||
assertEquals(4, pose.getX(), .01);
|
||||
assertEquals(4, pose.getY(), .01);
|
||||
assertEquals(0, pose.getZ(), .01);
|
||||
@@ -252,8 +242,6 @@ class RobotPoseEstimatorTest {
|
||||
|
||||
@Test
|
||||
void closestToReferencePoseStrategy() {
|
||||
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
|
||||
|
||||
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
||||
cameraOne.result =
|
||||
new PhotonPipelineResult(
|
||||
@@ -296,12 +284,7 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(1, 2),
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
|
||||
cameraTwo.result =
|
||||
new PhotonPipelineResult(
|
||||
4,
|
||||
List.of(
|
||||
new TargetCorner(7, 8))),
|
||||
new PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
@@ -321,18 +304,20 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setTimestampSeconds(17);
|
||||
|
||||
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
|
||||
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
|
||||
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
aprilTags,
|
||||
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
|
||||
cameraOne,
|
||||
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
||||
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
|
||||
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().estimatedPose;
|
||||
|
||||
assertEquals(4, estimatedPose.get().getSecond());
|
||||
assertEquals(17, estimatedPose.get().timestampSeconds);
|
||||
assertEquals(1, pose.getX(), .01);
|
||||
assertEquals(1.1, pose.getY(), .01);
|
||||
assertEquals(.9, pose.getZ(), .01);
|
||||
@@ -340,8 +325,6 @@ class RobotPoseEstimatorTest {
|
||||
|
||||
@Test
|
||||
void closestToLastPose() {
|
||||
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
|
||||
|
||||
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
||||
cameraOne.result =
|
||||
new PhotonPipelineResult(
|
||||
@@ -384,12 +367,7 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(1, 2),
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
|
||||
cameraTwo.result =
|
||||
new PhotonPipelineResult(
|
||||
4,
|
||||
List.of(
|
||||
new TargetCorner(7, 8))),
|
||||
new PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
@@ -410,16 +388,17 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
|
||||
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
|
||||
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
|
||||
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_LAST_POSE, cameras);
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
aprilTags,
|
||||
PoseStrategy.CLOSEST_TO_LAST_POSE,
|
||||
cameraOne,
|
||||
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
||||
|
||||
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
|
||||
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().estimatedPose;
|
||||
|
||||
cameraOne.result =
|
||||
new PhotonPipelineResult(
|
||||
@@ -462,11 +441,7 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(1, 2),
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraTwo.result =
|
||||
new PhotonPipelineResult(
|
||||
4,
|
||||
List.of(
|
||||
new TargetCorner(7, 8))),
|
||||
new PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
@@ -486,11 +461,12 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setTimestampSeconds(7);
|
||||
|
||||
estimatedPose = estimator.update();
|
||||
pose = estimatedPose.get().getFirst();
|
||||
pose = estimatedPose.get().estimatedPose;
|
||||
|
||||
assertEquals(2, estimatedPose.get().getSecond());
|
||||
assertEquals(7, estimatedPose.get().timestampSeconds);
|
||||
assertEquals(.9, pose.getX(), .01);
|
||||
assertEquals(1.1, pose.getY(), .01);
|
||||
assertEquals(1, pose.getZ(), .01);
|
||||
@@ -542,12 +518,7 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(1, 2),
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8))))); // 2 2 2 ambig .3
|
||||
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
|
||||
cameraTwo.result =
|
||||
new PhotonPipelineResult(
|
||||
4,
|
||||
List.of(
|
||||
new TargetCorner(7, 8))), // 2 2 2 ambig .3
|
||||
new PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
@@ -567,16 +538,19 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
|
||||
cameraOne.result.setTimestampSeconds(20);
|
||||
|
||||
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
|
||||
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
aprilTags,
|
||||
PoseStrategy.AVERAGE_BEST_TARGETS,
|
||||
cameraOne,
|
||||
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
||||
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
|
||||
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().estimatedPose;
|
||||
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
|
||||
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
|
||||
assertEquals(2.15, pose.getX(), .01);
|
||||
assertEquals(2.15, pose.getY(), .01);
|
||||
assertEquals(2.15, pose.getZ(), .01);
|
||||
Reference in New Issue
Block a user