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

Java-side bugfixes for RobotPoseEstimator and PhotonCamera (#685)

Browse Source 
Closes #684
Closes #683
Closes #681
Closes #680
Closes #678
This commit is contained in:
Chris Gerth
2023-01-01 21:40:48 -06:00
committed by GitHub
parent 7c49cfe625
commit aaa886bd73
5 changed files with 124 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,21 +26,22 @@ package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.io.IOException;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.RobotPoseEstimator.PoseStrategy;
@@ -49,6 +50,8 @@ import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
class RobotPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@BeforeAll
public static void init() {
JNIWrapper.Helper.setExtractOnStaticLoad(false);
@@ -63,14 +66,17 @@ class RobotPoseEstimatorTest {
// 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);
}
@Test
void testLowestAmbiguityStrategy() {
Map<Integer, Pose3d> aprilTags = new HashMap<>();
aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -143,10 +149,6 @@ class RobotPoseEstimatorTest {
@Test
void testClosestToCameraHeightStrategy() {
Map<Integer, Pose3d> aprilTags = new HashMap<>();
aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -214,15 +216,11 @@ class RobotPoseEstimatorTest {
assertEquals(2, estimatedPose.getSecond());
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(4, pose.getZ(), .01);
assertEquals(0, pose.getZ(), .01);
}
@Test
void closestToReferencePoseStrategy() {
Map<Integer, Pose3d> aprilTags = new HashMap<>();
aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -296,10 +294,6 @@ class RobotPoseEstimatorTest {
@Test
void closestToLastPose() {
Map<Integer, Pose3d> aprilTags = new HashMap<>();
aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -428,10 +422,6 @@ class RobotPoseEstimatorTest {
@Test
void averageBestPoses() {
Map<Integer, Pose3d> aprilTags = new HashMap<>();
aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();