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

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;
                     }