From 1c5712aa39c1e99d0474544ecac4f6a6484de5a9 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Fri, 17 Jul 2020 23:00:21 -0700
Subject: [PATCH] Draw contour index, fix frame provider static properties
 (#41)

* Fix frame provider's static properties not updating

* Draw contour index on stream

* Refactor magic numbers into params
---
 .../vision/camera/USBCameraSource.java        |  2 +-
 .../frame/provider/USBFrameProvider.java      | 14 +++++++----
 .../vision/pipe/impl/Draw2dTargetsPipe.java   | 24 +++++++++++++++++++
 3 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
index a87691e91..b4e864794 100644
--- a/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
+++ b/photon-server/src/main/java/org/photonvision/vision/camera/USBCameraSource.java
@@ -50,7 +50,7 @@ public class USBCameraSource implements VisionSource {
                         camera.getInfo().productId, camera.getInfo().vendorId, config.baseName);
         cvSink = CameraServer.getInstance().getVideo(this.camera);
         usbCameraSettables = new USBCameraSettables(config);
-        usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables.getFrameStaticProperties());
+        usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables);
     }
 
     @Override
diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
index 475ea3867..a77bec076 100644
--- a/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
+++ b/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
@@ -20,18 +20,22 @@ package org.photonvision.vision.frame.provider;
 import edu.wpi.cscore.CvSink;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameProvider;
-import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.processes.VisionSourceSettables;
 
 public class USBFrameProvider implements FrameProvider {
     private final CvSink cvSink;
-    private final FrameStaticProperties frameStaticProperties;
+
+    @SuppressWarnings("SpellCheckingInspection")
+    private final VisionSourceSettables settables;
+
     private final CVMat mat;
 
-    public USBFrameProvider(CvSink sink, FrameStaticProperties frameStaticProperties) {
+    @SuppressWarnings("SpellCheckingInspection")
+    public USBFrameProvider(CvSink sink, VisionSourceSettables visionSettables) {
         cvSink = sink;
         cvSink.setEnabled(true);
-        this.frameStaticProperties = frameStaticProperties;
+        this.settables = visionSettables;
         mat = new CVMat();
     }
 
@@ -41,7 +45,7 @@ public class USBFrameProvider implements FrameProvider {
             mat.release();
         }
         long time = cvSink.grabFrame(mat.getMat());
-        return new Frame(mat, time, frameStaticProperties);
+        return new Frame(mat, time, settables.getFrameStaticProperties());
     }
 
     @Override
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
index 9cf21f1ec..9a6a1b0e3 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java
@@ -93,6 +93,25 @@ public class Draw2dTargetsPipe
                 if (params.showCentroid) {
                     Imgproc.circle(in.getLeft(), target.getTargetOffsetPoint(), 3, centroidColour, 2);
                 }
+
+                if (params.showContourNumber) {
+                    var textSize = params.kPixelsToText * in.getLeft().rows();
+                    var thickness = params.kPixelsToThickness * in.getLeft().rows();
+                    var center = target.m_mainContour.getCenterPoint();
+                    var textPos =
+                            new Point(
+                                    center.x + params.kPixelsToOffset * in.getLeft().rows(),
+                                    center.y - params.kPixelsToOffset * in.getLeft().rows());
+
+                    Imgproc.putText(
+                            in.getLeft(),
+                            String.valueOf(i),
+                            textPos,
+                            0,
+                            textSize,
+                            ColorHelper.colorToScalar(params.textColor),
+                            (int) thickness);
+                }
             }
         }
 
@@ -100,16 +119,21 @@ public class Draw2dTargetsPipe
     }
 
     public static class Draw2dContoursParams {
+        public final double kPixelsToText = 0.003;
+        public final double kPixelsToThickness = 0.008;
+        public final double kPixelsToOffset = 0.02;
         public boolean showCentroid = true;
         public boolean showMultiple;
         public int boxOutlineSize = 1;
         public boolean showRotatedBox = true;
         public boolean showShape = false;
         public boolean showMaximumBox = true;
+        public boolean showContourNumber = true;
         public Color centroidColor = Color.GREEN;
         public Color rotatedBoxColor = Color.BLUE;
         public Color maximumBoxColor = Color.RED;
         public Color shapeOutlineColour = Color.MAGENTA;
+        public Color textColor = Color.GREEN;
 
         // TODO: set other params from UI/settings file?
         public Draw2dContoursParams(boolean showMultipleTargets) {