Calibration Rotation!

docs/source/conf.py

@@ -36,6 +36,7 @@
     "sphinxcontrib.ghcontributors",
     "sphinx_design",
     "myst_parser",
+    "sphinx.ext.mathjax",
 ]
 
 # Configure OpenGraph support

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
+```

docs/source/docs/contributing/design-descriptions/index.md

@@ -0,0 +1,6 @@
+# Software Architecture Design Descriptions
+
+```{toctree}
+:maxdepth: 1
+image-rotation
+```

docs/source/docs/contributing/index.md

@@ -4,4 +4,5 @@
 building-photon
 building-docs
 developer-docs/index
+design-descriptions/index
 ```

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)"
     />
-    <!-- Disable camera orientation as stop gap for Issue 1084 until calibration data gets rotated. https://github.com/PhotonVision/photonvision/issues/1084 -->
-    <v-banner
-      v-show="
-        useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
-        useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode != 0
-      "
-      rounded
-      dark
-      color="red"
-      text-color="white"
-      class="mt-3"
-      icon="mdi-alert-circle-outline"
-    >
-      Warning! A known bug affects rotation of calibrated camera. Turn off rotation here and rotate using
-      cameraToRobotTransform in your robot code.
-    </v-banner>
     <pv-select
       v-model="useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode"
       label="Orientation"
       tooltip="Rotates the camera stream. Rotation not available when camera has been calibrated."
       :items="cameraRotations"
       :select-cols="interactiveCols"
-      :disabled="
-        useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
-        useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode == 0
-      "
       @input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ inputImageRotationMode: args }, false)"
     />
     <pv-select

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

@@ -158,12 +158,23 @@ 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.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);
     }
 

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,

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
 }