From 051666677e80949378e1b5db100c134b2d6aa6a1 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 18 Oct 2024 19:06:04 -0700 Subject: [PATCH] Cleanup and support opencv8 --- .../design-descriptions/image-rotation.md | 9 ++++- .../configuration/CameraConfiguration.java | 7 ++++ .../CameraCalibrationCoefficients.java | 1 + .../vision/calibration/JsonMatOfDouble.java | 29 +++++---------- .../vision/frame/FrameStaticProperties.java | 18 ++------- .../vision/opencv/ImageRotationMode.java | 8 ++++ .../processes/VisionSourceSettables.java | 4 -- .../pipeline/CalibrationRotationPipeTest.java | 2 - .../photonvision/estimation/OpenCVHelp.java | 37 +++++++++++++------ 9 files changed, 61 insertions(+), 54 deletions(-) diff --git a/docs/source/docs/contributing/design-descriptions/image-rotation.md b/docs/source/docs/contributing/design-descriptions/image-rotation.md index 692430cea9..e307c26dd8 100644 --- a/docs/source/docs/contributing/design-descriptions/image-rotation.md +++ b/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 +``` diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index 1b9a445668..93baa446ce 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -158,6 +158,13 @@ public void setPipelineSettings(List 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() diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java index bbba6cecc8..abbd510c60 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java +++ b/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; diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java index 32794f5250..d4b3a3188e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java +++ b/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,20 +77,8 @@ 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; @@ -98,6 +86,10 @@ public Mat getAsMat() { @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); @@ -105,6 +97,7 @@ public MatOfDouble getAsMatOfDouble() { return this.wrappedMatOfDouble; } + @SuppressWarnings("unchecked") @JsonIgnore public Matrix getAsWpilibMat() { if (wpilibMat == null) { @@ -116,11 +109,7 @@ public Matrix getAsWpilibMat() { @Override public void release() { getAsMat().release(); - } - - public Packet populatePacket(Packet packet) { - packet.encode(this.data); - return packet; + this.released = true; } @Override 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 fd3be04196..45af6af14d 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 @@ -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(); - } } diff --git a/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java b/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java index df632a7859..4fa3dea7e3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java +++ b/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) { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index 2c049241ca..edbf3c07aa 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/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, diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java index e5af872460..31cf4ce579 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java +++ b/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 diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 15865b5096..9f328d9aea 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/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 distortPoints( List pointsList, Mat cameraMatrix, Mat distCoeffs) { @@ -265,26 +267,39 @@ public static List 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;