Cleanup and support opencv8

docs/source/docs/contributing/design-descriptions/image-rotation.md

@@ -15,7 +15,7 @@ Translation2d rotated = tag_corner1.relativeTo(ORIGIN_ROTATED_90_CCW);
 
 ## Image Distortion
 
-The distortion coefficients for OPENCV8 is given in order [k1 k2 p1 p2 ke k4 k5 k6]. Mrcal names these coefficients `[k_0 k_1, k_2, k_3, k_4, k_5, k_6, k_7]`.
+The distortion coefficients for OPENCV8 is given in order `[k1 k2 p1 p2 k3 k4 k5 k6]`. Mrcal names these coefficients `[k_0 k_1, k_2, k_3, k_4, k_5, k_6, k_7]`.
 
 ```{math}
     \begin{align*}
@@ -53,4 +53,9 @@ Let's try a concrete example. With a 90 degree CCW rotation, we have {math}`P0=-
     \end{align*}
 ```
 
-So how do I get from here to actually rotating k2/k3? lol
+By inspection, this results in just applying another 90 degree rotation to the k2/k3 parameters. Proof is left as an exercise for the reader. Note that we can repeat this rotation to yield equations for tangential distortion for 180 and 270 degrees.
+
+```{math}
+    k_2'=-k_3
+    k_3'=k_2
+```

photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java

@@ -158,6 +158,13 @@ public void setPipelineSettings(List<CVPipelineSettings> settings) {
         pipelineSettings = settings;
     }
 
+    /**
+     * Replace a calibration in our list with the same unrotatedImageSize with a new one, or add it if
+     * none exists yet. If we are replacing an existing calibration, the old one will be "released"
+     * and the underlying data matrices will become invalid.
+     *
+     * @param calibration The calibration to add.
+     */
     public void addCalibration(CameraCalibrationCoefficients calibration) {
         logger.info("adding calibration " + calibration.unrotatedImageSize);
         calibrations.stream()

photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java

@@ -123,6 +123,7 @@ public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotati
         double p1 = getDistCoeffsMat().get(0, 2)[0];
         double p2 = getDistCoeffsMat().get(0, 3)[0];
 
+        // A bunch of horrifying opaque rotation black magic. See image-rotation.md for more details.
         switch (rotation) {
             case DEG_0:
                 break;

photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java

@@ -26,7 +26,6 @@
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
-import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.vision.opencv.Releasable;
 
 /** JSON-serializable image. Data is stored as a raw JSON array. */
@@ -41,6 +40,7 @@ public class JsonMatOfDouble implements Releasable {
     @JsonIgnore private Matrix wpilibMat = null;
 
     @JsonIgnore private MatOfDouble wrappedMatOfDouble;
+    private boolean released = false;
 
     public JsonMatOfDouble(int rows, int cols, double[] data) {
         this(rows, cols, CvType.CV_64FC1, data);
@@ -77,34 +77,27 @@ public Mat getAsMat() {
             this.wrappedMat.put(0, 0, this.data);
         }
 
-        if (this.rows == 0 || this.cols == 0) {
-            throw new RuntimeException("Couldn't make cal mat??");
-        }
-        if (this.wrappedMat.rows() != this.rows) {
-            // whack
-            throw new RuntimeException("Couldn't make cal mat??");
-        }
-        if (this.wrappedMat.cols() != this.cols) {
-            // whack
-            throw new RuntimeException("Couldn't make cal mat??");
-        }
-        if (this.wrappedMat.type() != this.type) {
-            // whack
-            throw new RuntimeException("Couldn't make cal mat??");
+        if (this.released) {
+            throw new RuntimeException("This calibration object was already released");
         }
 
         return this.wrappedMat;
     }
 
     @JsonIgnore
     public MatOfDouble getAsMatOfDouble() {
+        if (this.released) {
+            throw new RuntimeException("This calibration object was already released");
+        }
+
         if (this.wrappedMatOfDouble == null) {
             this.wrappedMatOfDouble = new MatOfDouble();
             getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F);
         }
         return this.wrappedMatOfDouble;
     }
 
+    @SuppressWarnings("unchecked")
     @JsonIgnore
     public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
         if (wpilibMat == null) {
@@ -116,11 +109,7 @@ public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
     @Override
     public void release() {
         getAsMat().release();
-    }
-
-    public Packet populatePacket(Packet packet) {
-        packet.encode(this.data);
-        return packet;
+        this.released = true;
     }
 
     @Override

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

@@ -22,10 +22,9 @@
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.opencv.ImageRotationMode;
-import org.photonvision.vision.opencv.Releasable;
 
 /** Represents the properties of a frame. */
-public class FrameStaticProperties implements Releasable {
+public class FrameStaticProperties {
     public final int imageWidth;
     public final int imageHeight;
     public final double fov;
@@ -37,6 +36,7 @@ public class FrameStaticProperties implements Releasable {
     public final double verticalFocalLength;
     public CameraCalibrationCoefficients cameraCalibration;
 
+    // CameraCalibrationCoefficients hold native memory, so cache them here to avoid extra allocations
     private final FrameStaticProperties[] cachedRotationStaticProperties =
             new FrameStaticProperties[4];
 
@@ -70,9 +70,6 @@ public FrameStaticProperties(
         if (cameraCalibration != null && cameraCalibration.getCameraIntrinsicsMat() != null) {
             // Use calibration data
             var camIntrinsics = cameraCalibration.getCameraIntrinsicsMat();
-            if (camIntrinsics.rows() == 0) {
-                throw new RuntimeException("wut");
-            }
             centerX = camIntrinsics.get(0, 2)[0];
             centerY = camIntrinsics.get(1, 2)[0];
             centerPoint = new Point(centerX, centerY);
@@ -97,6 +94,7 @@ public FrameStaticProperties rotate(ImageRotationMode rotation) {
         if (rotation == ImageRotationMode.DEG_0) {
             return this;
         }
+
         int newWidth = imageWidth;
         int newHeight = imageHeight;
 
@@ -137,14 +135,4 @@ public static DoubleCouple calculateHorizontalVerticalFoV(
 
         return new DoubleCouple(Math.toDegrees(horizontalView), Math.toDegrees(verticalView));
     }
-
-    @Override
-    public void release() {
-        for (FrameStaticProperties prop : cachedRotationStaticProperties) {
-            if (prop != null) {
-                prop.release();
-            }
-        }
-        cameraCalibration.release();
-    }
 }

photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java

@@ -44,6 +44,14 @@ private ImageRotationMode(int value, Rotation2d tr) {
         this.rotation2d = tr;
     }
 
+    /**
+     * Rotate a point in an image
+     *
+     * @param point The point in the unrotated image
+     * @param width Image width, in pixels
+     * @param height Image height, in pixels
+     * @return The point in the rotated frame
+     */
     public Point rotatePoint(Point point, double width, double height) {
         Pose2d offset;
         switch (this) {

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java

@@ -107,10 +107,6 @@ public void addCalibration(CameraCalibrationCoefficients calibrationCoefficients
 
     private void calculateFrameStaticProps() {
         var videoMode = getCurrentVideoMode();
-        if (this.frameStaticProperties != null) {
-            // TODO: needs more thought on ownership model...
-            // this.frameStaticProperties.release();
-        }
         this.frameStaticProperties =
                 new FrameStaticProperties(
                         videoMode,

photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java

@@ -113,8 +113,6 @@ public void testUndistortImagePointsWithRotation(@Enum ImageRotationMode rot) {
 
         FrameStaticProperties rotatedFrameProps = frameProps.rotate(rot);
 
-        CameraCalibrationCoefficients rotatedCoeffs = rotatedFrameProps.cameraCalibration;
-
         Point[] originalPoints = {new Point(100, 100), new Point(200, 200), new Point(300, 100)};
 
         // Distort the origional points

photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java

@@ -252,10 +252,12 @@ private static Translation3d translationNWUtoEDN(Translation3d trl) {
     }
 
     /**
-     * Distort a list of points in
+     * Distort a list of points in pixels using the OPENCV5/8 models. See image-rotation.md or
+     * https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html for the math here.
      *
-     * @param src
-     * @param dst
+     * @param pointsList the undistorted points
+     * @param cameraMatrix standard OpenCV camera mat
+     * @param distCoeffs standard OpenCV distortion coefficeints. Must OPENCV5 or OPENCV8
      */
     public static List<Point> distortPoints(
             List<Point> pointsList, Mat cameraMatrix, Mat distCoeffs) {
@@ -265,26 +267,39 @@ public static List<Point> distortPoints(
         var cy = cameraMatrix.get(1, 2)[0];
         var fx = cameraMatrix.get(0, 0)[0];
         var fy = cameraMatrix.get(1, 1)[0];
+
         var k1 = distCoeffs.get(0, 0)[0];
         var k2 = distCoeffs.get(0, 1)[0];
-        var k3 = distCoeffs.get(0, 4)[0];
         var p1 = distCoeffs.get(0, 2)[0];
         var p2 = distCoeffs.get(0, 3)[0];
+        var k3 = distCoeffs.get(0, 4)[0];
+
+        double k4 = 0;
+        double k5 = 0;
+        double k6 = 0;
+        if (distCoeffs.cols() == 8) {
+            k4 = distCoeffs.get(0, 5)[0];
+            k5 = distCoeffs.get(0, 6)[0];
+            k6 = distCoeffs.get(0, 7)[0];
+        }
 
         for (Point point : pointsList) {
             // To relative coordinates
-            double x = (point.x - cx) / fx; // cx, cy is the center of distortion
-            double y = (point.y - cy) / fy;
+            double xprime = (point.x - cx) / fx; // cx, cy is the center of distortion
+            double yprime = (point.y - cy) / fy;
 
-            double r2 = x * x + y * y; // square of the radius from center
+            double r_sq = xprime * xprime + yprime * yprime; // square of the radius from center
 
             // Radial distortion
-            double xDistort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
-            double yDistort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
+            double radialDistortion =
+                    (1 + k1 * r_sq + k2 * r_sq * r_sq + k3 * r_sq * r_sq * r_sq)
+                            / (1 + k4 * r_sq + k5 * r_sq * r_sq + k6 * r_sq * r_sq * r_sq);
+            double xDistort = xprime * radialDistortion;
+            double yDistort = yprime * radialDistortion;
 
             // Tangential distortion
-            xDistort = xDistort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x));
-            yDistort = yDistort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y);
+            xDistort = xDistort + (2 * p1 * xprime * yprime + p2 * (r_sq + 2 * xprime * xprime));
+            yDistort = yDistort + (p1 * (r_sq + 2 * yprime * yprime) + 2 * p2 * xprime * yprime);
 
             // Back to absolute coordinates.
             xDistort = xDistort * fx + cx;