From cb6af1c99d7d41d3ead69522b52896c3237cfa5d Mon Sep 17 00:00:00 2001 From: amquake Date: Sun, 15 Oct 2023 02:28:41 -0700 Subject: [PATCH] test yaw/pitch accuracy --- .../vision/frame/FrameStaticProperties.java | 6 +- .../vision/target/TargetCalculationsTest.java | 74 ++++++++++++++++--- .../org/photonvision/VisionSystemSimTest.java | 8 +- 3 files changed, 70 insertions(+), 18 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java b/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java index 5a35388951..cedda04d22 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java +++ b/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); diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index c6f848c185..7b23d3a600 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/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 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, diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 5ab7d00ce2..c53e006717 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/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 distCalCParamProvider() { + private static Stream 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 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.