test yaw/pitch accuracy

photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java

@@ -62,14 +62,16 @@ public FrameStaticProperties(
         imageArea = this.imageWidth * this.imageHeight;
 
         // pinhole model calculations
-        if (cameraCalibration != null) { // Use calibration data
+        if (cameraCalibration != null && cameraCalibration.getCameraIntrinsicsMat() != null) {
+            // Use calibration data
             var camIntrinsics = cameraCalibration.getCameraIntrinsicsMat();
             centerX = camIntrinsics.get(0, 2)[0];
             centerY = camIntrinsics.get(1, 2)[0];
             centerPoint = new Point(centerX, centerY);
             horizontalFocalLength = camIntrinsics.get(0, 0)[0];
             verticalFocalLength = camIntrinsics.get(1, 1)[0];
-        } else { // No calibration data. Calculate from user provided diagonal FOV
+        } else {
+            // No calibration data. Calculate from user provided diagonal FOV
             centerX = (this.imageWidth / 2.0) - 0.5;
             centerY = (this.imageHeight / 2.0) - 0.5;
             centerPoint = new Point(centerX, centerY);

photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java

@@ -19,11 +19,17 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.util.stream.Stream;
 import org.junit.jupiter.api.Assertions;
-import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+import org.opencv.calib3d.Calib3d;
 import org.opencv.core.*;
-import org.opencv.core.Point;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.numbers.DoubleCouple;
@@ -51,13 +57,13 @@ public class TargetCalculationsTest {
                     props.verticalFocalLength,
                     imageSize.width * imageSize.height);
 
-    @BeforeEach
-    public void Init() {
+    @BeforeAll
+    public static void setup() {
         TestUtils.loadLibraries();
     }
 
     @Test
-    public void yawPitchTest() {
+    public void testYawPitchBehavior() {
         double targetPixelOffsetX = 100;
         double targetPixelOffsetY = 100;
         var targetCenterPoint =
@@ -98,8 +104,57 @@ public void yawPitchTest() {
         assertEquals(fovs.getSecond() / 2.0, maxPitch.getSecond(), 0.025, "Vertical FOV check failed");
     }
 
+    private static Stream<Arguments> testYawPitchCalcArgs() {
+        return Stream.of(
+                // (yaw, pitch) in degrees
+                Arguments.of(0, 0),
+                Arguments.of(10, 0),
+                Arguments.of(0, 10),
+                Arguments.of(10, 10),
+                Arguments.of(-10, -10),
+                Arguments.of(30, 45),
+                Arguments.of(-45, -20));
+    }
+
+    private static double[] testCameraMatrix = {240, 0, 320, 0, 240, 320, 0, 0, 1};
+
+    @ParameterizedTest
+    @MethodSource("testYawPitchCalcArgs")
+    public void testYawPitchCalc(double yawDeg, double pitchDeg) {
+        Mat testCameraMat = new Mat(3, 3, CvType.CV_64F);
+        testCameraMat.put(0, 0, testCameraMatrix);
+        // Since we create this translation using the given yaw/pitch, we should see the same angles
+        // calculated
+        var targetTrl =
+                new Translation3d(1, new Rotation3d(0, Math.toRadians(pitchDeg), Math.toRadians(yawDeg)));
+        // NWU to EDN
+        var objectPoints =
+                new MatOfPoint3f(new Point3(-targetTrl.getY(), -targetTrl.getZ(), targetTrl.getX()));
+        var imagePoints = new MatOfPoint2f();
+        // Project translation into camera image
+        Calib3d.projectPoints(
+                objectPoints,
+                new MatOfDouble(0, 0, 0),
+                new MatOfDouble(0, 0, 0),
+                testCameraMat,
+                new MatOfDouble(0, 0, 0, 0, 0),
+                imagePoints);
+        var point = imagePoints.toArray()[0];
+        // Test if the target yaw/pitch calculation matches what the target was created with
+        var yawPitch =
+                TargetCalculations.calculateYawPitch(
+                        point.x,
+                        testCameraMatrix[2],
+                        testCameraMatrix[0],
+                        point.y,
+                        testCameraMatrix[5],
+                        testCameraMatrix[4]);
+        assertEquals(yawDeg, yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect");
+        assertEquals(pitchDeg, yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect");
+    }
+
     @Test
-    public void targetOffsetTest() {
+    public void testTargetOffset() {
         Point center = new Point(0, 0);
         Size rectSize = new Size(10, 5);
         double angle = 30;
@@ -116,11 +171,6 @@ public void targetOffsetTest() {
         assertEquals(2.17, result.y, 0.1, "Target offset Y not as expected");
     }
 
-    public static void main(String[] args) {
-        TestUtils.loadLibraries();
-        new TargetCalculationsTest().targetOffsetTest();
-    }
-
     @Test
     public void testSkewCalculation() {
         // Setup
@@ -209,7 +259,7 @@ public void testCameraFOVCalculation() {
     }
 
     @Test
-    public void robotOffsetDualTest() {
+    public void testDualOffsetCrosshair() {
         final DualOffsetValues dualOffsetValues =
                 new DualOffsetValues(
                         new Point(400, 150), 10,

photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java

@@ -297,12 +297,12 @@ public void testYawAngles(double testYaw) {
         var res = camera.getLatestResult();
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
-        assertEquals(tgt.getYaw(), testYaw, kRotDeltaDeg);
+        assertEquals(testYaw, tgt.getYaw(), kRotDeltaDeg);
     }
 
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
-    public void testCameraPitch(double testPitch) {
+    public void testPitchAngles(double testPitch) {
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4));
         final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0));
@@ -331,7 +331,7 @@ public void testCameraPitch(double testPitch) {
         assertEquals(testPitch, tgt.getPitch(), kRotDeltaDeg);
     }
 
-    private static Stream<Arguments> distCalCParamProvider() {
+    private static Stream<Arguments> testDistanceCalcArgs() {
         // Arbitrary and fairly random assortment of distances, camera pitches, and heights
         return Stream.of(
                 Arguments.of(5, -15.98, 0),
@@ -354,7 +354,7 @@ private static Stream<Arguments> distCalCParamProvider() {
     }
 
     @ParameterizedTest
-    @MethodSource("distCalCParamProvider")
+    @MethodSource("testDistanceCalcArgs")
     public void testDistanceCalc(double testDist, double testPitch, double testHeight) {
         // Assume dist along ground and tgt height the same. Iterate over other parameters.