Add Camera Focus Mode (#2180)

## Description
 
Camera focus tool pipeline using a Laplacian and finding the variance.
Similar to Limelight.


closes #1597 

## Meta

Merge checklist:
- [x] Pull Request title is [short, imperative
summary](https://cbea.ms/git-commit/) of proposed changes
- [x] The description documents the _what_ and _why_
- [x] This PR has been
[linted](https://docs.photonvision.org/en/latest/docs/contributing/linting.html).
- [x] If this PR changes behavior or adds a feature, user documentation
is updated
- [ ] If this PR touches photon-serde, all messages have been
regenerated and hashes have not changed unexpectedly
- [ ] If this PR touches configuration, this is backwards compatible
with settings back to v2025.3.2
- [x] If this PR touches pipeline settings or anything related to data
exchange, the frontend typing is updated
- [ ] If this PR addresses a bug, a regression test for it is added

docs/source/docs/quick-start/camera-focusing.md

@@ -0,0 +1,19 @@
+# Camera Focusing
+
+## Prepare Camera
+:::{warning}
+Refocusing your camera **will** make your calibration inaccurate, make sure to recalibrate after focusing.
+:::
+To ensure that your camera is focused properly, mount it to a secure surface and ensure it does not move drastically. Point your camera at a detailed surface like a calibration board, and make sure that it not too close to the camera.
+
+## Using Focus Mode
+:::{important}
+When you enable Focus Mode, it will assign a *Score* to the current focus, this score depends on your environment and the lighting. This score cannot be compared to a focus score collected from other environments.
+:::
+- In the Cameras tab, turn on Focus Mode.
+- Rotate the lens on your camera to try and get the focus score as high as possible.
+- Once you cannot get a higher score, this indicates that your camera is fully focused and can be set in place using glue if desired.
+
+```{image} images/focusModeExample.png
+:scale: 50%
+```

docs/source/docs/quick-start/index.md

@@ -9,5 +9,6 @@ wiring
 networking
 camera-matching
 camera-calibration
+camera-focusing
 quick-configure
 ```

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

@@ -2,6 +2,7 @@
 import PvSelect, { type SelectItem } from "@/components/common/pv-select.vue";
 import PvInput from "@/components/common/pv-input.vue";
 import PvNumberInput from "@/components/common/pv-number-input.vue";
+import PvSwitch from "@/components/common/pv-switch.vue";
 import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { useStateStore } from "@/stores/StateStore";
 import { computed, ref, watchEffect } from "vue";
@@ -15,7 +16,14 @@ const tempSettingsStruct = ref<CameraSettingsChangeRequest>({
   fov: useCameraSettingsStore().currentCameraSettings.fov.value,
   quirksToChange: Object.assign({}, useCameraSettingsStore().currentCameraSettings.cameraQuirks.quirks)
 });
-
+const focusMode = computed<boolean>({
+  get: () => useCameraSettingsStore().isFocusMode,
+  set: (v) =>
+    useCameraSettingsStore().changeCurrentPipelineIndex(
+      v ? -3 : useCameraSettingsStore().currentCameraSettings.lastPipelineIndex || 0,
+      true
+    )
+});
 const arducamSelectWrapper = computed<number>({
   get: () => {
     if (tempSettingsStruct.value.quirksToChange.ArduOV9281Controls) return 1;
@@ -166,6 +174,11 @@ const wrappedCameras = computed<SelectItem[]>(() =>
         ]"
         :select-cols="8"
       />
+      <pv-switch
+        v-model="focusMode"
+        tooltip="Enable Focus Mode to start focusing the lens on your camera"
+        label="Focus Mode"
+      ></pv-switch>
     </v-card-text>
     <v-card-text class="d-flex pt-0">
       <v-col cols="6" class="pa-0 pr-2">

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

@@ -58,6 +58,15 @@ const fpsTooLow = computed<boolean>(() => {
         <v-chip v-else label color="red" variant="text" style="font-size: 1rem; padding: 0; margin: 0">
           <span class="pr-1">Camera not connected</span>
         </v-chip>
+        <v-chip
+          v-if="useCameraSettingsStore().isFocusMode"
+          label
+          color="primary"
+          variant="text"
+          style="font-size: 1rem; padding: 0; margin: auto"
+        >
+          <span class="pr-1"> Focus: {{ Math.round(useStateStore().currentPipelineResults?.focus || 0) }} </span>
+        </v-chip>
         <v-switch
           v-model="driverMode"
           :disabled="useCameraSettingsStore().isCalibrationMode || useCameraSettingsStore().pipelineNames.length === 0"
@@ -95,7 +104,11 @@ const fpsTooLow = computed<boolean>(() => {
           color="buttonPassive"
           class="fill"
           :variant="theme.global.name.value === 'LightTheme' ? 'elevated' : 'outlined'"
-          :disabled="useCameraSettingsStore().isDriverMode || useCameraSettingsStore().isCalibrationMode"
+          :disabled="
+            useCameraSettingsStore().isDriverMode ||
+            useCameraSettingsStore().isCalibrationMode ||
+            useCameraSettingsStore().isFocusMode
+          "
         >
           <v-icon start class="mode-btn-icon" size="large">mdi-import</v-icon>
           <span class="mode-btn-label">Raw</span>
@@ -104,7 +117,11 @@ const fpsTooLow = computed<boolean>(() => {
           color="buttonPassive"
           class="fill"
           :variant="theme.global.name.value === 'LightTheme' ? 'elevated' : 'outlined'"
-          :disabled="useCameraSettingsStore().isDriverMode || useCameraSettingsStore().isCalibrationMode"
+          :disabled="
+            useCameraSettingsStore().isDriverMode ||
+            useCameraSettingsStore().isCalibrationMode ||
+            useCameraSettingsStore().isFocusMode
+          "
         >
           <v-icon start class="mode-btn-icon" size="large">mdi-export</v-icon>
           <span class="mode-btn-label">Processed</span>

photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue

@@ -92,6 +92,9 @@ const pipelineNamesWrapper = computed<SelectItem[]>(() => {
   if (useCameraSettingsStore().isDriverMode) {
     pipelineNames.push({ name: "Driver Mode", value: WebsocketPipelineType.DriverMode });
   }
+  if (useCameraSettingsStore().isFocusMode) {
+    pipelineNames.push({ name: "Focus Mode", value: WebsocketPipelineType.FocusCamera });
+  }
   if (useCameraSettingsStore().isCalibrationMode) {
     pipelineNames.push({ name: "3D Calibration Mode", value: WebsocketPipelineType.Calib3d });
   }
@@ -177,6 +180,9 @@ const pipelineTypesWrapper = computed<{ name: string; value: number }[]>(() => {
   if (useCameraSettingsStore().isDriverMode) {
     pipelineTypes.push({ name: "Driver Mode", value: WebsocketPipelineType.DriverMode });
   }
+  if (useCameraSettingsStore().isFocusMode) {
+    pipelineTypes.push({ name: "Focus Mode", value: WebsocketPipelineType.FocusCamera });
+  }
   if (useCameraSettingsStore().isCalibrationMode) {
     pipelineTypes.push({ name: "3D Calibration Mode", value: WebsocketPipelineType.Calib3d });
   }
@@ -187,6 +193,7 @@ const pipelineType = ref<WebsocketPipelineType>(useCameraSettingsStore().current
 const currentPipelineType = computed<WebsocketPipelineType>({
   get: () => {
     if (useCameraSettingsStore().isDriverMode) return WebsocketPipelineType.DriverMode;
+    if (useCameraSettingsStore().isFocusMode) return WebsocketPipelineType.FocusCamera;
     if (useCameraSettingsStore().isCalibrationMode) return WebsocketPipelineType.Calib3d;
     return pipelineType.value;
   },
@@ -290,6 +297,7 @@ const wrappedCameras = computed<SelectItem[]>(() =>
           tooltip="Each pipeline runs on a camera output and stores a unique set of processing settings"
           :disabled="
             useCameraSettingsStore().isDriverMode ||
+            useCameraSettingsStore().isFocusMode ||
             useCameraSettingsStore().isCalibrationMode ||
             !useCameraSettingsStore().hasConnected
           "
@@ -366,6 +374,7 @@ const wrappedCameras = computed<SelectItem[]>(() =>
           tooltip="Changes the pipeline type, which changes the type of processing that will happen on input frames"
           :disabled="
             useCameraSettingsStore().isDriverMode ||
+            useCameraSettingsStore().isFocusMode ||
             useCameraSettingsStore().isCalibrationMode ||
             !useCameraSettingsStore().hasConnected
           "

photon-client/src/components/dashboard/StreamConfigCard.vue

@@ -21,7 +21,9 @@ const processingMode = computed<number>({
 
 <template>
   <v-card
-    :disabled="useCameraSettingsStore().isDriverMode || useStateStore().colorPickingMode"
+    :disabled="
+      useCameraSettingsStore().isDriverMode || useCameraSettingsStore().isFocusMode || useStateStore().colorPickingMode
+    "
     class="mt-3 rounded-12"
     color="surface"
     style="flex-grow: 1; display: flex; flex-direction: column"

photon-client/src/stores/settings/CameraSettingsStore.ts

@@ -76,6 +76,9 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
     isCalibrationMode(): boolean {
       return this.currentCameraSettings.currentPipelineIndex == WebsocketPipelineType.Calib3d;
     },
+    isFocusMode(): boolean {
+      return this.currentCameraSettings.currentPipelineIndex == WebsocketPipelineType.FocusCamera;
+    },
     isCSICamera(): boolean {
       return this.currentCameraSettings.isCSICamera;
     },

photon-client/src/types/PhotonTrackingTypes.ts

@@ -70,6 +70,8 @@ export interface PipelineResult {
   sequenceID: number;
   fps: number;
   latency: number;
+  // Focus pipeline
+  focus?: number;
   targets: PhotonTarget[];
   // undefined if multitag failed or non-tag pipeline
   multitagResult?: MultitagResult;

photon-client/src/types/WebsocketDataTypes.ts

@@ -110,6 +110,7 @@ export interface IncomingWebsocketData {
 }
 
 export enum WebsocketPipelineType {
+  FocusCamera = -3,
   Calib3d = -2,
   DriverMode = -1,
   Reflective = 0,

photon-client/src/views/CameraSettingsView.vue

@@ -12,8 +12,13 @@ const cameraViewType = computed<number[]>({
     // Only show the input stream in Color Picking Mode
     if (useStateStore().colorPickingMode) return [0];
 
-    // Only show the output stream in Driver Mode or Calibration Mode
+    // Only show the output stream in Driver Mode or Calibration Mode or Focus Mode
-    if (useCameraSettingsStore().isDriverMode || useCameraSettingsStore().isCalibrationMode) return [1];
+    if (
+      useCameraSettingsStore().isDriverMode ||
+      useCameraSettingsStore().isCalibrationMode ||
+      useCameraSettingsStore().isFocusMode
+    )
+      return [1];
 
     const ret: number[] = [];
     if (useCameraSettingsStore().currentPipelineSettings.inputShouldShow) {

photon-client/src/views/DashboardView.vue

@@ -17,8 +17,13 @@ const cameraViewType = computed<number[]>({
     // Only show the input stream in Color Picking Mode
     if (useStateStore().colorPickingMode) return [0];
 
-    // Only show the output stream in Driver Mode or Calibration Mode
+    // Only show the output stream in Driver Mode or Calibration Mode or Focus Mode
-    if (useCameraSettingsStore().isDriverMode || useCameraSettingsStore().isCalibrationMode) return [1];
+    if (
+      useCameraSettingsStore().isDriverMode ||
+      useCameraSettingsStore().isCalibrationMode ||
+      useCameraSettingsStore().isFocusMode
+    )
+      return [1];
 
     const ret: number[] = [];
     if (useCameraSettingsStore().currentPipelineSettings.inputShouldShow) {

photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java

@@ -27,6 +27,7 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
+import org.photonvision.vision.pipeline.result.FocusPipelineResult;
 
 public class UIDataPublisher implements CVPipelineResultConsumer {
     private static final Logger logger = new Logger(UIDataPublisher.class, LogGroup.VisionModule);
@@ -77,6 +78,10 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
         var uiMap = new HashMap<String, HashMap<String, Object>>();
         uiMap.put(uniqueName, dataMap);
 
+        if (result instanceof FocusPipelineResult focusResult) {
+            dataMap.put("focus", focusResult.focus);
+        }
+
         DataChangeService.getInstance()
                 .publishEvent(OutgoingUIEvent.wrappedOf("updatePipelineResult", uiMap));
         lastUIResultUpdateTime = now;

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

@@ -0,0 +1,56 @@
+/*
+ * 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.Core;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class FocusPipe extends CVPipe<Mat, FocusPipe.FocusResult, FocusPipe.FocusParams> {
+    private double maxVariance = 0.0;
+
+    @Override
+    protected FocusResult process(Mat in) {
+        var outputMat = new Mat();
+
+        Imgproc.Laplacian(in, outputMat, CvType.CV_64F, 3);
+
+        var mean = new MatOfDouble();
+        var stddev = new MatOfDouble();
+        Core.meanStdDev(outputMat, mean, stddev);
+        var sd = stddev.get(0, 0)[0];
+        var variance = sd * sd;
+
+        return new FocusResult(outputMat, variance);
+    }
+
+    public static class FocusResult {
+        public final Mat frame;
+        public final double variance;
+
+        public FocusResult(Mat frame, double variance) {
+            this.frame = frame;
+            this.variance = variance;
+        }
+    }
+
+    public static class FocusParams {}
+}

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

@@ -0,0 +1,94 @@
+/*
+ * 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 org.opencv.core.Mat;
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameThresholdType;
+import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
+import org.photonvision.vision.pipe.impl.FocusPipe;
+import org.photonvision.vision.pipe.impl.ResizeImagePipe;
+import org.photonvision.vision.pipeline.result.FocusPipelineResult;
+
+public class FocusPipeline extends CVPipeline<FocusPipelineResult, FocusPipelineSettings> {
+    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 FocusPipeline() {
+        super(PROCESSING_TYPE);
+        settings = new FocusPipelineSettings();
+    }
+
+    public FocusPipeline(FocusPipelineSettings settings) {
+        super(PROCESSING_TYPE);
+        this.settings = settings;
+    }
+
+    @Override
+    protected void setPipeParamsImpl() {
+        resizeImagePipe.setParams(
+                new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
+    }
+
+    @Override
+    public FocusPipelineResult process(Frame frame, FocusPipelineSettings settings) {
+        long totalNanos = 0;
+
+        var inputMat = frame.colorImage.getMat();
+        boolean emptyIn = inputMat.empty();
+        Mat displayMat = new Mat();
+        double variance = 0.0;
+
+        if (!emptyIn) {
+            totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
+
+            var focusResult = focusPipe.run(inputMat);
+            totalNanos += focusResult.nanosElapsed;
+            variance = focusResult.output.variance;
+            displayMat = focusResult.output.frame;
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        var processedCVMat = new CVMat(displayMat);
+
+        return new FocusPipelineResult(
+                frame.sequenceID,
+                MathUtils.nanosToMillis(totalNanos),
+                fps,
+                new Frame(
+                        frame.sequenceID,
+                        frame.colorImage,
+                        processedCVMat,
+                        frame.type,
+                        frame.frameStaticProperties),
+                variance);
+    }
+
+    @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/FocusPipelineSettings.java

@@ -0,0 +1,33 @@
+/*
+ * 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.vision.processes.PipelineManager;
+
+@JsonTypeName("FocusPipelineSettings")
+public class FocusPipelineSettings extends CVPipelineSettings {
+    public FocusPipelineSettings() {
+        super();
+        pipelineNickname = "Focus Camera";
+        pipelineIndex = PipelineManager.FOCUS_INDEX;
+        pipelineType = PipelineType.FocusCamera;
+        inputShouldShow = true;
+        cameraAutoExposure = true;
+    }
+}

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

@@ -19,6 +19,7 @@ package org.photonvision.vision.pipeline;
 
 @SuppressWarnings("rawtypes")
 public enum PipelineType {
+    FocusCamera(-3, FocusPipeline.class),
     Calib3d(-2, Calibrate3dPipeline.class),
     DriverMode(-1, DriverModePipeline.class),
     Reflective(0, ReflectivePipeline.class),

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

@@ -0,0 +1,31 @@
+/*
+ * 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.result;
+
+import java.util.List;
+import org.photonvision.vision.frame.Frame;
+
+public class FocusPipelineResult extends CVPipelineResult {
+    public final double focus;
+
+    public FocusPipelineResult(
+            long seq, double latencyNanos, double fps, Frame outputFrame, double focus) {
+        super(seq, latencyNanos, fps, List.of(), outputFrame);
+        this.focus = focus;
+    }
+}

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_INDEX = -3;
     public static final int CAL_3D_INDEX = -2;
 
     protected final List<CVPipelineSettings> userPipelineSettings;
     protected final Calibrate3dPipeline calibration3dPipeline;
+    protected final FocusPipeline focusPipeline = new FocusPipeline();
     protected final DriverModePipeline driverModePipeline = new DriverModePipeline();
 
     /** Index of the currently active pipeline. Defaults to 0. */
@@ -93,6 +95,7 @@ public class PipelineManager {
         return switch (index) {
             case DRIVERMODE_INDEX -> driverModePipeline.getSettings();
             case CAL_3D_INDEX -> calibration3dPipeline.getSettings();
+            case FOCUS_INDEX -> focusPipeline.getSettings();
             default -> {
                 for (var setting : userPipelineSettings) {
                     if (setting.pipelineIndex == index) yield setting;
@@ -112,6 +115,7 @@ public class PipelineManager {
         return switch (index) {
             case DRIVERMODE_INDEX -> driverModePipeline.getSettings().pipelineNickname;
             case CAL_3D_INDEX -> calibration3dPipeline.getSettings().pipelineNickname;
+            case FOCUS_INDEX -> focusPipeline.getSettings().pipelineNickname;
             default -> {
                 for (var setting : userPipelineSettings) {
                     if (setting.pipelineIndex == index) yield setting.pipelineNickname;
@@ -153,6 +157,7 @@ public class PipelineManager {
         return switch (currentPipelineIndex) {
             case CAL_3D_INDEX -> calibration3dPipeline;
             case DRIVERMODE_INDEX -> driverModePipeline;
+            case FOCUS_INDEX -> focusPipeline;
                 // Just return the current user pipeline, we're not on a built-in one
             default -> currentUserPipeline;
         };
@@ -261,7 +266,7 @@ public class PipelineManager {
                 currentUserPipeline =
                         new ObjectDetectionPipeline((ObjectDetectionPipelineSettings) desiredPipelineSettings);
             }
-            case Calib3d, DriverMode -> {}
+            case Calib3d, DriverMode, FocusCamera -> {}
         }
     }
 
@@ -335,7 +340,7 @@ public class PipelineManager {
                     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;
                     }