Undistort pitch/yaw using opencv

photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java

@@ -16,13 +16,18 @@
  */
 package org.photonvision.vision.target;
 
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
+import org.opencv.core.TermCriteria;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.opencv.DualOffsetValues;
 
 public class TargetCalculations {
+
     /**
      * Calculates the yaw and pitch of a point in the image. Yaw and pitch must be calculated together
      * to account for perspective distortion. Yaw is positive right, and pitch is positive up.
@@ -33,6 +38,7 @@ public class TargetCalculations {
      * @param offsetCenterY The Y value of the offset principal point (cy) in pixels
      * @param targetCenterY The Y value of the target's center point in pixels
      * @param verticalFocalLength The vertical focal length (fy) in pixels
+     * @param cameraCal Camera calibration parameters, or null if not calibrated
      * @return The yaw and pitch from the principal axis to the target center, in degrees.
      */
     public static DoubleCouple calculateYawPitch(
@@ -41,7 +47,34 @@ public static DoubleCouple calculateYawPitch(
             double horizontalFocalLength,
             double offsetCenterY,
             double targetCenterY,
-            double verticalFocalLength) {
+            double verticalFocalLength,
+            CameraCalibrationCoefficients cameraCal) {
+
+        if (cameraCal != null) {
+            // undistort
+            MatOfPoint2f temp = new MatOfPoint2f();
+            temp.fromArray(new Point(targetCenterX, targetCenterY));
+            // Tighten up termination criteria
+            var termCriteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 1e-6);
+            Calib3d.undistortImagePoints(
+                    temp,
+                    temp,
+                    cameraCal.getCameraIntrinsicsMat(),
+                    cameraCal.getDistCoeffsMat(),
+                    termCriteria);
+            float buff[] = new float[2];
+            temp.get(0, 0, buff);
+            temp.release();
+
+            // if outside of the imager, convergence fails, or really really bad user camera cal,
+            // undistort will fail by giving us nans. at some point we should log this failure
+            // if we can't undistort, don't change the cnter location
+            if (Float.isFinite(buff[0]) && Float.isFinite(buff[1])) {
+                targetCenterX = buff[0];
+                targetCenterY = buff[1];
+            }
+        }
+
         double yaw = Math.atan((targetCenterX - offsetCenterX) / horizontalFocalLength);
         double pitch =
                 Math.atan((offsetCenterY - targetCenterY) / (verticalFocalLength / Math.cos(yaw)));

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -33,6 +33,7 @@
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVShape;
 import org.photonvision.vision.opencv.Contour;
@@ -92,7 +93,8 @@ public TrackedTarget(
                         params.horizontalFocalLength,
                         params.cameraCenterPoint.y,
                         tagDetection.getCenterY(),
-                        params.verticalFocalLength);
+                        params.verticalFocalLength,
+                        params.cameraCal);
         m_yaw = yawPitch.getFirst();
         m_pitch = yawPitch.getSecond();
         var bestPose = new Transform3d();
@@ -187,7 +189,8 @@ public TrackedTarget(
                         params.horizontalFocalLength,
                         params.cameraCenterPoint.y,
                         result.getCenterY(),
-                        params.verticalFocalLength);
+                        params.verticalFocalLength,
+                        params.cameraCal);
         m_yaw = yawPitch.getFirst();
         m_pitch = yawPitch.getSecond();
 
@@ -323,7 +326,8 @@ public void calculateValues(TargetCalculationParameters params) {
                         params.horizontalFocalLength,
                         m_robotOffsetPoint.y,
                         m_targetOffsetPoint.y,
-                        params.verticalFocalLength);
+                        params.verticalFocalLength,
+                        params.cameraCal);
         m_yaw = yawPitch.getFirst();
         m_pitch = yawPitch.getSecond();
 
@@ -480,6 +484,9 @@ public static class TargetCalculationParameters {
         // area calculation values
         final double imageArea;
 
+        // Camera calibration, null if not calibrated
+        final CameraCalibrationCoefficients cameraCal;
+
         public TargetCalculationParameters(
                 boolean isLandscape,
                 TargetOffsetPointEdge targetOffsetPointEdge,
@@ -489,7 +496,8 @@ public TargetCalculationParameters(
                 Point cameraCenterPoint,
                 double horizontalFocalLength,
                 double verticalFocalLength,
-                double imageArea) {
+                double imageArea,
+                CameraCalibrationCoefficients cal) {
 
             this.isLandscape = isLandscape;
             this.targetOffsetPointEdge = targetOffsetPointEdge;
@@ -500,6 +508,7 @@ public TargetCalculationParameters(
             this.horizontalFocalLength = horizontalFocalLength;
             this.verticalFocalLength = verticalFocalLength;
             this.imageArea = imageArea;
+            this.cameraCal = cal;
         }
 
         public TargetCalculationParameters(
@@ -520,6 +529,7 @@ public TargetCalculationParameters(
             this.horizontalFocalLength = frameStaticProperties.horizontalFocalLength;
             this.verticalFocalLength = frameStaticProperties.verticalFocalLength;
             this.imageArea = frameStaticProperties.imageArea;
+            this.cameraCal = frameStaticProperties.cameraCalibration;
         }
     }
 }

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

@@ -21,6 +21,7 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import java.util.List;
 import java.util.stream.Stream;
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeAll;
@@ -33,12 +34,15 @@
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.TestUtils;
 import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.calibration.CameraLensModel;
+import org.photonvision.vision.calibration.JsonMatOfDouble;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.DualOffsetValues;
 
 public class TargetCalculationsTest {
 
-    private static Size imageSize = new Size(800, 600);
+    private static Size imageSize = new Size(1280, 720);
     private static Point imageCenterPoint =
             new Point(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5);
     private static final double diagFOV = Math.toRadians(70.0);
@@ -55,7 +59,8 @@ public class TargetCalculationsTest {
                     imageCenterPoint,
                     props.horizontalFocalLength,
                     props.verticalFocalLength,
-                    imageSize.width * imageSize.height);
+                    imageSize.width * imageSize.height,
+                    null);
 
     @BeforeAll
     public static void setup() {
@@ -76,7 +81,8 @@ public void testYawPitchBehavior() {
                         params.horizontalFocalLength,
                         imageCenterPoint.y,
                         targetCenterPoint.y,
-                        params.verticalFocalLength);
+                        params.verticalFocalLength,
+                        params.cameraCal);
 
         assertTrue(targetYawPitch.getFirst() > 0, "Yaw is not positive right");
         assertTrue(targetYawPitch.getSecond() < 0, "Pitch is not positive up");
@@ -91,7 +97,8 @@ public void testYawPitchBehavior() {
                         params.horizontalFocalLength,
                         imageCenterPoint.y,
                         imageCenterPoint.y,
-                        params.verticalFocalLength);
+                        params.verticalFocalLength,
+                        params.cameraCal);
         assertEquals(fovs.getFirst() / 2.0, maxYaw.getFirst(), 0.025, "Horizontal FOV check failed");
         var maxPitch =
                 TargetCalculations.calculateYawPitch(
@@ -100,7 +107,8 @@ public void testYawPitchBehavior() {
                         params.horizontalFocalLength,
                         imageCenterPoint.y,
                         0,
-                        params.verticalFocalLength);
+                        params.verticalFocalLength,
+                        params.cameraCal);
         assertEquals(fovs.getSecond() / 2.0, maxPitch.getSecond(), 0.025, "Vertical FOV check failed");
     }
 
@@ -112,17 +120,40 @@ private static Stream<Arguments> testYawPitchCalcArgs() {
                 Arguments.of(0, 10),
                 Arguments.of(10, 10),
                 Arguments.of(-10, -10),
-                Arguments.of(30, 45),
-                Arguments.of(-45, -20));
+                Arguments.of(-18, 14),
+                Arguments.of(-23, -16));
     }
 
-    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);
+        // FOV: ~58.5 deg horizontal, ~35 deg vertical
+        JsonMatOfDouble testCameraMatrix =
+                new JsonMatOfDouble(
+                        3, 3, new double[] {1142.341323, 0, 621.384201309, 0, 1139.92214, 349.897631, 0, 0, 1});
+        JsonMatOfDouble testDistortion =
+                new JsonMatOfDouble(
+                        5,
+                        1,
+                        new double[] {
+                            0.186841202993646,
+                            -1.482894102216622,
+                            0.005692954661309707,
+                            0.0006757267756945662,
+                            2.8659664873321287
+                        });
+        double IMAGER_WIDTH = 1280, IMAGER_HEIGHT = 720;
+        var testCameraCal =
+                new CameraCalibrationCoefficients(
+                        imageSize,
+                        testCameraMatrix,
+                        testDistortion,
+                        new double[0],
+                        List.of(),
+                        new Size(),
+                        0,
+                        CameraLensModel.LENSMODEL_OPENCV);
+
         // Since we create this translation using the given yaw/pitch, we should see the same angles
         // calculated
         var targetTrl =
@@ -136,21 +167,33 @@ public void testYawPitchCalc(double yawDeg, double pitchDeg) {
                 objectPoints,
                 new MatOfDouble(0, 0, 0),
                 new MatOfDouble(0, 0, 0),
-                testCameraMat,
-                new MatOfDouble(0, 0, 0, 0, 0),
+                testCameraCal.getCameraIntrinsicsMat(),
+                testCameraCal.getDistCoeffsMat(),
                 imagePoints);
         var point = imagePoints.toArray()[0];
+
+        // need point within FOV to be valid
+        assertTrue(Math.abs(point.x) >= 0);
+        assertTrue(Math.abs(point.x) <= IMAGER_WIDTH);
+        assertTrue(Math.abs(point.y) >= 0);
+        assertTrue(Math.abs(point.y) <= IMAGER_HEIGHT);
+
         // Test if the target yaw/pitch calculation matches what the target was created with
         var yawPitch =
                 TargetCalculations.calculateYawPitch(
+                        testCameraCal.cameraIntrinsics.data[2],
                         point.x,
-                        testCameraMatrix[2],
-                        testCameraMatrix[0],
+                        testCameraCal.cameraIntrinsics.data[0],
+                        testCameraCal.cameraIntrinsics.data[5],
                         point.y,
-                        testCameraMatrix[5],
-                        testCameraMatrix[4]);
-        assertEquals(yawDeg, yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect");
-        assertEquals(pitchDeg, yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect");
+                        testCameraCal.cameraIntrinsics.data[4],
+                        testCameraCal);
+        // convert photon angles to wpilib NWU angles
+        assertEquals(yawDeg, -yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect");
+        assertEquals(pitchDeg, -yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect");
+
+        testCameraCal.release();
+        testDistortion.release();
     }
 
     @Test

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

@@ -59,7 +59,8 @@ void axisTest() {
                         new Point(imageSize.width / 2, imageSize.height / 2),
                         61,
                         34.3,
-                        imageSize.area());
+                        imageSize.area(),
+                        null);
 
         var trackedTarget = new TrackedTarget(pTarget, setting, null);
         // TODO change these hardcoded values