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

RobotPoseEstimator Enhancements (#677)

Browse Source 
* Use List for RobotPoseEstimator constructor

* Update `RobotPoseEstimator` constructor to accept wpilib `AprilTagFieldLayout` java

* Initial cpp changes

* Java return optional from update

* Fix java test

* Clean up strategy switch

* small lint

* Actually link to vision_shared

* Fix auto optimized imports

* format

* report error

* small method changes

* format and clean up

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Nick Hadley
2023-01-02 19:22:39 -05:00
committed by GitHub
parent 971b471f92
commit 0b5256df12
7 changed files with 171 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java
View File

@@ -42,6 +42,7 @@ import edu.wpi.first.util.WPIUtilJNI;
import java.io.IOException;
import java.util.ArrayList;
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;
@@ -138,10 +139,10 @@ class RobotPoseEstimatorTest {
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.getSecond());
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(1, pose.getX(), .01);
assertEquals(3, pose.getY(), .01);
assertEquals(2, pose.getZ(), .01);
@@ -210,10 +211,10 @@ class RobotPoseEstimatorTest {
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.getSecond());
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);
@@ -283,10 +284,10 @@ class RobotPoseEstimatorTest {
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(4, estimatedPose.getSecond());
assertEquals(4, estimatedPose.get().getSecond());
assertEquals(1, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(.9, pose.getZ(), .01);
@@ -357,8 +358,8 @@ class RobotPoseEstimatorTest {
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
cameraOne.result =
new PhotonPipelineResult(
@@ -412,9 +413,9 @@ class RobotPoseEstimatorTest {
new TargetCorner(7, 8)))));
estimatedPose = estimator.update();
pose = estimatedPose.getFirst();
pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.getSecond());
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(.9, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(1, pose.getZ(), .01);
@@ -483,9 +484,9 @@ class RobotPoseEstimatorTest {
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
Pair<Pose3d, Double> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.getFirst();
assertEquals(2.6885245901639347, estimatedPose.getSecond(), .01);
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
assertEquals(2.15, pose.getX(), .01);
assertEquals(2.15, pose.getY(), .01);
assertEquals(2.15, pose.getZ(), .01);