Add GPU Acceleration on the Raspberry Pi (#140)

* Add native stuff

* use runtimeloader

* add more native methods

* more stuff

* Switch JNI methods to static

* Remove non-java classes from the picam jni

* Add gradle task for JNI generation

* Migrate my previous GPU accel work

* Initial work on defining JNI interface

* Change libpicam to a symlink for now

* Initial work on adding no-copy OMX GPU accel on the pi

* Make DIRECT_OMX GPU accel mode not crash

* Clean up OMX changes (still not getting valid data back)

* Re-add GPU unit test

* A couple debugging tweaks/notes

* Add temporary special cases to get RGB out of ProcessingMode.NONE

* Code clarity improvements; fix possible VBO bug

* Get DIRECT_OMX working

* Remove some debugging switches in GPUAccelerator

* Pipe in VCSM stuff to read out pixels FAST

* Apply Spotless

* Revert versioningHelper changes

* Add missing import

* Convert to MMAL and move everything to native

* Re-add shared object

* Rework to use MMAL and do everything natively

* Condense pipeline settings classes

* Add OutputStreamPipeline

* Apply spotless

* Fix duplicate variable inits and add more video modes

* Integrate color frames and latency measurements for GPU

* Fix camera detection on pi and other platforms

* Add proper color copy disabling and camera settings calls

* Fix things that were broken by rebase

* Fix spotless issues and remove uneeded prints

* Remove libpicam symlink

* Fix stream resolution limiting

* Remove testing code in GPUAcceleratedHSVPipe

* Make profiling options general to all computers

* Make PicamJNI load from resources

* run spotlessApply

* Address review comments

* Update Maven repo for JOGL

* Fix release race condition

* Only run GPU accel test on the pi

* Lint fix and merge conflict accident fixes

* Make Jackson ignore extra fields when unmarshalling HardwareConfig

* Fix Mat releasing data race

* Spotless apply

* Remove broken header generation task

* Fix shared library loading typo

* Add a ZeroCopyPicam quirk to allow setting gain with the MMAL backend

* Make sure that exposure/brightness/gain get set after res changes

* Make rawInputMat properly local

* Remove bogus set of shouldRun flag

* Clean up small GPUHSVPipe print

* Add in some things that missed the ZeroCopyPiCameraSource rename

* Fix incorrect scoping introduced in past rebase

* Don't filter out too-low resolutions

* Only show latency when GPU accel is enabled

* Don't free Mats in stream thread before we use them

* Fix use-after-free and latency caluclation bugs on USB camera source

* Update libpicam

* Remove unwanted print

* Add libpicam forceLoad in unit test

* Fix streaming during camera calibration

* Fix zerocopy Picam calculation

* Use logger trace method instead of raw prints

* Fix calibration and driver mode pipes with the Picam

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>

photon-server/src/main/java/org/photonvision/Main.java

@@ -31,6 +31,7 @@ import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.networking.NetworkManager;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.raspi.PicamJNI;
 import org.photonvision.server.Server;
 import org.photonvision.vision.camera.FileVisionSource;
 import org.photonvision.vision.opencv.ContourGroupingMode;
@@ -153,6 +154,7 @@ public class Main {
 
         try {
             CameraServerCvJNI.forceLoad();
+            PicamJNI.forceLoad();
             TestUtils.loadLibraries();
             logger.info("Native libraries loaded.");
         } catch (Exception e) {

photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java

@@ -17,10 +17,12 @@
 
 package org.photonvision.common.configuration;
 
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
 
+@JsonIgnoreProperties(ignoreUnknown = true)
 public class HardwareConfig {
 
     public final String deviceName;

photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java

@@ -25,6 +25,7 @@ import java.util.stream.Collectors;
 import org.photonvision.PhotonVersion;
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.util.SerializationUtils;
+import org.photonvision.raspi.PicamJNI;
 import org.photonvision.vision.processes.VisionModule;
 import org.photonvision.vision.processes.VisionModuleManager;
 
@@ -107,8 +108,11 @@ public class PhotonConfiguration {
 
         var generalSubmap = new HashMap<String, Object>();
         generalSubmap.put("version", PhotonVersion.versionString);
-        generalSubmap.put("gpuAcceleration", false); // TODO gpu accel and accel type
-        generalSubmap.put("gpuAccelerationType", "Unknown");
+        generalSubmap.put(
+                "gpuAcceleration",
+                PicamJNI.isSupported()
+                        ? "Zerocopy MMAL"
+                        : ""); // TODO add support for other types of GPU accel
         generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
         generalSubmap.put("hardwarePlatform", Platform.getCurrentPlatform().toString());
         settingsSubmap.put("general", generalSubmap);

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

@@ -47,6 +47,7 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
         var uiMap = new HashMap<Integer, HashMap<String, Object>>();
         var dataMap = new HashMap<String, Object>();
 
+        dataMap.put("fps", result.fps);
         dataMap.put("latency", result.getLatencyMillis());
 
         var targets = result.targets;

photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -43,6 +43,14 @@ public class MathUtils {
         return nanos / 1000000.0;
     }
 
+    public static long millisToNanos(long millis) {
+        return millis * 1000000;
+    }
+
+    public static long microsToNanos(long micros) {
+        return micros * 1000;
+    }
+
     public static double map(
             double value, double in_min, double in_max, double out_min, double out_max) {
         return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;

photon-server/src/main/java/org/photonvision/raspi/PicamJNI.java

@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2020 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.raspi;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.InputStream;
+import java.net.URL;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import org.photonvision.common.hardware.Platform;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class PicamJNI {
+
+    private static boolean libraryLoaded = false;
+    private static Logger logger = new Logger(PicamJNI.class, LogGroup.Camera);
+
+    public static synchronized void forceLoad() throws IOException {
+        if (libraryLoaded || !Platform.isRaspberryPi()) return;
+
+        try {
+            URL resourceURL = PicamJNI.class.getResource("/nativelibraries/libpicam.so");
+            File libFile = Path.of("lib/libpicam.so").toFile();
+            if (!Files.exists(libFile.toPath())) {
+                // Assumes that the directory doesn't exist if libpicam doesn't exist
+                Files.createDirectory(Path.of("lib/")).toFile();
+
+                try (InputStream in = resourceURL.openStream()) {
+                    Files.copy(in, libFile.toPath());
+                }
+            }
+            System.load(libFile.getAbsolutePath());
+
+            libraryLoaded = true;
+            logger.info("Successfully loaded libpicam shared object");
+        } catch (UnsatisfiedLinkError e) {
+            logger.error("Couldn't load libpicam shared object");
+            e.printStackTrace();
+        }
+    }
+
+    public static boolean isSupported() {
+        return libraryLoaded;
+    }
+
+    // Everything here is static because multiple picams are unsupported at the hardware level
+
+    /**
+    * Called once for each video mode change. Starts a native thread running MMAL that stays alive
+    * until destroyCamera is called.
+    *
+    * @return true on error.
+    */
+    public static native boolean createCamera(int width, int height, int fps);
+
+    /**
+    * Destroys MMAL and EGL contexts. Called once for each video mode change *before* createCamera.
+    *
+    * @return true on error.
+    */
+    public static native boolean destroyCamera();
+
+    public static native void setThresholds(
+            double hL, double sL, double vL, double hU, double sU, double vU);
+
+    public static native boolean setExposure(int exposure);
+
+    public static native boolean setBrightness(int brightness);
+
+    public static native boolean setGain(int gain);
+
+    public static native boolean setRotation(int rotation);
+
+    public static native void setShouldCopyColor(boolean shouldCopyColor);
+
+    public static native long getFrameLatency();
+
+    public static native long grabFrame(boolean shouldReturnColor);
+}

photon-server/src/main/java/org/photonvision/server/SocketHandler.java

@@ -84,6 +84,12 @@ public class SocketHandler {
         var reason = context.reason() != null ? context.reason() : "Connection closed by client";
         logger.info("Closing websocket connection from " + host + " for reason: " + reason);
         users.remove(context);
+
+        if (users.size() == 0) {
+            dcService.publishEvent(
+                    new IncomingWebSocketEvent<>(
+                            DataChangeDestination.DCD_ACTIVEPIPELINESETTINGS, "inputShouldShow", false));
+        }
     }
 
     @SuppressWarnings({"unchecked"})

photon-server/src/main/java/org/photonvision/vision/camera/CameraType.java

@@ -19,5 +19,6 @@ package org.photonvision.vision.camera;
 
 public enum CameraType {
     UsbCamera,
-    HttpCamera
+    HttpCamera,
+    ZeroCopyPicam
 }

photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java

@@ -26,10 +26,17 @@ public class QuirkyCamera {
     private static final List<QuirkyCamera> quirkyCameras =
             List.of(
                     new QuirkyCamera(0x2000, 0x1415, CameraQuirk.Gain), // PS3Eye
-                    new QuirkyCamera(-1, -1, "mmal service 16.1", CameraQuirk.PiCam) // PiCam
+                    new QuirkyCamera(-1, -1, "mmal service 16.1", CameraQuirk.PiCam) // PiCam (via V4L2)
                     );
 
     public static final QuirkyCamera DefaultCamera = new QuirkyCamera(0, 0, "");
+    public static final QuirkyCamera ZeroCopyPiCamera =
+            new QuirkyCamera(
+                    -1,
+                    -1,
+                    "mmal service 16.1",
+                    CameraQuirk.PiCam,
+                    CameraQuirk.Gain); // PiCam (special zerocopy version)
 
     public final String baseName;
     public final int usbVid;

photon-server/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java

@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2020 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.camera;
+
+import edu.wpi.cscore.VideoMode;
+import java.util.HashMap;
+import org.photonvision.common.configuration.CameraConfiguration;
+import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.raspi.PicamJNI;
+import org.photonvision.vision.frame.FrameProvider;
+import org.photonvision.vision.frame.provider.AcceleratedPicamFrameProvider;
+import org.photonvision.vision.processes.VisionSource;
+import org.photonvision.vision.processes.VisionSourceSettables;
+
+public class ZeroCopyPicamSource implements VisionSource {
+
+    private final VisionSourceSettables settables;
+    private final AcceleratedPicamFrameProvider frameProvider;
+
+    public ZeroCopyPicamSource(CameraConfiguration configuration) {
+        if (configuration.cameraType != CameraType.ZeroCopyPicam) {
+            throw new IllegalArgumentException(
+                    "GPUAcceleratedPicamSource only accepts CameraConfigurations with type Picam");
+        }
+
+        settables = new PicamSettables(configuration);
+        frameProvider = new AcceleratedPicamFrameProvider(settables);
+    }
+
+    @Override
+    public FrameProvider getFrameProvider() {
+        return frameProvider;
+    }
+
+    @Override
+    public VisionSourceSettables getSettables() {
+        return settables;
+    }
+
+    /**
+    * On the OV5649 the actual FPS we want to request from the GPU can be higher than the FPS that we
+    * can do after processing. On the IMX219 these FPSes match pretty closely, except for the
+    * 1280x720 mode. We use this to present a rated FPS to the user that's lower than the actual FPS
+    * we request from the GPU. This is important for setting user expectations, and is also used by
+    * the frontend to detect and explain FPS drops.
+    */
+    private static class FPSRatedVideoMode extends VideoMode {
+        public final int fpsActual;
+
+        public FPSRatedVideoMode(
+                PixelFormat pixelFormat, int width, int height, int ratedFPS, int actualFPS) {
+            super(pixelFormat, width, height, ratedFPS);
+
+            this.fpsActual = actualFPS;
+        }
+    }
+
+    public static class PicamSettables extends VisionSourceSettables {
+
+        private VideoMode currentVideoMode;
+        private double lastExposure;
+        private int lastBrightness;
+        private int lastGain;
+
+        public PicamSettables(CameraConfiguration configuration) {
+            super(configuration);
+
+            videoModes = new HashMap<>();
+            videoModes.put(
+                    0,
+                    new FPSRatedVideoMode(
+                            VideoMode.PixelFormat.kUnknown, 320, 240, 90, 90)); // Was 120 on IMX219
+            videoModes.put(
+                    1,
+                    new FPSRatedVideoMode(
+                            VideoMode.PixelFormat.kUnknown, 640, 480, 85, 90)); // Was 65-70 on IMX219
+            videoModes.put(
+                    2,
+                    new FPSRatedVideoMode(
+                            VideoMode.PixelFormat.kUnknown, 960, 720, 45, 60)); // Was 45 on IMX219
+            videoModes.put(
+                    3,
+                    new FPSRatedVideoMode(
+                            VideoMode.PixelFormat.kUnknown, 1280, 720, 30, 45)); // Was 40 on IMX219
+            videoModes.put(
+                    4,
+                    new FPSRatedVideoMode(
+                            VideoMode.PixelFormat.kUnknown, 1920, 1080, 15, 20)); // Was 15 on IMX219
+
+            currentVideoMode = videoModes.get(0);
+        }
+
+        @Override
+        public void setExposure(double exposure) {
+            lastExposure = exposure;
+            PicamJNI.setExposure((int) Math.round(exposure));
+        }
+
+        @Override
+        public void setBrightness(int brightness) {
+            lastBrightness = brightness;
+            PicamJNI.setBrightness(brightness);
+        }
+
+        @Override
+        public void setGain(int gain) {
+            lastGain = gain;
+            PicamJNI.setGain(gain);
+        }
+
+        @Override
+        public VideoMode getCurrentVideoMode() {
+            return currentVideoMode;
+        }
+
+        @Override
+        protected void setVideoModeInternal(VideoMode videoMode) {
+            PicamJNI.destroyCamera();
+            PicamJNI.createCamera(
+                    videoMode.width, videoMode.height, ((FPSRatedVideoMode) videoMode).fpsActual);
+
+            // We don't store last settings on the native side, and when you change video mode these get
+            // reset on MMAL's end
+            setExposure(lastExposure);
+            setBrightness(lastBrightness);
+            setGain(lastGain);
+
+            currentVideoMode = videoMode;
+        }
+
+        @Override
+        public HashMap<Integer, VideoMode> getAllVideoModes() {
+            return videoModes;
+        }
+    }
+
+    @Override
+    public boolean isVendorCamera() {
+        return ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV();
+    }
+}

photon-server/src/main/java/org/photonvision/vision/frame/Frame.java

@@ -17,7 +17,7 @@
 
 package org.photonvision.vision.frame;
 
-import org.opencv.core.Mat;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
 
@@ -36,15 +36,21 @@ public class Frame implements Releasable {
         this(image, System.nanoTime(), frameStaticProperties);
     }
 
+    public Frame() {
+        timestampNanos = 0;
+        image = new CVMat();
+        frameStaticProperties = new FrameStaticProperties(0, 0, 0, new Rotation2d(), null);
+    }
+
     public void copyTo(Frame destFrame) {
         image.getMat().copyTo(destFrame.image.getMat());
     }
 
     public static Frame copyFromAndRelease(Frame frame) {
-        Mat newMat = new Mat();
-        frame.image.getMat().copyTo(newMat);
+        var mat = new CVMat();
+        frame.image.copyTo(mat);
         frame.release();
-        return new Frame(new CVMat(newMat), frame.timestampNanos, frame.frameStaticProperties);
+        return new Frame(mat, frame.timestampNanos, frame.frameStaticProperties);
     }
 
     @Override

photon-server/src/main/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProvider.java

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2020 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.frame.provider;
+
+import org.opencv.core.Mat;
+import org.photonvision.raspi.PicamJNI;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameProvider;
+import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.processes.VisionSourceSettables;
+
+public class AcceleratedPicamFrameProvider implements FrameProvider {
+
+    private final VisionSourceSettables settables;
+
+    private CVMat mat;
+
+    public AcceleratedPicamFrameProvider(VisionSourceSettables visionSettables) {
+        this.settables = visionSettables;
+
+        var vidMode = settables.getCurrentVideoMode();
+        PicamJNI.createCamera(vidMode.width, vidMode.height, vidMode.fps);
+    }
+
+    @Override
+    public String getName() {
+        return "AcceleratedPicamFrameProvider";
+    }
+
+    @Override
+    public Frame get() {
+        long matHandle = PicamJNI.grabFrame(false);
+        mat = new CVMat(new Mat(matHandle));
+        return new Frame(
+                mat, System.nanoTime() - PicamJNI.getFrameLatency(), settables.getFrameStaticProperties());
+    }
+}

photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java

@@ -41,7 +41,8 @@ public class FileFrameProvider implements FrameProvider {
     private final Path path;
     private final int millisDelay;
     private final Frame originalFrame;
-    private final Frame outputFrame;
+
+    private final FrameStaticProperties properties;
 
     private long lastGetMillis = System.currentTimeMillis();
 
@@ -74,12 +75,9 @@ public class FileFrameProvider implements FrameProvider {
 
         Mat rawImage = Imgcodecs.imread(path.toString());
         if (rawImage.cols() > 0 && rawImage.rows() > 0) {
-            FrameStaticProperties m_properties =
+            properties =
                     new FrameStaticProperties(rawImage.width(), rawImage.height(), fov, pitch, calibration);
-            Mat originalImage = new Mat();
-            rawImage.copyTo(originalImage);
-            originalFrame = new Frame(new CVMat(rawImage), m_properties);
-            outputFrame = new Frame(new CVMat(originalImage), m_properties);
+            originalFrame = new Frame(new CVMat(rawImage), properties);
         } else {
             throw new RuntimeException("Image loading failed!");
         }
@@ -107,6 +105,7 @@ public class FileFrameProvider implements FrameProvider {
 
     @Override
     public Frame get() {
+        Frame outputFrame = new Frame(new CVMat(), properties);
         originalFrame.copyTo(outputFrame);
 
         // block to keep FPS at a defined rate

photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -18,34 +18,38 @@
 package org.photonvision.vision.frame.provider;
 
 import edu.wpi.cscore.CvSink;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameProvider;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.processes.VisionSourceSettables;
 
 public class USBFrameProvider implements FrameProvider {
+    private static final long unixEpochToNanoEpoch =
+            System.nanoTime()
+                    - MathUtils.millisToNanos(System.currentTimeMillis()); // Units are nanoseconds
     private final CvSink cvSink;
 
     @SuppressWarnings("SpellCheckingInspection")
     private final VisionSourceSettables settables;
 
-    private final CVMat mat;
-
     @SuppressWarnings("SpellCheckingInspection")
     public USBFrameProvider(CvSink sink, VisionSourceSettables visionSettables) {
         cvSink = sink;
         cvSink.setEnabled(true);
         this.settables = visionSettables;
-        mat = new CVMat();
     }
 
     @Override
     public Frame get() {
-        if (mat.getMat() != null) {
-            mat.release();
-        }
-        long time = cvSink.grabFrame(mat.getMat());
-        return new Frame(mat, time, settables.getFrameStaticProperties());
+        var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread
+        long time =
+                cvSink.grabFrame(
+                        mat.getMat()); // Units are microseconds, epoch is the same as the Unix epoch
+        return new Frame(
+                mat,
+                MathUtils.microsToNanos(time) + unixEpochToNanoEpoch,
+                settables.getFrameStaticProperties());
     }
 
     @Override

photon-server/src/main/java/org/photonvision/vision/opencv/CVMat.java

@@ -19,9 +19,12 @@ package org.photonvision.vision.opencv;
 
 import java.util.HashSet;
 import org.opencv.core.Mat;
-import org.photonvision.common.util.ReflectionUtils;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
 
 public class CVMat implements Releasable {
+    private static final Logger logger = new Logger(CVMat.class, LogGroup.General);
+
     private static final HashSet<Mat> allMats = new HashSet<>();
 
     private static boolean shouldPrint;
@@ -29,7 +32,7 @@ public class CVMat implements Releasable {
     private final Mat mat;
 
     public CVMat() {
-        this.mat = new Mat();
+        this(new Mat());
     }
 
     public void copyTo(CVMat srcMat) {
@@ -43,11 +46,13 @@ public class CVMat implements Releasable {
     public CVMat(Mat mat) {
         this.mat = mat;
         if (allMats.add(mat) && shouldPrint) {
-            System.out.println(
-                    "(CVMat) Added new Mat (count: "
-                            + allMats.size()
-                            + ") from: "
-                            + ReflectionUtils.getNthCaller(3));
+            var trace = Thread.currentThread().getStackTrace();
+
+            final var traceStr = new StringBuilder();
+            for (var elem : trace) {
+                traceStr.append("\t").append(elem);
+            }
+            logger.trace(traceStr::toString);
         }
     }
 

photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java

@@ -20,7 +20,7 @@ package org.photonvision.vision.pipe.impl;
 import java.awt.*;
 import java.util.ArrayList;
 import java.util.List;
-import org.apache.commons.lang3.tuple.Triple;
+import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.*;
 import org.opencv.core.Point;
 import org.opencv.imgproc.Imgproc;
@@ -29,31 +29,19 @@ import org.photonvision.vision.pipe.MutatingPipe;
 import org.photonvision.vision.target.TrackedTarget;
 
 public class Draw2dTargetsPipe
-        extends MutatingPipe<
-                Triple<Mat, List<TrackedTarget>, Integer>, Draw2dTargetsPipe.Draw2dTargetsParams> {
+        extends MutatingPipe<Pair<Mat, List<TrackedTarget>>, Draw2dTargetsPipe.Draw2dTargetsParams> {
 
     private List<MatOfPoint> m_drawnContours = new ArrayList<>();
 
     @Override
-    protected Void process(Triple<Mat, List<TrackedTarget>, Integer> in) {
-        // Always draw FPS
+    protected Void process(Pair<Mat, List<TrackedTarget>> in) {
         var imageSize = Math.sqrt(in.getLeft().rows() * in.getLeft().cols());
-
-        var fps = in.getRight();
         var textSize = params.kPixelsToText * imageSize;
         var thickness = params.kPixelsToThickness * imageSize;
-        Imgproc.putText(
-                in.getLeft(),
-                fps.toString(),
-                new Point(10, 10 + textSize * 25),
-                0,
-                textSize,
-                ColorHelper.colorToScalar(params.textColor),
-                (int) thickness);
 
         if (!params.shouldDraw) return null;
 
-        if (!in.getMiddle().isEmpty()
+        if (!in.getRight().isEmpty()
                 && (params.showCentroid
                         || params.showMaximumBox
                         || params.showRotatedBox
@@ -64,7 +52,7 @@ public class Draw2dTargetsPipe
             var rotatedBoxColour = ColorHelper.colorToScalar(params.rotatedBoxColor);
             var shapeColour = ColorHelper.colorToScalar(params.shapeOutlineColour);
 
-            for (int i = 0; i < (params.showMultipleTargets ? in.getMiddle().size() : 1); i++) {
+            for (int i = 0; i < (params.showMultipleTargets ? in.getRight().size() : 1); i++) {
                 Point[] vertices = new Point[4];
                 MatOfPoint contour = new MatOfPoint();
 
@@ -72,7 +60,7 @@ public class Draw2dTargetsPipe
                     break;
                 }
 
-                TrackedTarget target = in.getMiddle().get(i);
+                TrackedTarget target = in.getRight().get(i);
                 RotatedRect r = target.getMinAreaRect();
 
                 if (r == null) continue;

photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindContoursPipe.java

@@ -39,7 +39,7 @@ public class FindContoursPipe
         m_foundContours.clear();
 
         Imgproc.findContours(
-                in, m_foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1);
+                in, m_foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_KCOS);
 
         return m_foundContours.stream().map(Contour::new).collect(Collectors.toList());
     }

photon-server/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java

@@ -0,0 +1,540 @@
+/*
+ * Copyright (C) 2020 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 static com.jogamp.opengl.GL.*;
+import static com.jogamp.opengl.GL2ES2.*;
+
+import com.jogamp.opengl.*;
+import com.jogamp.opengl.util.GLBuffers;
+import com.jogamp.opengl.util.texture.Texture;
+import com.jogamp.opengl.util.texture.TextureData;
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+import java.nio.IntBuffer;
+import jogamp.opengl.GLOffscreenAutoDrawableImpl;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class GPUAcceleratedHSVPipe extends CVPipe<Mat, Mat, HSVPipe.HSVParams> {
+
+    private static final String k_vertexShader =
+            String.join(
+                    "\n",
+                    "#version 100",
+                    "",
+                    "attribute vec4 position;",
+                    "",
+                    "void main() {",
+                    "  gl_Position = position;",
+                    "}");
+    private static final String k_fragmentShader =
+            String.join(
+                    "\n",
+                    "#version 100",
+                    "",
+                    "precision highp float;",
+                    "precision highp int;",
+                    "",
+                    "uniform vec3 lowerThresh;",
+                    "uniform vec3 upperThresh;",
+                    "uniform vec2 resolution;",
+                    "uniform sampler2D texture0;",
+                    "",
+                    "vec3 rgb2hsv(vec3 c) {",
+                    "  vec4 K = vec4(0.0, -1.0 / 3.0, 2.0 / 3.0, -1.0);",
+                    "  vec4 p = mix(vec4(c.bg, K.wz), vec4(c.gb, K.xy), step(c.b, c.g));",
+                    "  vec4 q = mix(vec4(p.xyw, c.r), vec4(c.r, p.yzx), step(p.x, c.r));",
+                    "",
+                    "  float d = q.x - min(q.w, q.y);",
+                    "  float e = 1.0e-10;",
+                    "  return vec3(abs(q.z + (q.w - q.y) / (6.0 * d + e)), d / (q.x + e), q.x);",
+                    "}",
+                    "",
+                    "bool inRange(vec3 hsv) {",
+                    "  bvec3 botBool = greaterThanEqual(hsv, lowerThresh);",
+                    "  bvec3 topBool = lessThanEqual(hsv, upperThresh);",
+                    "  return all(botBool) && all(topBool);",
+                    "}",
+                    "",
+                    "void main() {",
+                    "  vec2 uv = gl_FragCoord.xy/resolution;",
+                    // Important! We do this .bgr swizzle because the image comes in as BGR but we pretend
+                    // it's RGB for convenience+speed
+                    "  vec3 col = texture2D(texture0, uv).bgr;",
+                    // Only the first value in the vec4 gets used for GL_RED, and only the last value gets
+                    // used for GL_ALPHA
+                    "  gl_FragColor = inRange(rgb2hsv(col)) ? vec4(1.0, 0.0, 0.0, 1.0) : vec4(0.0, 0.0, 0.0, 0.0);",
+                    "}");
+    private static final int k_startingWidth = 1280, k_startingHeight = 720;
+    private static final float[] k_vertexPositions = {
+        // Set up a quad that covers the screen
+        -1f, +1f, +1f, +1f, -1f, -1f, +1f, -1f
+    };
+    private static final int k_positionVertexAttribute =
+            0; // ID for the vertex shader position variable
+
+    public enum PBOMode {
+        NONE,
+        SINGLE_BUFFERED,
+        DOUBLE_BUFFERED
+    }
+
+    private final IntBuffer vertexVBOIds = GLBuffers.newDirectIntBuffer(1),
+            unpackPBOIds = GLBuffers.newDirectIntBuffer(2),
+            packPBOIds = GLBuffers.newDirectIntBuffer(2);
+
+    private final GL2ES2 gl;
+    private final GLProfile profile;
+    private final int outputFormat;
+    private final PBOMode pboMode;
+    private final GLOffscreenAutoDrawableImpl.FBOImpl drawable;
+    private final Texture texture;
+    // The texture uniform holds the image that's being processed
+    // The resolution uniform holds the current image resolution
+    // The lower and upper uniforms hold the lower and upper HSV limits for thresholding
+    private final int textureUniformId, resolutionUniformId, lowerUniformId, upperUniformId;
+
+    private final Logger logger = new Logger(GPUAcceleratedHSVPipe.class, LogGroup.General);
+
+    private byte[] inputBytes, outputBytes;
+    private Mat outputMat = new Mat(k_startingHeight, k_startingWidth, CvType.CV_8UC1);
+    private int previousWidth = -1, previousHeight = -1;
+    private int unpackIndex = 0, unpackNextIndex = 0, packIndex = 0, packNextIndex = 0;
+
+    public GPUAcceleratedHSVPipe(PBOMode pboMode) {
+        this.pboMode = pboMode;
+
+        // Set up GL profile and ask for specific capabilities
+        profile = GLProfile.get(pboMode == PBOMode.NONE ? GLProfile.GLES2 : GLProfile.GL4ES3);
+        final var capabilities = new GLCapabilities(profile);
+        capabilities.setHardwareAccelerated(true);
+        capabilities.setFBO(true);
+        capabilities.setDoubleBuffered(false);
+        capabilities.setOnscreen(false);
+        capabilities.setRedBits(8);
+        capabilities.setBlueBits(0);
+        capabilities.setGreenBits(0);
+        capabilities.setAlphaBits(0);
+
+        // Set up the offscreen area we're going to draw to
+        final var factory = GLDrawableFactory.getFactory(profile);
+        drawable =
+                (GLOffscreenAutoDrawableImpl.FBOImpl)
+                        factory.createOffscreenAutoDrawable(
+                                factory.getDefaultDevice(),
+                                capabilities,
+                                new DefaultGLCapabilitiesChooser(),
+                                k_startingWidth,
+                                k_startingHeight);
+        drawable.display();
+        drawable.getContext().makeCurrent();
+
+        // Get an OpenGL context; OpenGL ES 2.0 and OpenGL 2.0 are compatible with all the coprocs we
+        // care about but not compatible with PBOs. Open GL ES 3.0 and OpenGL 4.0 are compatible with
+        // select coprocs *and* PBOs
+        gl = pboMode == PBOMode.NONE ? drawable.getGL().getGLES2() : drawable.getGL().getGL4ES3();
+        final int programId = gl.glCreateProgram();
+
+        if (pboMode == PBOMode.NONE && !gl.glGetString(GL_EXTENSIONS).contains("GL_EXT_texture_rg")) {
+            logger.warn(
+                    "OpenGL ES 2.0 implementation does not have the 'GL_EXT_texture_rg' extension, falling back to GL_ALPHA instead of GL_RED output format");
+            outputFormat = GL_ALPHA;
+        } else {
+            outputFormat = GL_RED;
+        }
+
+        // JOGL creates a framebuffer color attachment that has RGB set as the format, which is not
+        // appropriate for us because we want a single-channel format
+        // We make ourown FBO color attachment to remedy this
+        // Detach and destroy the FBO color attachment that JOGL made for us
+        drawable.getFBObject(GL_FRONT).detachColorbuffer(gl, 0, true);
+        // Equivalent to calling glBindFramebuffer
+        drawable.getFBObject(GL_FRONT).bind(gl);
+        if (true) { // For now renderbuffers are disabled
+            // Create a color attachment texture to hold our rendered output
+            var colorBufferIds = GLBuffers.newDirectIntBuffer(1);
+            gl.glGenTextures(1, colorBufferIds);
+            gl.glBindTexture(GL_TEXTURE_2D, colorBufferIds.get(0));
+            gl.glTexImage2D(
+                    GL_TEXTURE_2D,
+                    0,
+                    outputFormat == GL_RED ? GL_R8 : GL_ALPHA8,
+                    k_startingWidth,
+                    k_startingHeight,
+                    0,
+                    outputFormat,
+                    GL_UNSIGNED_BYTE,
+                    null);
+            gl.glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+            gl.glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+            // Attach the texture to the framebuffer
+            gl.glBindTexture(GL_TEXTURE_2D, 0);
+            gl.glFramebufferTexture2D(
+                    GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, colorBufferIds.get(0), 0);
+            // Cleanup
+            gl.glBindTexture(GL_TEXTURE_2D, 0);
+        } else {
+            // Create a color attachment texture to hold our rendered output
+            var renderBufferIds = GLBuffers.newDirectIntBuffer(1);
+            gl.glGenRenderbuffers(1, renderBufferIds);
+            gl.glBindRenderbuffer(GL_RENDERBUFFER, renderBufferIds.get(0));
+            gl.glRenderbufferStorage(
+                    GL_RENDERBUFFER,
+                    outputFormat == GL_RED ? GL_R8 : GL_ALPHA8,
+                    k_startingWidth,
+                    k_startingHeight);
+            // Attach the texture to the framebuffer
+            gl.glFramebufferRenderbuffer(
+                    GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_RENDERBUFFER, renderBufferIds.get(0));
+            // Cleanup
+            gl.glBindRenderbuffer(GL_RENDERBUFFER, 0);
+        }
+        drawable.getFBObject(GL_FRONT).unbind(gl);
+
+        // Check that the FBO is attached
+        int fboStatus = gl.glCheckFramebufferStatus(GL_FRAMEBUFFER);
+        if (fboStatus == GL_FRAMEBUFFER_UNSUPPORTED) {
+            throw new RuntimeException(
+                    "GL implementation does not support rendering to internal format '"
+                            + String.format("0x%08X", outputFormat == GL_RED ? GL_R8 : GL_ALPHA8)
+                            + "' with type '"
+                            + String.format("0x%08X", GL_UNSIGNED_BYTE)
+                            + "'");
+        } else if (fboStatus != GL_FRAMEBUFFER_COMPLETE) {
+            throw new RuntimeException(
+                    "Framebuffer is not complete; framebuffer status is "
+                            + String.format("0x%08X", fboStatus));
+        }
+
+        logger.debug(
+                "Created an OpenGL context with renderer '"
+                        + gl.glGetString(GL_RENDERER)
+                        + "', version '"
+                        + gl.glGetString(GL.GL_VERSION)
+                        + "', and profile '"
+                        + profile.toString()
+                        + "'");
+
+        var fmt = GLBuffers.newDirectIntBuffer(1);
+        gl.glGetIntegerv(GLES3.GL_IMPLEMENTATION_COLOR_READ_FORMAT, fmt);
+        var type = GLBuffers.newDirectIntBuffer(1);
+        gl.glGetIntegerv(GLES3.GL_IMPLEMENTATION_COLOR_READ_TYPE, type);
+
+        // Tell OpenGL that the attribute in the vertex shader named position is bound to index 0 (the
+        // index for the generic position input)
+        gl.glBindAttribLocation(programId, 0, "position");
+
+        // Compile and setup our two shaders with our program
+        final int vertexId = createShader(gl, programId, k_vertexShader, GL_VERTEX_SHADER);
+        final int fragmentId = createShader(gl, programId, k_fragmentShader, GL_FRAGMENT_SHADER);
+
+        // Link our program together and check for errors
+        gl.glLinkProgram(programId);
+        IntBuffer status = GLBuffers.newDirectIntBuffer(1);
+        gl.glGetProgramiv(programId, GL_LINK_STATUS, status);
+        if (status.get(0) == GL_FALSE) {
+
+            IntBuffer infoLogLength = GLBuffers.newDirectIntBuffer(1);
+            gl.glGetProgramiv(programId, GL_INFO_LOG_LENGTH, infoLogLength);
+
+            ByteBuffer bufferInfoLog = GLBuffers.newDirectByteBuffer(infoLogLength.get(0));
+            gl.glGetProgramInfoLog(programId, infoLogLength.get(0), null, bufferInfoLog);
+            byte[] bytes = new byte[infoLogLength.get(0)];
+            bufferInfoLog.get(bytes);
+            String strInfoLog = new String(bytes);
+
+            throw new RuntimeException("Linker failure: " + strInfoLog);
+        }
+        gl.glValidateProgram(programId);
+
+        // Cleanup shaders that are now compiled in
+        gl.glDetachShader(programId, vertexId);
+        gl.glDetachShader(programId, fragmentId);
+        gl.glDeleteShader(vertexId);
+        gl.glDeleteShader(fragmentId);
+
+        // Tell OpenGL to use our program
+        gl.glUseProgram(programId);
+
+        // Set up our texture
+        texture = new Texture(GL_TEXTURE_2D);
+        texture.setTexParameteri(gl, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+        texture.setTexParameteri(gl, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+        texture.setTexParameteri(gl, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+        texture.setTexParameteri(gl, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+
+        // Set up a uniform holding our image as a texture
+        textureUniformId = gl.glGetUniformLocation(programId, "texture0");
+        gl.glUniform1i(textureUniformId, 0);
+
+        // Set up a uniform to hold image resolution
+        resolutionUniformId = gl.glGetUniformLocation(programId, "resolution");
+
+        // Set up uniforms for the HSV thresholds
+        lowerUniformId = gl.glGetUniformLocation(programId, "lowerThresh");
+        upperUniformId = gl.glGetUniformLocation(programId, "upperThresh");
+
+        // Set up a quad that covers the entire screen so that our fragment shader draws onto the entire
+        // screen
+        gl.glGenBuffers(1, vertexVBOIds);
+
+        FloatBuffer vertexBuffer = GLBuffers.newDirectFloatBuffer(k_vertexPositions);
+        gl.glBindBuffer(GL_ARRAY_BUFFER, vertexVBOIds.get(0));
+        gl.glBufferData(
+                GL_ARRAY_BUFFER, vertexBuffer.capacity() * Float.BYTES, vertexBuffer, GL_STATIC_DRAW);
+
+        // Set up pixel unpack buffer (a PBO to transfer image data to the GPU)
+        if (pboMode != PBOMode.NONE) {
+            gl.glGenBuffers(2, unpackPBOIds);
+
+            gl.glBindBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER, unpackPBOIds.get(0));
+            gl.glBufferData(
+                    GLES3.GL_PIXEL_UNPACK_BUFFER,
+                    k_startingHeight * k_startingWidth * 3,
+                    null,
+                    GLES3.GL_STREAM_DRAW);
+            if (pboMode == PBOMode.DOUBLE_BUFFERED) {
+                gl.glBindBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER, unpackPBOIds.get(1));
+                gl.glBufferData(
+                        GLES3.GL_PIXEL_UNPACK_BUFFER,
+                        k_startingHeight * k_startingWidth * 3,
+                        null,
+                        GLES3.GL_STREAM_DRAW);
+            }
+            gl.glBindBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER, 0);
+        }
+
+        // Set up pixel pack buffer (a PBO to transfer the processed image back from the GPU)
+        if (pboMode != PBOMode.NONE) {
+            gl.glGenBuffers(2, packPBOIds);
+
+            gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(0));
+            gl.glBufferData(
+                    GLES3.GL_PIXEL_PACK_BUFFER,
+                    k_startingHeight * k_startingWidth,
+                    null,
+                    GLES3.GL_STREAM_READ);
+            if (pboMode == PBOMode.DOUBLE_BUFFERED) {
+                gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(1));
+                gl.glBufferData(
+                        GLES3.GL_PIXEL_PACK_BUFFER,
+                        k_startingHeight * k_startingWidth,
+                        null,
+                        GLES3.GL_STREAM_READ);
+            }
+            gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, 0);
+        }
+    }
+
+    private static int createShader(GL2ES2 gl, int programId, String glslCode, int shaderType) {
+        int shaderId = gl.glCreateShader(shaderType);
+        if (shaderId == 0) throw new RuntimeException("Shader ID is zero");
+
+        IntBuffer length = GLBuffers.newDirectIntBuffer(new int[] {glslCode.length()});
+        gl.glShaderSource(shaderId, 1, new String[] {glslCode}, length);
+        gl.glCompileShader(shaderId);
+
+        IntBuffer intBuffer = IntBuffer.allocate(1);
+        gl.glGetShaderiv(shaderId, GL_COMPILE_STATUS, intBuffer);
+
+        if (intBuffer.get(0) != 1) {
+            gl.glGetShaderiv(shaderId, GL_INFO_LOG_LENGTH, intBuffer);
+            int size = intBuffer.get(0);
+            if (size > 0) {
+                ByteBuffer byteBuffer = ByteBuffer.allocate(size);
+                gl.glGetShaderInfoLog(shaderId, size, intBuffer, byteBuffer);
+                System.err.println(new String(byteBuffer.array()));
+            }
+            throw new RuntimeException("Couldn't compile shader");
+        }
+
+        gl.glAttachShader(programId, shaderId);
+
+        return shaderId;
+    }
+
+    @Override
+    protected Mat process(Mat in) {
+        if (in.width() != previousWidth && in.height() != previousHeight) {
+            logger.debug("Resizing OpenGL viewport, byte buffers, and PBOs");
+
+            drawable.setSurfaceSize(in.width(), in.height());
+            gl.glViewport(0, 0, in.width(), in.height());
+
+            previousWidth = in.width();
+            previousHeight = in.height();
+
+            inputBytes = new byte[in.width() * in.height() * 3];
+
+            outputBytes = new byte[in.width() * in.height()];
+            outputMat = new Mat(k_startingHeight, k_startingWidth, CvType.CV_8UC1);
+
+            if (pboMode != PBOMode.NONE) {
+                gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(0));
+                gl.glBufferData(
+                        GLES3.GL_PIXEL_PACK_BUFFER, in.width() * in.height(), null, GLES3.GL_STREAM_READ);
+
+                if (pboMode == PBOMode.DOUBLE_BUFFERED) {
+                    gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(1));
+                    gl.glBufferData(
+                            GLES3.GL_PIXEL_PACK_BUFFER, in.width() * in.height(), null, GLES3.GL_STREAM_READ);
+                }
+            }
+        }
+
+        if (pboMode == PBOMode.DOUBLE_BUFFERED) {
+            unpackIndex = (unpackIndex + 1) % 2;
+            unpackNextIndex = (unpackIndex + 1) % 2;
+        }
+
+        // Reset the fullscreen quad
+        gl.glBindBuffer(GL_ARRAY_BUFFER, vertexVBOIds.get(0));
+        gl.glEnableVertexAttribArray(k_positionVertexAttribute);
+        gl.glVertexAttribPointer(0, 2, GL_FLOAT, false, 0, 0);
+        gl.glBindBuffer(GL_ARRAY_BUFFER, 0);
+
+        // Load and bind our image as a 2D texture
+        gl.glActiveTexture(GL_TEXTURE0);
+        texture.enable(gl);
+        texture.bind(gl);
+
+        // Load our image into the texture
+        in.get(0, 0, inputBytes);
+        if (pboMode == PBOMode.NONE) {
+            ByteBuffer buf = ByteBuffer.wrap(inputBytes);
+            // (We're actually taking in BGR even though this says RGB; it's much easier and faster to
+            // switch it around in the fragment shader)
+            texture.updateImage(
+                    gl,
+                    new TextureData(
+                            profile,
+                            GL_RGB8,
+                            in.width(),
+                            in.height(),
+                            0,
+                            GL_RGB,
+                            GL_UNSIGNED_BYTE,
+                            false,
+                            false,
+                            false,
+                            buf,
+                            null));
+        } else {
+            // Bind the PBO to the texture
+            gl.glBindBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER, unpackPBOIds.get(unpackIndex));
+
+            // Copy pixels from the PBO to the texture object
+            gl.glTexSubImage2D(
+                    GLES3.GL_TEXTURE_2D,
+                    0,
+                    0,
+                    0,
+                    in.width(),
+                    in.height(),
+                    GLES3.GL_RGB8,
+                    GLES3.GL_UNSIGNED_BYTE,
+                    0);
+
+            // Bind (potentially) another PBO to update the texture source
+            gl.glBindBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER, unpackPBOIds.get(unpackNextIndex));
+
+            // This call with a nullptr for the data arg tells OpenGL *not* to wait to be in sync with the
+            // GPU
+            // This causes the previous data in the PBO to be discarded
+            gl.glBufferData(
+                    GLES3.GL_PIXEL_UNPACK_BUFFER, in.width() * in.height() * 3, null, GLES3.GL_STREAM_DRAW);
+
+            // Map the a buffer of GPU memory into a place that's accessible by us
+            var buf =
+                    gl.glMapBufferRange(
+                            GLES3.GL_PIXEL_UNPACK_BUFFER,
+                            0,
+                            in.width() * in.height() * 3,
+                            GLES3.GL_MAP_WRITE_BIT);
+            buf.put(inputBytes);
+
+            gl.glUnmapBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER);
+            gl.glBindBuffer(GLES3.GL_PIXEL_UNPACK_BUFFER, 0);
+        }
+
+        // Put values in a uniform holding the image resolution
+        gl.glUniform2f(resolutionUniformId, in.width(), in.height());
+
+        // Put values in threshold uniforms
+        var lowr = params.getHsvLower().val;
+        var upr = params.getHsvUpper().val;
+        gl.glUniform3f(lowerUniformId, (float) lowr[0], (float) lowr[1], (float) lowr[2]);
+        gl.glUniform3f(upperUniformId, (float) upr[0], (float) upr[1], (float) upr[2]);
+
+        // Draw the fullscreen quad
+        gl.glDrawArrays(GL_TRIANGLE_STRIP, 0, k_vertexPositions.length);
+
+        // Cleanup
+        texture.disable(gl);
+        gl.glDisableVertexAttribArray(k_positionVertexAttribute);
+        gl.glUseProgram(0);
+
+        if (pboMode == PBOMode.NONE) {
+            return saveMatNoPBO(gl, in.width(), in.height());
+        } else {
+            return saveMatPBO((GLES3) gl, in.width(), in.height(), pboMode == PBOMode.DOUBLE_BUFFERED);
+        }
+    }
+
+    private Mat saveMatNoPBO(GL2ES2 gl, int width, int height) {
+        ByteBuffer buffer = GLBuffers.newDirectByteBuffer(width * height);
+        // We use GL_RED/GL_ALPHA to get things in a single-channel format
+        // Note that which pixel format you use is *very* important to performance
+        // E.g. GL_ALPHA is super slow in this case
+        gl.glReadPixels(0, 0, width, height, outputFormat, GL_UNSIGNED_BYTE, buffer);
+
+        return new Mat(height, width, CvType.CV_8UC1, buffer);
+    }
+
+    private Mat saveMatPBO(GLES3 gl, int width, int height, boolean doubleBuffered) {
+        if (doubleBuffered) {
+            packIndex = (packIndex + 1) % 2;
+            packNextIndex = (packIndex + 1) % 2;
+        }
+
+        // Set the target framebuffer attachment to read
+        gl.glReadBuffer(GLES3.GL_COLOR_ATTACHMENT0);
+
+        // Read pixels from the framebuffer to the PBO
+        gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(packIndex));
+        // We use GL_RED (which is always supported in GLES3) to get things in a single-channel format
+        // Note that which pixel format you use is *very* important to performance
+        // E.g. GL_ALPHA is super slow in this case
+        gl.glReadPixels(0, 0, width, height, GLES3.GL_RED, GLES3.GL_UNSIGNED_BYTE, 0);
+
+        // Map the PBO into the CPU's memory
+        gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, packPBOIds.get(packNextIndex));
+        var buf =
+                gl.glMapBufferRange(GLES3.GL_PIXEL_PACK_BUFFER, 0, width * height, GLES3.GL_MAP_READ_BIT);
+        buf.get(outputBytes);
+        outputMat.put(0, 0, outputBytes);
+        gl.glUnmapBuffer(GLES3.GL_PIXEL_PACK_BUFFER);
+        gl.glBindBuffer(GLES3.GL_PIXEL_PACK_BUFFER, 0);
+
+        return outputMat;
+    }
+}

photon-server/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java

@@ -25,15 +25,13 @@ import org.photonvision.common.util.numbers.IntegerCouple;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class HSVPipe extends CVPipe<Mat, Mat, HSVPipe.HSVParams> {
-
-    private final Mat m_outputMat = new Mat();
-
     @Override
     protected Mat process(Mat in) {
-        in.copyTo(m_outputMat);
-        Imgproc.cvtColor(m_outputMat, m_outputMat, Imgproc.COLOR_BGR2HSV, 3);
-        Core.inRange(m_outputMat, params.getHsvLower(), params.getHsvUpper(), m_outputMat);
-        return m_outputMat;
+        var outputMat = new Mat();
+        in.copyTo(outputMat);
+        Imgproc.cvtColor(outputMat, outputMat, Imgproc.COLOR_BGR2HSV, 3);
+        Core.inRange(outputMat, params.getHsvLower(), params.getHsvUpper(), outputMat);
+        return outputMat;
     }
 
     public static class HSVParams {

photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java

@@ -21,8 +21,12 @@ import java.util.Objects;
 import org.opencv.core.Point;
 import org.photonvision.common.util.numbers.DoubleCouple;
 import org.photonvision.common.util.numbers.IntegerCouple;
+import org.photonvision.vision.opencv.ContourGroupingMode;
+import org.photonvision.vision.opencv.ContourIntersectionDirection;
 import org.photonvision.vision.opencv.ContourSortMode;
+import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
 import org.photonvision.vision.target.RobotOffsetPointMode;
+import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TargetOffsetPointEdge;
 import org.photonvision.vision.target.TargetOrientation;
 
@@ -68,32 +72,58 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
     public Point offsetDualPointB = new Point();
     public double offsetDualPointBArea = 0;
 
+    // how many contours to attempt to group (Single, Dual)
+    public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
+
+    // the direction in which contours must intersect to be considered intersecting
+    public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
+
+    // 3d settings
+    public boolean solvePNPEnabled = false;
+    public TargetModel targetModel = TargetModel.k2020HighGoalOuter;
+
+    // Corner detection settings
+    public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
+            CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
+    public boolean cornerDetectionUseConvexHulls = true;
+    public boolean cornerDetectionExactSideCount = false;
+    public int cornerDetectionSideCount = 4;
+    public double cornerDetectionAccuracyPercentage = 10;
+
     @Override
     public boolean equals(Object o) {
         if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
+        if (!(o instanceof AdvancedPipelineSettings)) return false;
         if (!super.equals(o)) return false;
         AdvancedPipelineSettings that = (AdvancedPipelineSettings) o;
         return outputShouldDraw == that.outputShouldDraw
                 && outputShowMultipleTargets == that.outputShowMultipleTargets
                 && contourSpecklePercentage == that.contourSpecklePercentage
-                && Double.compare(that.offsetDualPointA.x, offsetDualPointA.x) == 0
-                && Double.compare(that.offsetDualPointA.y, offsetDualPointA.y) == 0
                 && Double.compare(that.offsetDualPointAArea, offsetDualPointAArea) == 0
-                && Double.compare(that.offsetDualPointB.x, offsetDualPointB.x) == 0
-                && Double.compare(that.offsetDualPointB.y, offsetDualPointB.y) == 0
                 && Double.compare(that.offsetDualPointBArea, offsetDualPointBArea) == 0
-                && hsvHue.equals(that.hsvHue)
-                && hsvSaturation.equals(that.hsvSaturation)
-                && hsvValue.equals(that.hsvValue)
-                && contourArea.equals(that.contourArea)
-                && contourRatio.equals(that.contourRatio)
-                && contourFullness.equals(that.contourFullness)
+                && solvePNPEnabled == that.solvePNPEnabled
+                && cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
+                && cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
+                && cornerDetectionSideCount == that.cornerDetectionSideCount
+                && Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
+                        == 0
+                && Objects.equals(hsvHue, that.hsvHue)
+                && Objects.equals(hsvSaturation, that.hsvSaturation)
+                && Objects.equals(hsvValue, that.hsvValue)
+                && Objects.equals(contourArea, that.contourArea)
+                && Objects.equals(contourRatio, that.contourRatio)
+                && Objects.equals(contourFullness, that.contourFullness)
                 && contourSortMode == that.contourSortMode
                 && contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge
                 && contourTargetOrientation == that.contourTargetOrientation
                 && offsetRobotOffsetMode == that.offsetRobotOffsetMode
-                && offsetSinglePoint.equals(that.offsetSinglePoint);
+                && Objects.equals(offsetSinglePoint, that.offsetSinglePoint)
+                && Objects.equals(offsetDualPointA, that.offsetDualPointA)
+                && Objects.equals(offsetDualPointB, that.offsetDualPointB)
+                && contourGroupingMode == that.contourGroupingMode
+                && contourIntersection == that.contourIntersection
+                && Objects.equals(targetModel, that.targetModel)
+                && cornerDetectionStrategy == that.cornerDetectionStrategy;
     }
 
     @Override
@@ -117,6 +147,15 @@ public class AdvancedPipelineSettings extends CVPipelineSettings {
                 offsetDualPointA,
                 offsetDualPointAArea,
                 offsetDualPointB,
-                offsetDualPointBArea);
+                offsetDualPointBArea,
+                contourGroupingMode,
+                contourIntersection,
+                solvePNPEnabled,
+                targetModel,
+                cornerDetectionStrategy,
+                cornerDetectionUseConvexHulls,
+                cornerDetectionExactSideCount,
+                cornerDetectionSideCount,
+                cornerDetectionAccuracyPercentage);
     }
 }

photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java

@@ -39,19 +39,19 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
     }
 
     public R run(Frame frame) {
-        long pipelineStartNanos = System.nanoTime();
-
         if (settings == null) {
             throw new RuntimeException("No settings provided for pipeline!");
         }
         setPipeParams(frame.frameStaticProperties, settings);
 
         if (frame.image.getMat().empty()) {
-            return (R) new CVPipelineResult(0, List.of(), frame);
+            return (R) new CVPipelineResult(0, 0, List.of(), frame);
         }
         R result = process(frame, settings);
 
-        result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - pipelineStartNanos));
+        // Important! This assumes that the frame timestamp has the same epoch as System.nanoTime (which
+        // itself has an arbitrary epoch)
+        result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - frame.timestampNanos));
 
         return result;
     }

photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java

@@ -45,6 +45,8 @@ public class CVPipelineSettings implements Cloneable {
     public int cameraVideoModeIndex = 0;
     public FrameDivisor streamingFrameDivisor = FrameDivisor.NONE;
     public boolean ledMode = false;
+    public boolean inputShouldShow = false;
+    public boolean outputShouldShow = true;
 
     @Override
     public boolean equals(Object o) {
@@ -61,7 +63,9 @@ public class CVPipelineSettings implements Cloneable {
                 && inputImageFlipMode == that.inputImageFlipMode
                 && inputImageRotationMode == that.inputImageRotationMode
                 && pipelineNickname.equals(that.pipelineNickname)
-                && streamingFrameDivisor == that.streamingFrameDivisor;
+                && streamingFrameDivisor == that.streamingFrameDivisor
+                && inputShouldShow == that.inputShouldShow
+                && outputShouldShow == that.outputShouldShow;
     }
 
     @Override
@@ -77,7 +81,9 @@ public class CVPipelineSettings implements Cloneable {
                 cameraGain,
                 cameraVideoModeIndex,
                 streamingFrameDivisor,
-                ledMode);
+                ledMode,
+                inputShouldShow,
+                outputShouldShow);
     }
 
     @Override

photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -34,12 +34,14 @@ import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.file.FileUtils;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.raspi.PicamJNI;
 import org.photonvision.server.SocketHandler;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
+import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
 import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
 import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -53,6 +55,7 @@ public class Calibrate3dPipeline
     // Only 2 pipes needed, one for finding the board corners and one for actually calibrating
     private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
     private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
     // Getter methods have been set for calibrate and takeSnapshot
     private boolean takeSnapshot = false;
@@ -78,6 +81,10 @@ public class Calibrate3dPipeline
         this.settings = new Calibration3dPipelineSettings();
         this.foundCornersList = new ArrayList<>();
         this.minSnapshots = minSnapshots;
+
+        if (PicamJNI.isSupported()) {
+            PicamJNI.setShouldCopyColor(true);
+        }
     }
 
     @Override
@@ -96,20 +103,30 @@ public class Calibrate3dPipeline
 
     @Override
     protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings settings) {
+        Mat inputColorMat = frame.image.getMat();
+        if (inputColorMat.channels() == 1 && PicamJNI.isSupported()) {
+            long colorMatPtr = PicamJNI.grabFrame(true);
+            if (colorMatPtr == 0) throw new RuntimeException("Got null Mat from GPU Picam driver");
+            inputColorMat = new Mat(colorMatPtr);
+        }
+
         // Set the pipe parameters
         setPipeParams(frame.frameStaticProperties, settings);
 
         if (this.calibrating) {
             return new CVPipelineResult(
-                    0, null, new Frame(new CVMat(frame.image.getMat()), frame.frameStaticProperties));
+                    0, 0, null, new Frame(new CVMat(inputColorMat), frame.frameStaticProperties));
         }
 
         long sumPipeNanosElapsed = 0L;
 
         // Check if the frame has chessboard corners
-        var outFrame = new Mat();
-        frame.image.getMat().copyTo(outFrame);
-        var findBoardResult = findBoardCornersPipe.run(Pair.of(frame.image.getMat(), outFrame)).output;
+        var outputColorMat = new Mat();
+        inputColorMat.copyTo(outputColorMat);
+        var findBoardResult = findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorMat)).output;
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
 
         if (takeSnapshot) {
             // Set snapshot to false even if we don't find a board
@@ -119,21 +136,25 @@ public class Calibrate3dPipeline
                 foundCornersList.add(findBoardResult);
                 Imgcodecs.imwrite(
                         Path.of(imageDir.toString(), "img" + foundCornersList.size() + ".jpg").toString(),
-                        frame.image.getMat());
+                        inputColorMat);
 
                 // update the UI
                 broadcastState();
 
                 return new CVPipelineResult(
-                        MathUtils.nanosToMillis(sumPipeNanosElapsed), Collections.emptyList(), frame);
+                        MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                        fps,
+                        Collections.emptyList(),
+                        new Frame(new CVMat(inputColorMat), frame.frameStaticProperties));
             }
         }
 
         // Return the drawn chessboard if corners are found, if not, then return the input image.
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                fps, // Unused but here in case
                 null,
-                new Frame(new CVMat(outFrame), frame.frameStaticProperties));
+                new Frame(new CVMat(outputColorMat), frame.frameStaticProperties));
     }
 
     public void deleteSavedImages() {

photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -21,7 +21,6 @@ import java.util.Arrays;
 import java.util.List;
 import java.util.stream.Collectors;
 import org.apache.commons.lang3.tuple.Pair;
-import org.apache.commons.lang3.tuple.Triple;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
 import org.photonvision.common.util.math.MathUtils;
@@ -55,6 +54,7 @@ public class ColoredShapePipeline
     private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
     private final Draw2dTargetsPipe draw2DTargetsPipe = new Draw2dTargetsPipe();
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
     private final Mat rawInputMat = new Mat();
     private final Point[] rectPoints = new Point[4];
@@ -158,7 +158,9 @@ public class ColoredShapePipeline
 
         var solvePNPParams =
                 new SolvePNPPipe.SolvePNPPipeParams(
-                        settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
+                        frameStaticProperties.cameraCalibration,
+                        frameStaticProperties.cameraPitch,
+                        settings.targetModel);
         solvePNPPipe.setParams(solvePNPParams);
 
         Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
@@ -178,10 +180,12 @@ public class ColoredShapePipeline
                         frameStaticProperties);
         draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
 
-        var draw3dContoursParams =
+        var draw3dTargetsParams =
                 new Draw3dTargetsPipe.Draw3dContoursParams(
-                        settings.outputShouldDraw, settings.cameraCalibration, settings.targetModel);
-        draw3dTargetsPipe.setParams(draw3dContoursParams);
+                        settings.outputShouldDraw,
+                        frameStaticProperties.cameraCalibration,
+                        settings.targetModel);
+        draw3dTargetsPipe.setParams(draw3dTargetsParams);
     }
 
     @Override
@@ -269,12 +273,11 @@ public class ColoredShapePipeline
 
         // Draw 2D contours on input and output
         var draw2dContoursResultOnInput =
-                draw2DTargetsPipe.run(Triple.of(rawInputMat, collect2dTargetsResult.output, -12345));
+                draw2DTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output));
         sumPipeNanosElapsed += draw2dContoursResultOnInput.nanosElapsed;
 
         var draw2dContoursResultOnOutput =
-                draw2DTargetsPipe.run(
-                        Triple.of(hsvPipeResult.output, collect2dTargetsResult.output, -12345));
+                draw2DTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output));
         sumPipeNanosElapsed += draw2dContoursResultOnOutput.nanosElapsed;
 
         if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
@@ -291,8 +294,12 @@ public class ColoredShapePipeline
         var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output);
         sumPipeNanosElapsed += outputMatPipeResult.nanosElapsed;
 
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                fps,
                 targetList,
                 new Frame(new CVMat(hsvPipeResult.output), frame.frameStaticProperties),
                 new Frame(new CVMat(rawInputMat), frame.frameStaticProperties));

photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java

@@ -132,6 +132,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
                 cornerDetectionSideCount,
                 cornerDetectionAccuracyPercentage,
                 erode,
-                dilate);
+                dilate,
+                accuracy);
     }
 }

photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -19,10 +19,13 @@ package org.photonvision.vision.pipeline;
 
 import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.raspi.PicamJNI;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
 import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
 import org.photonvision.vision.pipe.impl.RotateImagePipe;
 import org.photonvision.vision.pipeline.result.DriverModePipelineResult;
@@ -32,6 +35,7 @@ public class DriverModePipeline
 
     private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
     private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
     public DriverModePipeline() {
         settings = new DriverModePipelineSettings();
@@ -47,12 +51,19 @@ public class DriverModePipeline
         Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
                 new Draw2dCrosshairPipe.Draw2dCrosshairParams(frameStaticProperties);
         draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
+
+        PicamJNI.setShouldCopyColor(true);
     }
 
     @Override
     public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) {
         // apply pipes
         var inputMat = frame.image.getMat();
+        if (inputMat.channels() == 1 && PicamJNI.isSupported()) {
+            long colorMatPtr = PicamJNI.grabFrame(true);
+            if (colorMatPtr == 0) throw new RuntimeException("Got null Mat from GPU Picam driver");
+            inputMat = new Mat(colorMatPtr);
+        }
 
         var rotateImageResult = rotateImagePipe.run(inputMat);
         var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
@@ -60,8 +71,12 @@ public class DriverModePipeline
         // calculate elapsed nanoseconds
         long totalNanos = rotateImageResult.nanosElapsed + draw2dCrosshairResult.nanosElapsed;
 
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
         return new DriverModePipelineResult(
                 MathUtils.nanosToMillis(totalNanos),
+                fps,
                 new Frame(new CVMat(inputMat), frame.frameStaticProperties));
     }
 }

photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java

@@ -32,5 +32,6 @@ public class DriverModePipelineSettings extends CVPipelineSettings {
         pipelineNickname = "Driver Mode";
         pipelineIndex = PipelineManager.DRIVERMODE_INDEX;
         pipelineType = PipelineType.DriverMode;
+        inputShouldShow = true;
     }
 }

photon-server/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2020 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 java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameStaticProperties;
+import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.opencv.DualOffsetValues;
+import org.photonvision.vision.pipe.impl.*;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TrackedTarget;
+
+/**
+* This is a "fake" pipeline that is just used to move identical pipe sets out of real pipelines. It
+* shall not get its settings saved, nor shall it be managed by PipelineManager
+*/
+public class OutputStreamPipeline {
+
+    private final OutputMatPipe outputMatPipe = new OutputMatPipe();
+    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
+    private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
+    private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+
+    private final long[] pipeProfileNanos = new long[10];
+
+    protected void setPipeParams(
+            FrameStaticProperties frameStaticProperties, AdvancedPipelineSettings settings) {
+
+        var dualOffsetValues =
+                new DualOffsetValues(
+                        settings.offsetDualPointA,
+                        settings.offsetDualPointAArea,
+                        settings.offsetDualPointB,
+                        settings.offsetDualPointBArea);
+
+        var draw2DTargetsParams =
+                new Draw2dTargetsPipe.Draw2dTargetsParams(
+                        settings.outputShouldDraw, settings.outputShowMultipleTargets);
+        draw2dTargetsPipe.setParams(draw2DTargetsParams);
+
+        var draw2dCrosshairParams =
+                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
+                        settings.outputShouldDraw,
+                        settings.offsetRobotOffsetMode,
+                        settings.offsetSinglePoint,
+                        dualOffsetValues,
+                        frameStaticProperties);
+        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
+
+        var draw3dTargetsParams =
+                new Draw3dTargetsPipe.Draw3dContoursParams(
+                        settings.outputShouldDraw,
+                        frameStaticProperties.cameraCalibration,
+                        settings.targetModel);
+        draw3dTargetsPipe.setParams(draw3dTargetsParams);
+    }
+
+    public CVPipelineResult process(
+            Frame inputFrame,
+            Frame outputFrame,
+            AdvancedPipelineSettings settings,
+            List<TrackedTarget> targetsToDraw) {
+        setPipeParams(inputFrame.frameStaticProperties, settings);
+        var inMat = inputFrame.image.getMat();
+        var outMat = outputFrame.image.getMat();
+
+        long sumPipeNanosElapsed = 0L;
+
+        // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
+        var outputMatPipeResult = outputMatPipe.run(outMat);
+        sumPipeNanosElapsed += pipeProfileNanos[0] = outputMatPipeResult.nanosElapsed;
+
+        // Draw 2D Crosshair on input and output
+        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
+        sumPipeNanosElapsed += pipeProfileNanos[1] = draw2dCrosshairResultOnInput.nanosElapsed;
+
+        var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(inMat, targetsToDraw));
+        sumPipeNanosElapsed += pipeProfileNanos[2] = draw2dCrosshairResultOnOutput.nanosElapsed;
+
+        // Draw 2D contours on input and output
+        var draw2dTargetsOnInput = draw2dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
+        sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dTargetsOnInput.nanosElapsed;
+
+        var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+        sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dTargetsOnOutput.nanosElapsed;
+
+        // Draw 3D Targets on input and output if necessary
+        if (settings.solvePNPEnabled) {
+            var drawOnInputResult = draw3dTargetsPipe.run(Pair.of(inMat, targetsToDraw));
+            sumPipeNanosElapsed += pipeProfileNanos[5] = drawOnInputResult.nanosElapsed;
+
+            var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+            sumPipeNanosElapsed += pipeProfileNanos[6] = drawOnOutputResult.nanosElapsed;
+        } else {
+            pipeProfileNanos[5] = 0;
+            pipeProfileNanos[6] = 0;
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        return new CVPipelineResult(
+                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                fps, // Unused but here just in case
+                targetsToDraw,
+                new Frame(new CVMat(outMat), outputFrame.frameStaticProperties),
+                new Frame(new CVMat(inMat), inputFrame.frameStaticProperties));
+    }
+}

photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java

@@ -38,7 +38,6 @@ public class PipelineProfiler {
     private static final String[] ReflectivePipeNames =
             new String[] {
                 "RotateImage",
-                "InputCopy",
                 "HSV",
                 "FindContours",
                 "SpeckleReject",
@@ -48,13 +47,6 @@ public class PipelineProfiler {
                 "Collect2dTargets",
                 "CornerDetection",
                 "SolvePNP",
-                "OutputConversion",
-                "Draw2dCrosshairInput",
-                "Draw2dCrosshairOutput",
-                "Draw2dTargetsInput",
-                "Draw2dTargetsOutput",
-                "Draw3dTargetsInput",
-                "Draw3dTargetsOutput",
             };
 
     public static final int ReflectivePipeCount = ReflectivePipeNames.length;

photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java

@@ -18,10 +18,9 @@
 package org.photonvision.vision.pipeline;
 
 import java.util.List;
-import org.apache.commons.lang3.tuple.Pair;
-import org.apache.commons.lang3.tuple.Triple;
 import org.opencv.core.Mat;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.raspi.PicamJNI;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVMat;
@@ -47,13 +46,8 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
     private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
     private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
     private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
-    private final OutputMatPipe outputMatPipe = new OutputMatPipe();
-    private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
-    private final Draw2dTargetsPipe draw2dTargetsPipe = new Draw2dTargetsPipe();
-    private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
-    private final Mat rawInputMat = new Mat();
     private final long[] pipeProfileNanos = new long[PipelineProfiler.ReflectivePipeCount];
 
     public ReflectivePipeline() {
@@ -68,30 +62,41 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
     protected void setPipeParams(
             FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
 
-        DualOffsetValues dualOffsetValues =
+        var dualOffsetValues =
                 new DualOffsetValues(
                         settings.offsetDualPointA,
                         settings.offsetDualPointAArea,
                         settings.offsetDualPointB,
                         settings.offsetDualPointBArea);
 
-        RotateImagePipe.RotateImageParams rotateImageParams =
-                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        var rotateImageParams = new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
         rotateImagePipe.setParams(rotateImageParams);
 
-        HSVPipe.HSVParams hsvParams =
-                new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
-        hsvPipe.setParams(hsvParams);
+        if (!PicamJNI.isSupported()) {
+            var hsvParams =
+                    new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
+            hsvPipe.setParams(hsvParams);
+        } else {
+            PicamJNI.setThresholds(
+                    settings.hsvHue.getFirst() / 180d,
+                    settings.hsvSaturation.getFirst() / 255d,
+                    settings.hsvValue.getFirst() / 255d,
+                    settings.hsvHue.getSecond() / 180d,
+                    settings.hsvSaturation.getSecond() / 255d,
+                    settings.hsvValue.getSecond() / 255d);
 
-        FindContoursPipe.FindContoursParams findContoursParams =
-                new FindContoursPipe.FindContoursParams();
+            PicamJNI.setRotation(settings.inputImageRotationMode.value);
+            PicamJNI.setShouldCopyColor(settings.inputShouldShow);
+        }
+
+        var findContoursParams = new FindContoursPipe.FindContoursParams();
         findContoursPipe.setParams(findContoursParams);
 
-        SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
+        var speckleRejectParams =
                 new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
         speckleRejectPipe.setParams(speckleRejectParams);
 
-        FilterContoursPipe.FilterContoursParams filterContoursParams =
+        var filterContoursParams =
                 new FilterContoursPipe.FilterContoursParams(
                         settings.contourArea,
                         settings.contourRatio,
@@ -99,19 +104,19 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
                         frameStaticProperties);
         filterContoursPipe.setParams(filterContoursParams);
 
-        GroupContoursPipe.GroupContoursParams groupContoursParams =
+        var groupContoursParams =
                 new GroupContoursPipe.GroupContoursParams(
                         settings.contourGroupingMode, settings.contourIntersection);
         groupContoursPipe.setParams(groupContoursParams);
 
-        SortContoursPipe.SortContoursParams sortContoursParams =
+        var sortContoursParams =
                 new SortContoursPipe.SortContoursParams(
                         settings.contourSortMode,
                         settings.outputShowMultipleTargets ? 5 : 1, // TODO don't hardcode?
                         frameStaticProperties);
         sortContoursPipe.setParams(sortContoursParams);
 
-        Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
+        var collect2dTargetsParams =
                 new Collect2dTargetsPipe.Collect2dTargetsParams(
                         settings.offsetRobotOffsetMode,
                         settings.offsetSinglePoint,
@@ -121,35 +126,14 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
                         frameStaticProperties);
         collect2dTargetsPipe.setParams(collect2dTargetsParams);
 
-        var params =
+        var cornerDetectionPipeParams =
                 new CornerDetectionPipe.CornerDetectionPipeParameters(
                         settings.cornerDetectionStrategy,
                         settings.cornerDetectionUseConvexHulls,
                         settings.cornerDetectionExactSideCount,
                         settings.cornerDetectionSideCount,
                         settings.cornerDetectionAccuracyPercentage);
-        cornerDetectionPipe.setParams(params);
-
-        Draw2dTargetsPipe.Draw2dTargetsParams draw2DTargetsParams =
-                new Draw2dTargetsPipe.Draw2dTargetsParams(
-                        settings.outputShouldDraw, settings.outputShowMultipleTargets);
-        draw2dTargetsPipe.setParams(draw2DTargetsParams);
-
-        Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
-                new Draw2dCrosshairPipe.Draw2dCrosshairParams(
-                        settings.outputShouldDraw,
-                        settings.offsetRobotOffsetMode,
-                        settings.offsetSinglePoint,
-                        dualOffsetValues,
-                        frameStaticProperties);
-        draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
-
-        var draw3dContoursParams =
-                new Draw3dTargetsPipe.Draw3dContoursParams(
-                        settings.outputShouldDraw,
-                        frameStaticProperties.cameraCalibration,
-                        settings.targetModel);
-        draw3dTargetsPipe.setParams(draw3dContoursParams);
+        cornerDetectionPipe.setParams(cornerDetectionPipeParams);
 
         var solvePNPParams =
                 new SolvePNPPipe.SolvePNPPipeParams(
@@ -161,107 +145,81 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
 
     @Override
     public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
-        setPipeParams(frame.frameStaticProperties, settings);
-
         long sumPipeNanosElapsed = 0L;
 
-        var rotateImageResult = rotateImagePipe.run(frame.image.getMat());
-        sumPipeNanosElapsed += pipeProfileNanos[0] = rotateImageResult.nanosElapsed;
+        CVPipeResult<Mat> hsvPipeResult;
+        Mat rawInputMat;
+        if (frame.image.getMat().channels() != 1) {
+            var rotateImageResult = rotateImagePipe.run(frame.image.getMat());
+            sumPipeNanosElapsed += pipeProfileNanos[0] = rotateImageResult.nanosElapsed;
 
-        // TODO: make this a pipe?
-        long inputCopyStartNanos = System.nanoTime();
-        rawInputMat.release();
-        frame.image.getMat().copyTo(rawInputMat);
-        long inputCopyElapsedNanos = System.nanoTime() - inputCopyStartNanos;
-        sumPipeNanosElapsed += pipeProfileNanos[1] = inputCopyElapsedNanos;
+            rawInputMat = frame.image.getMat();
 
-        CVPipeResult<Mat> hsvPipeResult = hsvPipe.run(rawInputMat);
-        sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
-        pipeProfileNanos[2] = pipeProfileNanos[2] = hsvPipeResult.nanosElapsed;
+            hsvPipeResult = hsvPipe.run(rawInputMat);
+            sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
+            pipeProfileNanos[1] = pipeProfileNanos[1] = hsvPipeResult.nanosElapsed;
+        } else {
+            long inputMatPtr = PicamJNI.grabFrame(true);
+            if (inputMatPtr != 0) rawInputMat = new Mat(inputMatPtr);
+            else rawInputMat = frame.image.getMat();
+
+            // We can skip a few steps if the image is single channel because we've already done them on
+            // the GPU
+            hsvPipeResult = new CVPipeResult<>();
+            hsvPipeResult.output = frame.image.getMat();
+            hsvPipeResult.nanosElapsed = System.nanoTime() - frame.timestampNanos;
+
+            sumPipeNanosElapsed = pipeProfileNanos[1] = hsvPipeResult.nanosElapsed;
+        }
 
         CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.run(hsvPipeResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[3] = findContoursResult.nanosElapsed;
+        sumPipeNanosElapsed += pipeProfileNanos[2] = findContoursResult.nanosElapsed;
 
         CVPipeResult<List<Contour>> speckleRejectResult =
                 speckleRejectPipe.run(findContoursResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[4] = speckleRejectResult.nanosElapsed;
+        sumPipeNanosElapsed += pipeProfileNanos[3] = speckleRejectResult.nanosElapsed;
 
         CVPipeResult<List<Contour>> filterContoursResult =
                 filterContoursPipe.run(speckleRejectResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[5] = filterContoursResult.nanosElapsed;
+        sumPipeNanosElapsed += pipeProfileNanos[4] = filterContoursResult.nanosElapsed;
 
         CVPipeResult<List<PotentialTarget>> groupContoursResult =
                 groupContoursPipe.run(filterContoursResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[6] = groupContoursResult.nanosElapsed;
+        sumPipeNanosElapsed += pipeProfileNanos[5] = groupContoursResult.nanosElapsed;
 
         CVPipeResult<List<PotentialTarget>> sortContoursResult =
                 sortContoursPipe.run(groupContoursResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[7] = sortContoursResult.nanosElapsed;
+        sumPipeNanosElapsed += pipeProfileNanos[6] = sortContoursResult.nanosElapsed;
 
         CVPipeResult<List<TrackedTarget>> collect2dTargetsResult =
                 collect2dTargetsPipe.run(sortContoursResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[8] = collect2dTargetsResult.nanosElapsed;
+        sumPipeNanosElapsed += pipeProfileNanos[7] = collect2dTargetsResult.nanosElapsed;
 
         List<TrackedTarget> targetList;
 
         // 3d stuff
         if (settings.solvePNPEnabled) {
             var cornerDetectionResult = cornerDetectionPipe.run(collect2dTargetsResult.output);
-            sumPipeNanosElapsed += pipeProfileNanos[9] = cornerDetectionResult.nanosElapsed;
+            sumPipeNanosElapsed += pipeProfileNanos[8] = cornerDetectionResult.nanosElapsed;
 
             var solvePNPResult = solvePNPPipe.run(cornerDetectionResult.output);
-            sumPipeNanosElapsed += pipeProfileNanos[10] = solvePNPResult.nanosElapsed;
+            sumPipeNanosElapsed += pipeProfileNanos[9] = solvePNPResult.nanosElapsed;
 
             targetList = solvePNPResult.output;
         } else {
+            pipeProfileNanos[8] = 0;
             pipeProfileNanos[9] = 0;
-            pipeProfileNanos[10] = 0;
             targetList = collect2dTargetsResult.output;
         }
 
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
-        sumPipeNanosElapsed += fpsResult.nanosElapsed;
-
-        // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
-        var outputMatPipeResult = outputMatPipe.run(hsvPipeResult.output);
-        sumPipeNanosElapsed += pipeProfileNanos[11] = outputMatPipeResult.nanosElapsed;
-
-        // Draw 2D Crosshair on input and output
-        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(rawInputMat, targetList));
-        sumPipeNanosElapsed += pipeProfileNanos[12] = draw2dCrosshairResultOnInput.nanosElapsed;
-
-        var draw2dCrosshairResultOnOutput =
-                draw2dCrosshairPipe.run(Pair.of(hsvPipeResult.output, targetList));
-        sumPipeNanosElapsed += pipeProfileNanos[13] = draw2dCrosshairResultOnOutput.nanosElapsed;
-
-        // Draw 2D contours on input and output
-        var draw2dTargetsOnInput =
-                draw2dTargetsPipe.run(Triple.of(rawInputMat, collect2dTargetsResult.output, fps));
-        sumPipeNanosElapsed += pipeProfileNanos[14] = draw2dTargetsOnInput.nanosElapsed;
-
-        var draw2dTargetsOnOutput =
-                draw2dTargetsPipe.run(Triple.of(hsvPipeResult.output, collect2dTargetsResult.output, fps));
-        sumPipeNanosElapsed += pipeProfileNanos[15] = draw2dTargetsOnOutput.nanosElapsed;
-
-        // Draw 3D Targets on input and output if necessary
-        if (settings.solvePNPEnabled) {
-            var drawOnInputResult =
-                    draw3dTargetsPipe.run(Pair.of(rawInputMat, collect2dTargetsResult.output));
-            sumPipeNanosElapsed += pipeProfileNanos[16] = drawOnInputResult.nanosElapsed;
-
-            var drawOnOutputResult =
-                    draw3dTargetsPipe.run(Pair.of(hsvPipeResult.output, collect2dTargetsResult.output));
-            sumPipeNanosElapsed += pipeProfileNanos[17] = drawOnOutputResult.nanosElapsed;
-        } else {
-            pipeProfileNanos[16] = 0;
-            pipeProfileNanos[17] = 0;
-        }
 
         PipelineProfiler.printReflectiveProfile(pipeProfileNanos);
 
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                fps,
                 targetList,
                 new Frame(new CVMat(hsvPipeResult.output), frame.frameStaticProperties),
                 new Frame(new CVMat(rawInputMat), frame.frameStaticProperties));

photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java

@@ -18,7 +18,6 @@
 package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.annotation.JsonTypeName;
-import java.util.Objects;
 import org.photonvision.vision.opencv.ContourGroupingMode;
 import org.photonvision.vision.opencv.ContourIntersectionDirection;
 import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
@@ -48,37 +47,4 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
         super();
         pipelineType = PipelineType.Reflective;
     }
-
-    @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        if (!super.equals(o)) return false;
-        ReflectivePipelineSettings that = (ReflectivePipelineSettings) o;
-        return solvePNPEnabled == that.solvePNPEnabled
-                && cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
-                && cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
-                && cornerDetectionSideCount == that.cornerDetectionSideCount
-                && Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
-                        == 0
-                && contourGroupingMode == that.contourGroupingMode
-                && contourIntersection == that.contourIntersection
-                && targetModel.equals(that.targetModel)
-                && cornerDetectionStrategy == that.cornerDetectionStrategy;
-    }
-
-    @Override
-    public int hashCode() {
-        return Objects.hash(
-                super.hashCode(),
-                contourGroupingMode,
-                contourIntersection,
-                solvePNPEnabled,
-                targetModel,
-                cornerDetectionStrategy,
-                cornerDetectionUseConvexHulls,
-                cornerDetectionExactSideCount,
-                cornerDetectionSideCount,
-                cornerDetectionAccuracyPercentage);
-    }
 }

photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java

@@ -26,21 +26,28 @@ import org.photonvision.vision.target.TrackedTarget;
 public class CVPipelineResult implements Releasable {
     private double latencyMillis;
     public final double processingMillis;
+    public final double fps;
     public final List<TrackedTarget> targets;
     public final Frame outputFrame;
     public final Frame inputFrame;
 
     public CVPipelineResult(
-            double processingMillis, List<TrackedTarget> targets, Frame outputFrame, Frame inputFrame) {
+            double processingMillis,
+            double fps,
+            List<TrackedTarget> targets,
+            Frame outputFrame,
+            Frame inputFrame) {
         this.processingMillis = processingMillis;
+        this.fps = fps;
         this.targets = targets != null ? targets : Collections.emptyList();
 
-        this.outputFrame = Frame.copyFromAndRelease(outputFrame);
-        this.inputFrame = inputFrame != null ? Frame.copyFromAndRelease(inputFrame) : null;
+        this.outputFrame = outputFrame;
+        this.inputFrame = inputFrame;
     }
 
-    public CVPipelineResult(double processingMillis, List<TrackedTarget> targets, Frame outputFrame) {
-        this(processingMillis, targets, outputFrame, null);
+    public CVPipelineResult(
+            double processingMillis, double fps, List<TrackedTarget> targets, Frame outputFrame) {
+        this(processingMillis, fps, targets, outputFrame, null);
     }
 
     public boolean hasTargets() {

photon-server/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java

@@ -21,7 +21,7 @@ import java.util.List;
 import org.photonvision.vision.frame.Frame;
 
 public class DriverModePipelineResult extends CVPipelineResult {
-    public DriverModePipelineResult(double latencyMillis, Frame outputFrame) {
-        super(latencyMillis, List.of(), outputFrame);
+    public DriverModePipelineResult(double latencyMillis, double fps, Frame outputFrame) {
+        super(latencyMillis, fps, List.of(), outputFrame);
     }
 }

photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -37,8 +37,12 @@ import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.CameraQuirk;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.camera.USBCameraSource;
+import org.photonvision.vision.camera.ZeroCopyPicamSource;
+import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer;
 import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
+import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
+import org.photonvision.vision.pipeline.OutputStreamPipeline;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -53,12 +57,13 @@ import org.photonvision.vision.target.TrackedTarget;
 */
 public class VisionModule {
 
-    private static final int StreamFPSCap = 30;
+    private static final int streamFPSCap = 30;
 
     private final Logger logger;
     protected final PipelineManager pipelineManager;
     protected final VisionSource visionSource;
     private final VisionRunner visionRunner;
+    private final StreamRunnable streamRunnable;
     private final LinkedList<CVPipelineResultConsumer> resultConsumers = new LinkedList<>();
     private final LinkedList<CVPipelineResultConsumer> fpsLimitedResultConsumers = new LinkedList<>();
     private final NTDataPublisher ntConsumer;
@@ -88,11 +93,14 @@ public class VisionModule {
                         this.visionSource.getFrameProvider(),
                         this.pipelineManager::getCurrentUserPipeline,
                         this::consumeResult);
+        this.streamRunnable = new StreamRunnable(new OutputStreamPipeline());
         this.moduleIndex = index;
 
         // do this
         if (visionSource instanceof USBCameraSource) {
             cameraQuirks = ((USBCameraSource) visionSource).cameraQuirks;
+        } else if (visionSource instanceof ZeroCopyPicamSource) {
+            cameraQuirks = QuirkyCamera.ZeroCopyPiCamera;
         } else {
             cameraQuirks = QuirkyCamera.DefaultCamera;
         }
@@ -100,10 +108,8 @@ public class VisionModule {
         DataChangeService.getInstance().addSubscriber(new VisionModuleChangeSubscriber(this));
 
         createStreams();
-        fpsLimitedResultConsumers.add(result -> dashboardInputStreamer.accept(result.inputFrame));
-        fpsLimitedResultConsumers.add(result -> dashboardOutputStreamer.accept(result.outputFrame));
-        fpsLimitedResultConsumers.add(result -> inputFrameSaver.accept(result.inputFrame));
-        fpsLimitedResultConsumers.add(result -> outputFrameSaver.accept(result.outputFrame));
+
+        recreateFpsLimitedResultConsumers();
 
         ntConsumer =
                 new NTDataPublisher(
@@ -169,6 +175,99 @@ public class VisionModule {
                         visionSource.getSettables().getConfiguration().nickname, "output");
     }
 
+    private void recreateFpsLimitedResultConsumers() {
+        // Important! These must come before the stream result consumers because the stream result
+        // consumers release the frame
+        fpsLimitedResultConsumers.add(result -> inputFrameSaver.accept(result.inputFrame));
+        fpsLimitedResultConsumers.add(result -> outputFrameSaver.accept(result.outputFrame));
+
+        fpsLimitedResultConsumers.add(
+                result -> {
+                    if (this.pipelineManager.getCurrentPipelineSettings().inputShouldShow)
+                        dashboardInputStreamer.accept(result.inputFrame);
+                });
+        fpsLimitedResultConsumers.add(
+                result -> {
+                    if (this.pipelineManager.getCurrentPipelineSettings().outputShouldShow)
+                        dashboardOutputStreamer.accept(result.outputFrame);
+                });
+    }
+
+    private class StreamRunnable extends Thread {
+        private final OutputStreamPipeline outputStreamPipeline;
+
+        private final Object frameLock = new Object();
+        private Frame inputFrame, outputFrame;
+        private AdvancedPipelineSettings settings = new AdvancedPipelineSettings();
+        private List<TrackedTarget> targets = new ArrayList<>();
+
+        private boolean shouldRun = false;
+
+        public StreamRunnable(OutputStreamPipeline outputStreamPipeline) {
+            this.outputStreamPipeline = outputStreamPipeline;
+        }
+
+        public void updateData(
+                Frame inputFrame,
+                Frame outputFrame,
+                AdvancedPipelineSettings settings,
+                List<TrackedTarget> targets) {
+            synchronized (frameLock) {
+                if (shouldRun && this.inputFrame != null && this.outputFrame != null) {
+                    logger.trace("Fell behind; releasing last unused Mats");
+                    this.inputFrame.release();
+                    this.outputFrame.release();
+                }
+
+                this.inputFrame = inputFrame;
+                this.outputFrame = outputFrame;
+                this.settings = settings;
+                this.targets = targets;
+
+                shouldRun =
+                        inputFrame != null
+                                && !inputFrame.image.getMat().empty()
+                                && outputFrame != null
+                                && !outputFrame.image.getMat().empty();
+            }
+        }
+
+        @Override
+        public void run() {
+            while (true) {
+                final Frame inputFrame, outputFrame;
+                final AdvancedPipelineSettings settings;
+                final List<TrackedTarget> targets;
+                final boolean shouldRun;
+                synchronized (frameLock) {
+                    inputFrame = this.inputFrame;
+                    outputFrame = this.outputFrame;
+                    this.inputFrame = null;
+                    this.outputFrame = null;
+
+                    settings = this.settings;
+                    targets = this.targets;
+                    shouldRun = this.shouldRun;
+
+                    this.shouldRun = false;
+                }
+                if (shouldRun) {
+                    var osr = outputStreamPipeline.process(inputFrame, outputFrame, settings, targets);
+                    consumeFpsLimitedResult(osr);
+                    inputFrame.release();
+                    outputFrame.release();
+                } else {
+                    // busy wait! hurray!
+                    try {
+                        Thread.sleep(1);
+                    } catch (InterruptedException e) {
+                        e.printStackTrace();
+                    }
+                }
+            }
+        }
+    }
+
     void setDriverMode(boolean isDriverMode) {
         pipelineManager.setDriverMode(isDriverMode);
         setVisionLEDs(!isDriverMode);
@@ -177,6 +276,7 @@ public class VisionModule {
 
     public void start() {
         visionRunner.startProcess();
+        streamRunnable.start();
     }
 
     public void setFovAndPitch(double fov, Rotation2d pitch) {
@@ -271,7 +371,7 @@ public class VisionModule {
         if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             config.cameraGain = -1;
         } else {
-            visionSource.getSettables().setGain(config.cameraGain);
+            visionSource.getSettables().setGain(Math.max(0, config.cameraGain));
         }
 
         setVisionLEDs(config.ledMode);
@@ -313,17 +413,15 @@ public class VisionModule {
         inputFrameSaver.updateCameraNickname(newName);
         outputFrameSaver.updateCameraNickname(newName);
 
-        // rename streams
+        // Rename streams
         fpsLimitedResultConsumers.clear();
 
         // Teardown and recreate streams
         destroyStreams();
         createStreams();
 
-        fpsLimitedResultConsumers.add(result -> dashboardInputStreamer.accept(result.inputFrame));
-        fpsLimitedResultConsumers.add(result -> dashboardOutputStreamer.accept(result.outputFrame));
-        fpsLimitedResultConsumers.add(result -> inputFrameSaver.accept(result.inputFrame));
-        fpsLimitedResultConsumers.add(result -> outputFrameSaver.accept(result.outputFrame));
+        // Rebuild streamers
+        recreateFpsLimitedResultConsumers();
 
         // Push new data to the UI
         saveAndBroadcastAll();
@@ -395,9 +493,21 @@ public class VisionModule {
 
     private void consumeResult(CVPipelineResult result) {
         consumePipelineResult(result);
-        consumeFpsLimitedResult(result);
 
-        result.release();
+        // Pipelines like DriverMode and Calibrate3dPipeline have null output frames
+        if (result.inputFrame != null) {
+            streamRunnable.updateData(
+                    result.inputFrame,
+                    result.outputFrame,
+                    (AdvancedPipelineSettings) pipelineManager.getCurrentPipelineSettings(),
+                    result.targets);
+            // The streamRunnable manages releasing in this case
+        } else {
+            consumeFpsLimitedResult(result);
+
+            result.release();
+            // In this case we don't bother with a separate streaming thread and we release
+        }
     }
 
     private void consumePipelineResult(CVPipelineResult result) {
@@ -407,7 +517,8 @@ public class VisionModule {
     }
 
     private void consumeFpsLimitedResult(CVPipelineResult result) {
-        if (System.currentTimeMillis() - lastFrameConsumeMillis > 1000 / StreamFPSCap) {
+        long dt = System.currentTimeMillis() - lastFrameConsumeMillis;
+        if (dt > 1000 / streamFPSCap) {
             for (var c : fpsLimitedResultConsumers) {
                 c.accept(result);
             }

photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java

@@ -29,8 +29,10 @@ import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;
+import org.photonvision.raspi.PicamJNI;
 import org.photonvision.vision.camera.CameraType;
 import org.photonvision.vision.camera.USBCameraSource;
+import org.photonvision.vision.camera.ZeroCopyPicamSource;
 
 public class VisionSourceManager {
 
@@ -152,17 +154,17 @@ public class VisionSourceManager {
         // Turn these camera configs into vision sources
         var sources = loadVisionSourcesFromCamConfigs(matchedCameras);
 
-        // These sources can be turned into USB cameras, which can be added to the config manager
+        // We want to return a map between vision sources and camera configurations
         var visionSourceMap = new HashMap<VisionSource, CameraConfiguration>();
         for (var src : sources) {
-            var usbSrc = (USBCameraSource) src;
-            visionSourceMap.put(usbSrc, usbSrc.configuration);
+            var usbSrc = src;
+            visionSourceMap.put(usbSrc, usbSrc.getSettables().getConfiguration());
             logger.debug(
                     () ->
                             "Matched config for camera \""
                                     + src.getFrameProvider().getName()
                                     + "\" and loaded "
-                                    + usbSrc.configuration.pipelineSettings.size()
+                                    + usbSrc.getSettables().getConfiguration().pipelineSettings.size()
                                     + " pipelines");
         }
         return visionSourceMap;
@@ -255,12 +257,6 @@ public class VisionSourceManager {
         return cfg;
     }
 
-    private List<VisionSource> loadVisionSourcesFromCamConfigs(List<CameraConfiguration> camConfigs) {
-        List<VisionSource> usbCameraSources = new ArrayList<>();
-        camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
-        return usbCameraSources;
-    }
-
     private List<UsbCameraInfo> filterAllowedDevices(List<UsbCameraInfo> allDevices) {
         List<UsbCameraInfo> filteredDevices = new ArrayList<>();
         for (var device : allDevices) {
@@ -294,6 +290,21 @@ public class VisionSourceManager {
         return baseName.replaceAll(" ", "_");
     }
 
+    private static List<VisionSource> loadVisionSourcesFromCamConfigs(
+            List<CameraConfiguration> camConfigs) {
+        List<VisionSource> cameraSources = new ArrayList<>();
+        for (var configuration : camConfigs) {
+            if (configuration.baseName.startsWith("mmal service") && PicamJNI.isSupported()) {
+                configuration.cameraType = CameraType.ZeroCopyPicam;
+                VisionSource picamSrc = new ZeroCopyPicamSource(configuration);
+                cameraSources.add(picamSrc);
+                continue;
+            }
+            cameraSources.add(new USBCameraSource(configuration));
+        }
+        return cameraSources;
+    }
+
     /**
     * Check if a given config list contains the given unique name.
     *