diff --git a/docs/source/conf.py b/docs/source/conf.py index 09c6604cc1..dca2301a4d 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -36,6 +36,7 @@ "sphinxcontrib.ghcontributors", "sphinx_design", "myst_parser", + "sphinx.ext.mathjax", ] # Configure OpenGraph support diff --git a/docs/source/docs/contributing/design-descriptions/image-rotation.md b/docs/source/docs/contributing/design-descriptions/image-rotation.md new file mode 100644 index 0000000000..e307c26dd8 --- /dev/null +++ b/docs/source/docs/contributing/design-descriptions/image-rotation.md @@ -0,0 +1,61 @@ +# Calibration and Image Rotation + +## Rotating Points + +To stay consistent with the OpenCV camera coordinate frame, we put the origin in the top left, with X right, Y down, and Z out (as required by the right-hand rule). Intuitively though, if I ask you to rotate an image 90 degrees clockwise though, you'd probably rotate it about -Z in this coordinate system. Just be aware of this inconsistency. + +![](images/image_corner_frames.png) + +If we have any one point in any of those coordinate systems, we can transform it into any of the other ones using standard geometry libraries by performing relative transformations (like in this pseudocode): + +``` +Translation2d tag_corner1 = new Translation2d(); +Translation2d rotated = tag_corner1.relativeTo(ORIGIN_ROTATED_90_CCW); +``` + +## Image Distortion + +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*} + \vec P &\equiv \frac{\vec p_{xy}}{p_z} \\ + r &\equiv \left|\vec P\right| \\ + \vec P_\mathrm{radial} &\equiv \frac{ 1 + k_0 r^2 + k_1 r^4 + k_4 r^6}{ 1 + k_5 r^2 + k_6 r^4 + k_7 r^6} \vec P \\ + \vec P_\mathrm{tangential} &\equiv + \left[ \begin{aligned} + 2 k_2 P_0 P_1 &+ k_3 \left(r^2 + 2 P_0^2 \right) \\ + 2 k_3 P_0 P_1 &+ k_2 \left(r^2 + 2 P_1^2 \right) + \end{aligned}\right] \\ + \vec q &= \vec f_{xy} \left( \vec P_\mathrm{radial} + \vec P_\mathrm{tangential} \right) + \vec c_{xy} + \end{align*} +``` + +From this, we observe at `k_0, k_1, k_4, k_5, k_6, k_7` depend only on the norm of {math}`\vec P`, and will be constant given a rotated image. However, `k_2` and `k_3` go with {math}`P_0 \cdot P_1`, `k_3` with {math}`P_0^2`, and `k_2` with {math}`P_1^2`. + +Let's try a concrete example. With a 90 degree CCW rotation, we have {math}`P0=-P_{1\mathrm{rotated}}` and {math}`P1=P_{0\mathrm{rotated}}`. Let's substitute in + +```{math} + \begin{align*} + \left[ \begin{aligned} + 2 k_2 P_0 P_1 &+ k_3 \left(r^2 + 2 P_0^2 \right) \\ + 2 k_3 P_0 P_1 &+ k_2 \left(r^2 + 2 P_1^2 \right) + \end{aligned}\right] &= + \left[ \begin{aligned} + 2 k_{2\mathrm{rotated}} (-P_{1\mathrm{rotated}}) P_{0\mathrm{rotated}} &+ k_{3\mathrm{rotated}} \left(r^2 + 2 (-P_{1\mathrm{rotated}})^2 \right) \\ + 2 k_{3\mathrm{rotated}} (-P_{1\mathrm{rotated}}) P_{0\mathrm{rotated}} &+ k_{2\mathrm{rotated}} \left(r^2 + 2 P_{0\mathrm{rotated}}^2 \right) + \end{aligned}\right] \\ + &= + \left[ \begin{aligned} + -2 k_{2\mathrm{rotated}} P_{1\mathrm{rotated}} P_{0\mathrm{rotated}} &+ k_{3\mathrm{rotated}} \left(r^2 + 2 P_{1\mathrm{rotated}}^2 \right) \\ + -2 k_{3\mathrm{rotated}} P_{1\mathrm{rotated}} P_{0\mathrm{rotated}} &+ k_{2\mathrm{rotated}} \left(r^2 + 2 P_{0\mathrm{rotated}}^2 \right) + \end{aligned}\right] + \end{align*} +``` + +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/docs/source/docs/contributing/design-descriptions/images/image_corner_frames.png b/docs/source/docs/contributing/design-descriptions/images/image_corner_frames.png new file mode 100644 index 0000000000..e00a898764 Binary files /dev/null and b/docs/source/docs/contributing/design-descriptions/images/image_corner_frames.png differ diff --git a/docs/source/docs/contributing/design-descriptions/index.md b/docs/source/docs/contributing/design-descriptions/index.md new file mode 100644 index 0000000000..f1a598f891 --- /dev/null +++ b/docs/source/docs/contributing/design-descriptions/index.md @@ -0,0 +1,6 @@ +# Software Architecture Design Descriptions + +```{toctree} +:maxdepth: 1 +image-rotation +``` diff --git a/docs/source/docs/contributing/index.md b/docs/source/docs/contributing/index.md index d9f1137b3e..4de6c437af 100644 --- a/docs/source/docs/contributing/index.md +++ b/docs/source/docs/contributing/index.md @@ -4,4 +4,5 @@ building-photon building-docs developer-docs/index +design-descriptions/index ``` diff --git a/photon-client/src/components/dashboard/tabs/InputTab.vue b/photon-client/src/components/dashboard/tabs/InputTab.vue index f69812ad63..d812300930 100644 --- a/photon-client/src/components/dashboard/tabs/InputTab.vue +++ b/photon-client/src/components/dashboard/tabs/InputTab.vue @@ -130,32 +130,12 @@ const interactiveCols = computed(() => tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions" @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBlueGain: args }, false)" /> - - - Warning! A known bug affects rotation of calibrated camera. Turn off rotation here and rotate using - cameraToRobotTransform in your robot code. - 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.resolution); + logger.info("adding calibration " + calibration.unrotatedImageSize); calibrations.stream() - .filter(it -> it.resolution.equals(calibration.resolution)) + .filter(it -> it.unrotatedImageSize.equals(calibration.unrotatedImageSize)) .findAny() - .ifPresent(calibrations::remove); + .ifPresent( + (it) -> { + it.release(); + calibrations.remove(it); + }); calibrations.add(calibration); } 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 99c5fc0127..2bba3c2bc7 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 @@ -27,12 +27,13 @@ import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Size; +import org.photonvision.vision.opencv.ImageRotationMode; import org.photonvision.vision.opencv.Releasable; @JsonIgnoreProperties(ignoreUnknown = true) public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("resolution") - public final Size resolution; + public final Size unrotatedImageSize; @JsonProperty("cameraIntrinsics") public final JsonMatOfDouble cameraIntrinsics; @@ -56,9 +57,6 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonProperty("lensmodel") public final CameraLensModel lensmodel; - @JsonIgnore private final double[] intrinsicsArr = new double[9]; - @JsonIgnore private final double[] distCoeffsArr = new double[5]; - /** * Contains all camera calibration data for a particular resolution of a camera. Designed for use * with standard opencv camera calibration matrices. For details on the layout of camera @@ -87,7 +85,7 @@ public CameraCalibrationCoefficients( @JsonProperty("calobjectSize") Size calobjectSize, @JsonProperty("calobjectSpacing") double calobjectSpacing, @JsonProperty("lensmodel") CameraLensModel lensmodel) { - this.resolution = resolution; + this.unrotatedImageSize = resolution; this.cameraIntrinsics = cameraIntrinsics; this.distCoeffs = distCoeffs; this.calobjectWarp = calobjectWarp; @@ -100,10 +98,93 @@ public CameraCalibrationCoefficients( observations = List.of(); } this.observations = observations; + } + + public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotation) { + if (rotation == ImageRotationMode.DEG_0) { + return this; + } + Mat rotatedIntrinsics = getCameraIntrinsicsMat().clone(); + Mat rotatedDistCoeffs = getDistCoeffsMat().clone(); + double cx = getCameraIntrinsicsMat().get(0, 2)[0]; + double cy = getCameraIntrinsicsMat().get(1, 2)[0]; + double fx = getCameraIntrinsicsMat().get(0, 0)[0]; + double fy = getCameraIntrinsicsMat().get(1, 1)[0]; + + // only adjust p1 and p2 the rest are radial distortion coefficients + + 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; + case DEG_270_CCW: + // FX + rotatedIntrinsics.put(0, 0, fy); + // FY + rotatedIntrinsics.put(1, 1, fx); + + // CX + rotatedIntrinsics.put(0, 2, unrotatedImageSize.height - cy); + // CY + rotatedIntrinsics.put(1, 2, cx); + + // P1 + rotatedDistCoeffs.put(0, 2, p2); + // P2 + rotatedDistCoeffs.put(0, 3, -p1); + + break; + case DEG_180_CCW: + // CX + rotatedIntrinsics.put(0, 2, unrotatedImageSize.width - cx); + // CY + rotatedIntrinsics.put(1, 2, unrotatedImageSize.height - cy); + + // P1 + rotatedDistCoeffs.put(0, 2, -p1); + // P2 + rotatedDistCoeffs.put(0, 3, -p2); + break; + case DEG_90_CCW: + // FX + rotatedIntrinsics.put(0, 0, fy); + // FY + rotatedIntrinsics.put(1, 1, fx); + + // CX + rotatedIntrinsics.put(0, 2, cy); + // CY + rotatedIntrinsics.put(1, 2, unrotatedImageSize.width - cx); + + // P1 + rotatedDistCoeffs.put(0, 2, -p2); + // P2 + rotatedDistCoeffs.put(0, 3, p1); + + break; + } + + JsonMatOfDouble newIntrinsics = JsonMatOfDouble.fromMat(rotatedIntrinsics); + + JsonMatOfDouble newDistCoeffs = JsonMatOfDouble.fromMat(rotatedDistCoeffs); - // do this once so gets are quick - getCameraIntrinsicsMat().get(0, 0, intrinsicsArr); - getDistCoeffsMat().get(0, 0, distCoeffsArr); + rotatedIntrinsics.release(); + rotatedDistCoeffs.release(); + + var rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width); + + return new CameraCalibrationCoefficients( + rotatedImageSize, + newIntrinsics, + newDistCoeffs, + calobjectWarp, + observations, + calobjectSize, + calobjectSpacing, + lensmodel); } @JsonIgnore @@ -118,12 +199,12 @@ public MatOfDouble getDistCoeffsMat() { @JsonIgnore public double[] getIntrinsicsArr() { - return intrinsicsArr; + return cameraIntrinsics.data; } @JsonIgnore public double[] getDistCoeffsArr() { - return distCoeffsArr; + return distCoeffs.data; } @JsonIgnore @@ -140,7 +221,7 @@ public void release() { @Override public String toString() { return "CameraCalibrationCoefficients [resolution=" - + resolution + + unrotatedImageSize + ", cameraIntrinsics=" + cameraIntrinsics + ", distCoeffs=" @@ -149,16 +230,12 @@ public String toString() { + observations.size() + ", calobjectWarp=" + Arrays.toString(calobjectWarp) - + ", intrinsicsArr=" - + Arrays.toString(intrinsicsArr) - + ", distCoeffsArr=" - + Arrays.toString(distCoeffsArr) + "]"; } public UICameraCalibrationCoefficients cloneWithoutObservations() { return new UICameraCalibrationCoefficients( - resolution, + unrotatedImageSize, cameraIntrinsics, distCoeffs, calobjectWarp, diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java index 3294e89968..99568c54ac 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java @@ -28,7 +28,7 @@ public enum CameraLensModel { /** Mrcal steriographic lens model. See LENSMODEL_STEREOGRAPHIC in the mrcal docs */ LENSMODEL_STERIOGRAPHIC, /** - * Mrcal splined-steriographic lens model. See LENSMODEL_SPLINED_STEREOGRAPHIC_ in the mrcal docs + * Mrcal splined-steriographic lens model. See LENSMODEL_SPLINED_STEREOGRAPHIC in the mrcal docs */ LENSMODEL_SPLINED_STERIOGRAPHIC } 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 7dbb820ec4..aa80685f9e 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); @@ -57,36 +57,14 @@ public JsonMatOfDouble( this.data = data; } - private static boolean isCameraMatrixMat(Mat mat) { - return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3; - } - - private static boolean isDistortionCoeffsMat(Mat mat) { - return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1; - } - - private static boolean isCalibrationMat(Mat mat) { - return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat); - } - @JsonIgnore public static double[] getDataFromMat(Mat mat) { - if (!isCalibrationMat(mat)) return null; - double[] data = new double[(int) (mat.total() * mat.elemSize())]; mat.get(0, 0, data); - - int dataLen = -1; - - if (isCameraMatrixMat(mat)) dataLen = 9; - if (isDistortionCoeffsMat(mat)) dataLen = 5; - - // truncate Mat data to correct number data points. - return Arrays.copyOfRange(data, 0, dataLen); + return data; } public static JsonMatOfDouble fromMat(Mat mat) { - if (!isCalibrationMat(mat)) return null; return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat)); } @@ -98,11 +76,20 @@ public Mat getAsMat() { this.wrappedMat = new Mat(this.rows, this.cols, this.type); this.wrappedMat.put(0, 0, this.data); } + + 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); @@ -110,6 +97,7 @@ public MatOfDouble getAsMatOfDouble() { return this.wrappedMatOfDouble; } + @SuppressWarnings("unchecked") @JsonIgnore public Matrix getAsWpilibMat() { if (wpilibMat == null) { @@ -120,12 +108,14 @@ public Matrix getAsWpilibMat() { @Override public void release() { - getAsMat().release(); - } + if (wrappedMat != null) { + wrappedMat.release(); + } + if (wrappedMatOfDouble != null) { + wrappedMatOfDouble.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/camera/LibcameraGpuSettables.java b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java index 7d87974c18..249f3fe05f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java @@ -55,6 +55,10 @@ public void setRotation(ImageRotationMode rotationMode) { } } + public ImageRotationMode getRotation() { + return m_rotationMode; + } + public LibcameraGpuSettables(CameraConfiguration configuration) { super(configuration); @@ -212,7 +216,7 @@ protected void setVideoModeInternal(VideoMode videoMode) { getConfiguration().path, mode.width, mode.height, - (m_rotationMode == ImageRotationMode.DEG_180 ? 180 : 0)); + (m_rotationMode == ImageRotationMode.DEG_180_CCW ? 180 : 0)); if (r_ptr == 0) { logger.error("Couldn't create a zero copy Pi Camera while switching video modes"); if (!LibCameraJNI.destroyCamera(r_ptr)) { 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 cedda04d22..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 @@ -21,6 +21,7 @@ import org.opencv.core.Point; import org.photonvision.common.util.numbers.DoubleCouple; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.opencv.ImageRotationMode; /** Represents the properties of a frame. */ public class FrameStaticProperties { @@ -35,6 +36,10 @@ public class FrameStaticProperties { 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]; + /** * Instantiates a new Frame static properties. * @@ -85,6 +90,32 @@ public FrameStaticProperties( } } + public FrameStaticProperties rotate(ImageRotationMode rotation) { + if (rotation == ImageRotationMode.DEG_0) { + return this; + } + + int newWidth = imageWidth; + int newHeight = imageHeight; + + if (rotation == ImageRotationMode.DEG_90_CCW || rotation == ImageRotationMode.DEG_270_CCW) { + newWidth = imageHeight; + newHeight = imageWidth; + } + + if (cameraCalibration == null) { + return new FrameStaticProperties(newWidth, newHeight, fov, null); + } + + if (cachedRotationStaticProperties[rotation.ordinal()] == null) { + cachedRotationStaticProperties[rotation.ordinal()] = + new FrameStaticProperties( + newWidth, newHeight, fov, cameraCalibration.rotateCoefficients(rotation)); + } + + return cachedRotationStaticProperties[rotation.ordinal()]; + } + /** * Calculates the horizontal and vertical FOV components from a given diagonal FOV and image size. * diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java index 396c72e3af..2238db496f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java @@ -99,7 +99,7 @@ public final Frame get() { outputMat, m_processType, input.captureTimestamp, - input.staticProps); + input.staticProps.rotate(m_rImagePipe.getParams().rotation)); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index 0f9f301b7b..513deaadaf 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -101,7 +101,7 @@ public Frame get() { processedMat, type, MathUtils.wpiNanoTime() - latency, - settables.getFrameStaticProperties()); + settables.getFrameStaticProperties().rotate(settables.getRotation())); } } 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 e72dd50e27..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 @@ -17,19 +17,62 @@ package org.photonvision.vision.opencv; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import org.opencv.core.Core; +import org.opencv.core.Point; + +/** + * An image rotation about the camera's +Z axis, which points out of the camera towards the world. + * This is mirrored relative to what you might traditionally think of as image rotation, which is + * about an axis coming out of the image towards the viewer or camera. TODO: pull this from + * image-rotation.md + */ public enum ImageRotationMode { - DEG_0(-1), - DEG_90(0), - DEG_180(1), - DEG_270(2); + DEG_0(-1, new Rotation2d()), + // rotating an image matrix clockwise is a ccw rotation about camera +Z, lmao + DEG_90_CCW(Core.ROTATE_90_COUNTERCLOCKWISE, new Rotation2d(Units.degreesToRadians(90))), + DEG_180_CCW(Core.ROTATE_180, new Rotation2d(Units.degreesToRadians(180))), + DEG_270_CCW(Core.ROTATE_90_CLOCKWISE, new Rotation2d(Units.degreesToRadians(-90))); public final int value; + public final Rotation2d rotation2d; - ImageRotationMode(int value) { + private ImageRotationMode(int value, Rotation2d tr) { this.value = value; + this.rotation2d = tr; } - public boolean isRotated() { - return this.value == DEG_90.value || this.value == DEG_270.value; + /** + * 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) { + case DEG_0: + return point; + case DEG_90_CCW: + offset = new Pose2d(width, 0, rotation2d); + break; + case DEG_180_CCW: + offset = new Pose2d(width, height, rotation2d); + break; + case DEG_270_CCW: + offset = new Pose2d(0, height, rotation2d); + break; + default: + throw new RuntimeException("Totally bjork"); + } + + var pointAsPose = new Pose2d(point.x, point.y, new Rotation2d()); + var ret = pointAsPose.relativeTo(offset); + + return new Point(ret.getX(), ret.getY()); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java index 53f5852e72..cdb1609825 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java @@ -47,13 +47,6 @@ protected Void process(Pair> in) { double y = params.frameStaticProperties.centerY; double scale = params.frameStaticProperties.imageWidth / (double) params.divisor.value / 32.0; - if (this.params.rotMode == ImageRotationMode.DEG_270 - || this.params.rotMode == ImageRotationMode.DEG_90) { - var tmp = x; - x = y; - y = tmp; - } - switch (params.robotOffsetPointMode) { case Single: if (params.singleOffsetPoint.x != 0 && params.singleOffsetPoint.y != 0) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java index 99927ea0f1..e3e24d188c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java @@ -18,7 +18,6 @@ package org.photonvision.vision.pipe.impl; import java.awt.*; -import java.util.ArrayList; import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.opencv.calib3d.Calib3d; @@ -28,6 +27,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ColorHelper; +import org.photonvision.estimation.OpenCVHelp; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.FrameDivisor; import org.photonvision.vision.pipe.MutatingPipe; @@ -92,7 +92,11 @@ protected Void process(Pair> in) { if (params.redistortPoints) { // Distort the points, so they match the image they're being overlaid on - distortPoints(tempMat, tempMat); + tempMat.fromList( + OpenCVHelp.distortPoints( + tempMat.toList(), + params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(), + params.cameraCalibrationCoefficients.getDistCoeffsMat())); } var bottomPoints = tempMat.toList(); @@ -108,7 +112,11 @@ protected Void process(Pair> in) { if (params.redistortPoints) { // Distort the points, so they match the image they're being overlaid on - distortPoints(tempMat, tempMat); + tempMat.fromList( + OpenCVHelp.distortPoints( + tempMat.toList(), + params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(), + params.cameraCalibrationCoefficients.getDistCoeffsMat())); } var topPoints = tempMat.toList(); @@ -223,45 +231,6 @@ protected Void process(Pair> in) { return null; } - private void distortPoints(MatOfPoint2f src, MatOfPoint2f dst) { - var pointsList = src.toList(); - var dstList = new ArrayList(); - final Mat cameraMatrix = params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(); - // k1, k2, p1, p2, k3 - final Mat distCoeffs = params.cameraCalibrationCoefficients.getDistCoeffsMat(); - var cx = cameraMatrix.get(0, 2)[0]; - 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]; - - for (Point point : pointsList) { - // To relative coordinates <- this is the step you are missing. - double x = (point.x - cx) / fx; // cx, cy is the center of distortion - double y = (point.y - cy) / fy; - - double r2 = x * x + y * y; // 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); - - // Tangential distortion - xDistort = xDistort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x)); - yDistort = yDistort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y); - - // Back to absolute coordinates. - xDistort = xDistort * fx + cx; - yDistort = yDistort * fy + cy; - dstList.add(new Point(xDistort, yDistort)); - } - dst.fromList(dstList); - } - private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) { var hull = src.toArray(); var pointArray = new Point[hull.length]; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index f378f511c5..68ed0e4a49 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -619,8 +619,8 @@ public void setTargetModel(TargetModel targetModel) { public void addCalibrationToConfig(CameraCalibrationCoefficients newCalibration) { if (newCalibration != null) { - logger.info("Got new calibration for " + newCalibration.resolution); - visionSource.getSettables().getConfiguration().addCalibration(newCalibration); + logger.info("Got new calibration for " + newCalibration.unrotatedImageSize); + visionSource.getSettables().addCalibration(newCalibration); } else { logger.error("Got null calibration?"); } 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 56db6214f9..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 @@ -114,8 +114,8 @@ private void calculateFrameStaticProps() { configuration.calibrations.stream() .filter( it -> - it.resolution.width == videoMode.width - && it.resolution.height == videoMode.height) + it.unrotatedImageSize.width == videoMode.width + && it.unrotatedImageSize.height == videoMode.height) .findFirst() .orElse(null)); } 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 new file mode 100644 index 0000000000..90fae648e8 --- /dev/null +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java @@ -0,0 +1,252 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipeline; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.io.IOException; +import java.util.Arrays; +import java.util.List; +import java.util.stream.Collectors; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.junitpioneer.jupiter.cartesian.CartesianTest; +import org.junitpioneer.jupiter.cartesian.CartesianTest.Enum; +import org.opencv.core.Point; +import org.opencv.core.Size; +import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.LogLevel; +import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TestUtils; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.mrcal.MrCalJNILoader; +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.ImageRotationMode; +import org.photonvision.vision.pipe.impl.SolvePNPPipe; +import org.photonvision.vision.pipe.impl.SolvePNPPipe.SolvePNPPipeParams; +import org.photonvision.vision.target.TargetModel; +import org.photonvision.vision.target.TrackedTarget; + +public class CalibrationRotationPipeTest { + @BeforeAll + public static void init() throws IOException { + TestUtils.loadLibraries(); + MrCalJNILoader.forceLoad(); + + var logLevel = LogLevel.DEBUG; + Logger.setLevel(LogGroup.Camera, logLevel); + Logger.setLevel(LogGroup.WebServer, logLevel); + Logger.setLevel(LogGroup.VisionModule, logLevel); + Logger.setLevel(LogGroup.Data, logLevel); + Logger.setLevel(LogGroup.Config, logLevel); + Logger.setLevel(LogGroup.General, logLevel); + ConfigManager.getInstance().load(); + } + + @Test + public void meme() { + var s = new Size(200, 100); + + var p = new Point(2, 1); + + { + var angle = ImageRotationMode.DEG_90_CCW; + var expected = new Point(p.y, s.width - p.x); + var rotatedP = angle.rotatePoint(p, s.width, s.height); + assertEquals(expected.x, rotatedP.x, 1e-6); + assertEquals(expected.y, rotatedP.y, 1e-6); + } + { + var angle = ImageRotationMode.DEG_180_CCW; + var expected = new Point(s.width - p.x, s.height - p.y); + var rotatedP = angle.rotatePoint(p, s.width, s.height); + assertEquals(expected.x, rotatedP.x, 1e-6); + assertEquals(expected.y, rotatedP.y, 1e-6); + } + { + var angle = ImageRotationMode.DEG_270_CCW; + var expected = new Point(s.height - p.y, p.x); + var rotatedP = angle.rotatePoint(p, s.width, s.height); + assertEquals(expected.x, rotatedP.x, 1e-6); + assertEquals(expected.y, rotatedP.y, 1e-6); + } + } + + @CartesianTest + public void testUndistortImagePointsWithRotation(@Enum ImageRotationMode rot) { + if (rot == ImageRotationMode.DEG_0) { + return; + } + + CameraCalibrationCoefficients coeffs = + new CameraCalibrationCoefficients( + new Size(1270, 720), + new JsonMatOfDouble( + 3, + 3, + new double[] { + 900, 0, 500, + 0, 951, 321, + 0, 0, 1 + }), + new JsonMatOfDouble( + 1, + 8, + new double[] { + 0.25, + -1.5, + 0.0017808248356550637, + .00004, + 2.179764689221826, + -0.034952777924711353, + 0.09625562194891251, + -0.1860797479660746 + }), + new double[] {}, + List.of(), + new Size(), + 1, + CameraLensModel.LENSMODEL_OPENCV); + + FrameStaticProperties frameProps = + new FrameStaticProperties( + (int) coeffs.unrotatedImageSize.width, + (int) coeffs.unrotatedImageSize.height, + 66, + coeffs); + + FrameStaticProperties rotatedFrameProps = frameProps.rotate(rot); + + Point[] originalPoints = {new Point(100, 100), new Point(200, 200), new Point(300, 100)}; + + // Distort the origional points + var distortedOriginalPoints = + OpenCVHelp.distortPoints( + List.of(originalPoints), + frameProps.cameraCalibration.getCameraIntrinsicsMat(), + frameProps.cameraCalibration.getDistCoeffsMat()); + + // and rotate them once distorted + var rotatedDistortedPoints = + distortedOriginalPoints.stream() + .map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight)) + .collect(Collectors.toList()); + + // Now let's instead rotate then distort + var rotatedOrigionalPoints = + Arrays.stream(originalPoints) + .map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight)) + .collect(Collectors.toList()); + + var distortedRotatedPoints = + OpenCVHelp.distortPoints( + rotatedOrigionalPoints, + rotatedFrameProps.cameraCalibration.getCameraIntrinsicsMat(), + rotatedFrameProps.cameraCalibration.getDistCoeffsMat()); + + System.out.println("Rotated distorted: " + rotatedDistortedPoints.toString()); + System.out.println("Distorted rotated: " + distortedRotatedPoints.toString()); + + for (int i = 0; i < distortedRotatedPoints.size(); i++) { + assertEquals(rotatedDistortedPoints.get(i).x, distortedRotatedPoints.get(i).x, 1e-6); + assertEquals(rotatedDistortedPoints.get(i).y, distortedRotatedPoints.get(i).y, 1e-6); + } + } + + @Test + public void testApriltagRotated() { + // matt's lifecam + CameraCalibrationCoefficients coeffs = + new CameraCalibrationCoefficients( + new Size(1270, 720), + new JsonMatOfDouble( + 3, + 3, + new double[] { + 1132.983599412085, 0.0, 610.3195830765223, + 0.0, 1138.2884596791835, 346.4121207400337, + 0.0, 0.0, 1.0 + }), + new JsonMatOfDouble( + 1, + 8, + new double[] { + 0.11508197558262527, + -1.158603446817735, + 0.0017808248356550637, + 4.3915976993589873E-4, + 2.179764689221826, + -0.034952777924711353, + 0.04625562194891251, + -0.0860797479660746 + }), + new double[] {}, + List.of(), + new Size(), + 1, + CameraLensModel.LENSMODEL_OPENCV); + + // Matt's lifecam pointing at a wall + var distortedCorners = + List.of( + new Point(834.702271, 338.878143), + new Point(1011.808899, 345.824463), + new Point(964.300476, 225.330795), + new Point(803.971191, 217.359055)); + + SolvePNPPipe pipe = new SolvePNPPipe(); + + pipe.setParams(new SolvePNPPipeParams(coeffs, TargetModel.kAprilTag6p5in_36h11)); + var ret = pipe.run(List.of(new TrackedTarget(distortedCorners))); + + // rotate and try again + var rotAngle = ImageRotationMode.DEG_90_CCW; + var rotatedDistortedPoints = + distortedCorners.stream() + .map(it -> rotAngle.rotatePoint(it, 1280, 720)) + .collect(Collectors.toList()); + pipe.setParams( + new SolvePNPPipeParams( + coeffs.rotateCoefficients(rotAngle), TargetModel.kAprilTag6p5in_36h11)); + var retRotated = pipe.run(List.of(new TrackedTarget(rotatedDistortedPoints))); + + var pose_base = ret.output.get(0).getBestCameraToTarget3d(); + // So this is ostensibly a rotation about camera +Z, + // but this is actually camera +X for our AprilTag pipe since we rotate to stay in ""WPILib""". + // Negative to return to upright + var pose_unrotated = retRotated.output.get(0).getBestCameraToTarget3d(); + + System.out.println("Base: " + pose_base); + System.out.println("rot-unrot: " + pose_unrotated); + + Assertions.assertEquals(pose_base.getX(), pose_unrotated.getX(), 0.01); + Assertions.assertEquals(pose_base.getY(), pose_unrotated.getY(), 0.01); + Assertions.assertEquals(pose_base.getZ(), pose_unrotated.getZ(), 0.01); + Assertions.assertEquals( + pose_base.getRotation().getX(), pose_unrotated.getRotation().getX(), 0.01); + Assertions.assertEquals( + pose_base.getRotation().getY(), pose_unrotated.getRotation().getY(), 0.01); + Assertions.assertEquals( + pose_base.getRotation().getZ(), pose_unrotated.getRotation().getZ(), 0.01); + } +} diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index 50607cdf27..b286adea44 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -568,8 +568,8 @@ public static void onCalibrationSnapshotRequest(Context ctx) { .stream() .filter( it -> - Math.abs(it.resolution.width - width) < 1e-4 - && Math.abs(it.resolution.height - height) < 1e-4) + Math.abs(it.unrotatedImageSize.width - width) < 1e-4 + && Math.abs(it.unrotatedImageSize.height - height) < 1e-4) .findFirst() .orElse(null); @@ -617,8 +617,8 @@ public static void onCalibrationExportRequest(Context ctx) { cc.calibrations.stream() .filter( it -> - Math.abs(it.resolution.width - width) < 1e-4 - && Math.abs(it.resolution.height - height) < 1e-4) + Math.abs(it.unrotatedImageSize.width - width) < 1e-4 + && Math.abs(it.unrotatedImageSize.height - height) < 1e-4) .findFirst() .orElse(null); 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 7a1f23a608..9f328d9aea 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -251,6 +251,65 @@ private static Translation3d translationNWUtoEDN(Translation3d trl) { return trl.rotateBy(NWU_TO_EDN); } + /** + * 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 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) { + var ret = new ArrayList(); + + var cx = cameraMatrix.get(0, 2)[0]; + 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 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 xprime = (point.x - cx) / fx; // cx, cy is the center of distortion + double yprime = (point.y - cy) / fy; + + double r_sq = xprime * xprime + yprime * yprime; // square of the radius from center + + // Radial distortion + 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 * 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; + yDistort = yDistort * fy + cy; + ret.add(new Point(xDistort, yDistort)); + } + + return ret; + } + /** * Project object points from the 3d world into the 2d camera image. The camera * properties(intrinsics, distortion) determine the results of this projection.