From 580bbb4a4da93757a17f4ec3d0f80f1d34b9d0d9 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sat, 20 Jan 2024 20:04:15 -0500
Subject: [PATCH] Draw calibration rainbow and scale thickness based on image
 size (#1174)

---
 .../cameras/CameraCalibrationCard.vue         |  4 +--
 .../vision/pipe/impl/DrawCalibrationPipe.java | 33 ++++++++++++++++---
 2 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue
index 5f9e0b44b..ba6ca5470 100644
--- a/photon-client/src/components/cameras/CameraCalibrationCard.vue
+++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue
@@ -311,7 +311,7 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
             />
             <pv-number-input
               v-model="patternWidth"
-              label="Board Width (in)"
+              label="Board Width (squares)"
               tooltip="Width of the board in dots or chessboard squares"
               :disabled="isCalibrating"
               :rules="[(v) => v >= 4 || 'Width must be at least 4']"
@@ -319,7 +319,7 @@ const setSelectedVideoFormat = (format: VideoFormat) => {
             />
             <pv-number-input
               v-model="patternHeight"
-              label="Board Height (in)"
+              label="Board Height (squares)"
               tooltip="Height of the board in dots or chessboard squares"
               :disabled="isCalibrating"
               :rules="[(v) => v >= 4 || 'Height must be at least 4']"
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java
index a1207030a..af3f5ffeb 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java
+++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java
@@ -22,6 +22,7 @@ import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
+import org.opencv.core.Scalar;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.ColorHelper;
 import org.photonvision.vision.frame.FrameDivisor;
@@ -31,22 +32,44 @@ import org.photonvision.vision.target.TrackedTarget;
 public class DrawCalibrationPipe
         extends MutatingPipe<
                 Pair<Mat, List<TrackedTarget>>, DrawCalibrationPipe.DrawCalibrationPipeParams> {
+    Scalar[] chessboardColors =
+            new Scalar[] {
+                ColorHelper.colorToScalar(Color.RED, 0.4),
+                ColorHelper.colorToScalar(Color.ORANGE, 0.4),
+                ColorHelper.colorToScalar(Color.GREEN, 0.4),
+                ColorHelper.colorToScalar(Color.BLUE, 0.4),
+                ColorHelper.colorToScalar(Color.MAGENTA, 0.4),
+            };
+
     @Override
     protected Void process(Pair<Mat, List<TrackedTarget>> in) {
         var image = in.getLeft();
 
+        var imgSz = image.size();
+        var diag = Math.hypot(imgSz.width, imgSz.height);
+
+        // heuristic: about 4px at a diagonal of 750px, or .5%, 'looks good'. keep it at least 3px at
+        // worst tho
+        int r = (int) Math.max(diag * 4.0 / 750.0, 3);
+        int thickness = (int) Math.max(diag * 1.0 / 600.0, 1);
+
+        int i = 0;
         for (var target : in.getRight()) {
             for (var c : target.getTargetCorners()) {
                 c =
                         new Point(
                                 c.x / params.divisor.value.doubleValue(), c.y / params.divisor.value.doubleValue());
-                var r = 4;
+
                 var r2 = r / Math.sqrt(2);
-                var color = ColorHelper.colorToScalar(Color.RED, 0.4);
-                Imgproc.circle(image, c, r, color, 1);
-                Imgproc.line(image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color);
-                Imgproc.line(image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color);
+                var color = chessboardColors[i % chessboardColors.length];
+                Imgproc.circle(image, c, r, color, thickness);
+                Imgproc.line(
+                        image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color, thickness);
+                Imgproc.line(
+                        image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color, thickness);
             }
+
+            i++;
         }
 
         return null;