Add Camera Focus Mode

photon-client/src/components/cameras/CamerasView.vue

@@ -15,7 +15,7 @@ const driverMode = computed<boolean>({
   get: () => useCameraSettingsStore().isDriverMode,
   set: (v) =>
     useCameraSettingsStore().changeCurrentPipelineIndex(
-      v ? -1 : useCameraSettingsStore().currentCameraSettings.lastPipelineIndex || 0,
+      v ? -2 : useCameraSettingsStore().currentCameraSettings.lastPipelineIndex || 0,
       true
     )
 });

photon-client/src/types/PipelineTypes.ts

@@ -7,7 +7,8 @@ export enum PipelineType {
   ColoredShape = 3,
   AprilTag = 4,
   Aruco = 5,
-  ObjectDetection = 6
+  ObjectDetection = 6,
+  Focus
 }
 
 export enum AprilTagFamily {

photon-client/src/types/WebsocketDataTypes.ts

@@ -116,5 +116,6 @@ export enum WebsocketPipelineType {
   ColoredShape = 1,
   AprilTag = 2,
   Aruco = 3,
-  ObjectDetection = 4
+  ObjectDetection = 4,
+  FocusCamera
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FocusPipe.java

@@ -0,0 +1,60 @@
+/*
+ * 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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import org.opencv.core.Mat;
+import org.opencv.core.CvType;
+import org.opencv.core.Core;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class FocusPipe extends CVPipe<Mat, Mat, FocusPipe.FocusParams> {
+    @Override
+    protected Mat process(Mat in) {
+        var outputMat = new Mat();
+        // We can save a copy here by sending the output of cvtcolor to outputMat directly
+        // rather than copying. Free performance!
+        //Imgproc.cvtColor(in, outputMat, Imgproc.COLOR_BGR2GRAY, 3);
+    // Compute the Laplacian in double precision (like cv2.CV_64F)
+    Imgproc.Laplacian(in, outputMat, CvType.CV_64F, 3);
+
+    // Compute standard deviation (and square it for variance) on the double Laplacian
+    var mean = new org.opencv.core.MatOfDouble();
+    var stddev = new org.opencv.core.MatOfDouble();
+    Core.meanStdDev(outputMat, mean, stddev);
+    var sd = stddev.get(0, 0)[0];
+    var variance = sd * sd;
+
+
+
+        // Draw the variance on the image (displayMat is 8-bit)
+        Imgproc.putText(
+            outputMat,
+            String.format("%.2f", variance),
+            new org.opencv.core.Point(10, 30),
+            Imgproc.FONT_HERSHEY_SIMPLEX,
+            0.8,
+            new org.opencv.core.Scalar(255, 255, 255),
+            2
+        );
+
+        return outputMat;
+    }
+
+    public static class FocusParams {}
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/GrayscalePipe.java

@@ -33,4 +33,4 @@ protected Mat process(Mat in) {
     }
 
     public static class GrayscaleParams {}
-}
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/FocusCameraPipeline.java

@@ -0,0 +1,105 @@
+/*
+ * 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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import edu.wpi.first.math.Pair;
+import java.util.List;
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameThresholdType;
+import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
+import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
+import org.photonvision.vision.pipe.impl.ResizeImagePipe;
+import org.photonvision.vision.pipeline.result.DriverModePipelineResult;
+
+public class FocusCameraPipeline
+        extends CVPipeline<DriverModePipelineResult, DriverModePipelineSettings> {
+    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
+    private final FocusPipe focusPipe = new FocusPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+    private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
+
+    private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
+
+    public FocusCameraPipeline() {
+        super(PROCESSING_TYPE);
+        settings = new FocusCameraPipelineSettings();
+    }
+
+    public FocusCameraPipeline(DriverModePipelineSettings settings) {
+        super(PROCESSING_TYPE);
+        this.settings = settings;
+    }
+  public DoubleCouple offsetPoint = new DoubleCouple();
+    @Override
+    protected void setPipeParamsImpl() {
+        draw2dCrosshairPipe.setParams(
+                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
+                        frameStaticProperties,
+                        settings.streamingFrameDivisor,
+                        settings.inputImageRotationMode));
+
+        resizeImagePipe.setParams(
+                new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
+    }
+
+    @Override
+    public FocusCameraPipelineResult process(Frame frame, FocusCameraPipelineSettings settings) {
+        long totalNanos = 0;
+
+        // apply pipes
+        var inputMat = frame.colorImage.getMat();
+
+        boolean emptyIn = inputMat.empty();
+
+
+        if (!emptyIn) {
+            totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
+            var focusResult = focusPipe.run(inputMat);
+            inputMat = focusResult.output;
+
+            if (settings.crosshair) {
+                var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
+
+                // calculate elapsed nanoseconds
+                totalNanos += draw2dCrosshairResult.nanosElapsed;
+            }
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        // Flip-flop input and output in the Frame
+        return new DriverModePipelineResult(
+                frame.sequenceID,
+                MathUtils.nanosToMillis(totalNanos),
+                fps,
+                new Frame(
+                        frame.sequenceID,
+                        frame.processedImage,
+                        frame.colorImage,
+                        frame.type,
+                        frame.frameStaticProperties));
+    }
+
+    @Override
+    public void release() {
+        // we never actually need to give resources up since pipelinemanager only makes
+        // one of us
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/FocusCameraPipelineSettings.java

@@ -0,0 +1,36 @@
+/*
+ * 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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import com.fasterxml.jackson.annotation.JsonTypeName;
+import org.photonvision.common.util.numbers.DoubleCouple;
+import org.photonvision.vision.processes.PipelineManager;
+
+@JsonTypeName("FocusCameraPipelineSettings")
+public class FocusCameraPipelineSettings extends CVPipelineSettings {
+
+
+    public FocusCameraPipelineSettings() {
+        super();
+        pipelineNickname = "Focus Camera";
+        pipelineIndex = PipelineManager.FOCUS_CAMERA_INDEX;
+        pipelineType = PipelineType.FocusCamera;
+        inputShouldShow = true;
+        cameraAutoExposure = true;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java

@@ -19,6 +19,7 @@
 
 @SuppressWarnings("rawtypes")
 public enum PipelineType {
+    FocusCamera(-3, FocusCameraPipeline.class),
     Calib3d(-2, Calibrate3dPipeline.class),
     DriverMode(-1, DriverModePipeline.class),
     Reflective(0, ReflectivePipeline.class),

photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java

@@ -36,10 +36,12 @@ public class PipelineManager {
     private static final Logger logger = new Logger(PipelineManager.class, LogGroup.VisionModule);
 
     public static final int DRIVERMODE_INDEX = -1;
+    public static final int FOCUS_CAMERA_INDEX = -3;
     public static final int CAL_3D_INDEX = -2;
 
     protected final List<CVPipelineSettings> userPipelineSettings;
     protected final Calibrate3dPipeline calibration3dPipeline;
+    protected final FocusCameraPipeline focusCameraPipeline = new FocusCameraPipeline();
     protected final DriverModePipeline driverModePipeline = new DriverModePipeline();
 
     /** Index of the currently active pipeline. Defaults to 0. */
@@ -93,6 +95,7 @@ public CVPipelineSettings getPipelineSettings(int index) {
         return switch (index) {
             case DRIVERMODE_INDEX -> driverModePipeline.getSettings();
             case CAL_3D_INDEX -> calibration3dPipeline.getSettings();
+            case FOCUS_CAMERA_INDEX -> focusCameraPipeline.getSettings();
             default -> {
                 for (var setting : userPipelineSettings) {
                     if (setting.pipelineIndex == index) yield setting;
@@ -112,6 +115,7 @@ public String getPipelineNickname(int index) {
         return switch (index) {
             case DRIVERMODE_INDEX -> driverModePipeline.getSettings().pipelineNickname;
             case CAL_3D_INDEX -> calibration3dPipeline.getSettings().pipelineNickname;
+            case FOCUS_CAMERA_INDEX -> focusCameraPipeline.getSettings().pipelineNickname;
             default -> {
                 for (var setting : userPipelineSettings) {
                     if (setting.pipelineIndex == index) yield setting.pipelineNickname;
@@ -153,6 +157,7 @@ public CVPipeline getCurrentPipeline() {
         return switch (currentPipelineIndex) {
             case CAL_3D_INDEX -> calibration3dPipeline;
             case DRIVERMODE_INDEX -> driverModePipeline;
+             case FOCUS_CAMERA_INDEX -> focusCameraPipeline;
                 // Just return the current user pipeline, we're not on a built-in one
             default -> currentUserPipeline;
         };
@@ -261,7 +266,7 @@ private void recreateUserPipeline() {
                 currentUserPipeline =
                         new ObjectDetectionPipeline((ObjectDetectionPipelineSettings) desiredPipelineSettings);
             }
-            case Calib3d, DriverMode -> {}
+            case Calib3d, DriverMode, FocusCamera -> {}
         }
     }
 
@@ -335,7 +340,7 @@ private CVPipelineSettings createSettingsForType(PipelineType type, String nickn
                     case AprilTag -> new AprilTagPipelineSettings();
                     case Aruco -> new ArucoPipelineSettings();
                     case ObjectDetection -> new ObjectDetectionPipelineSettings();
-                    case Calib3d, DriverMode -> {
+                    case Calib3d, DriverMode, FocusCamera -> {
                         logger.error("Got invalid pipeline type: " + type);
                         yield null;
                     }