diff --git a/.gitignore b/.gitignore index 9f8966e07..c0af66100 100644 --- a/.gitignore +++ b/.gitignore @@ -125,3 +125,4 @@ photon-server/build photon-server/photon-vision photon-server/src/main/resources/web photon-server/src/main/java/org/photonvision/PhotonVersion.java +photon-server/src/main/generated/native/include/org_photonvision_raspi_PicamJNI.h diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js index fad74334b..2f09ef3f9 100644 --- a/photon-client/src/store/index.js +++ b/photon-client/src/store/index.js @@ -21,7 +21,6 @@ export default new Vuex.Store({ compactMode: localStorage.getItem("compactMode") === undefined ? undefined : localStorage.getItem("compactMode") === "true", // Compact mode is initially unset on purpose logMessages: [], currentCameraIndex: 0, - selectedOutputs: [0, 1], // 0 indicates normal, 1 indicates threshold cameraSettings: [ // This is a list of objects representing the settings of all cameras { tiltDegrees: 0.0, @@ -65,6 +64,8 @@ export default new Vuex.Store({ contourGroupingMode: 0, contourIntersection: 0, contourSortMode: 0, + inputShouldShow: true, + outputShouldShow: true, outputShouldDraw: true, outputShowMultipleTargets: false, offsetRobotOffsetMode: 0, @@ -144,7 +145,7 @@ export default new Vuex.Store({ metrics: set('metrics'), logString: (state, newStr) => { const str = state.logMessages; - str.push(newStr) + str.push(newStr); Vue.set(state, 'logString', str) }, @@ -238,6 +239,7 @@ export default new Vuex.Store({ currentCameraIndex: state => state.currentCameraIndex, currentPipelineIndex: state => state.cameraSettings[state.currentCameraIndex].currentPipelineIndex, currentPipelineSettings: state => state.cameraSettings[state.currentCameraIndex].currentPipelineSettings, + currentVideoFormat: state => state.cameraSettings[state.currentCameraIndex].videoFormatList[state.cameraSettings[state.currentCameraIndex].currentPipelineSettings.cameraVideoModeIndex], videoFormatList: state => { return Object.values(state.cameraSettings[state.currentCameraIndex].videoFormatList); // convert to a list }, diff --git a/photon-client/src/views/LogsView.vue b/photon-client/src/views/LogsView.vue index dc77e14bf..70b136004 100644 --- a/photon-client/src/views/LogsView.vue +++ b/photon-client/src/views/LogsView.vue @@ -1,67 +1,69 @@ \ No newline at end of file + diff --git a/photon-client/src/views/PipelineViews/PnPTab.vue b/photon-client/src/views/PipelineViews/PnPTab.vue index 7c76c62b3..a9912f3f9 100644 --- a/photon-client/src/views/PipelineViews/PnPTab.vue +++ b/photon-client/src/views/PipelineViews/PnPTab.vue @@ -53,7 +53,7 @@ import CVslider from '../../components/common/cv-slider' export default { - name: "SolvePNP", + name: "PnP", components: { CVslider, miniMap diff --git a/photon-client/src/views/PipelineViews/ThresholdTab.vue b/photon-client/src/views/PipelineViews/ThresholdTab.vue index 75b4fd419..9dbbdab48 100644 --- a/photon-client/src/views/PipelineViews/ThresholdTab.vue +++ b/photon-client/src/views/PipelineViews/ThresholdTab.vue @@ -174,6 +174,7 @@ case 0: this.currentFunction = undefined; this.$store.state.colorPicking = false; + this.handlePipelineUpdate("outputShouldDraw", true); return; case 1: @@ -187,7 +188,10 @@ break; } this.$store.state.colorPicking = true; + this.handlePipelineUpdate("outputShouldDraw", false); + this.$store.commit("mutatePipeline", {"inputShouldShow": true}); + this.handlePipelineUpdate("inputShouldShow", true); } } } diff --git a/photon-client/src/views/SettingsViews/General.vue b/photon-client/src/views/SettingsViews/General.vue index 6a98d2cae..67b7ceb57 100644 --- a/photon-client/src/views/SettingsViews/General.vue +++ b/photon-client/src/views/SettingsViews/General.vue @@ -3,43 +3,88 @@ - - - - + + + + - - - - + + + +
Version Hardware Model Platform GPU Acceleration + Version + + Hardware Model + + Platform + + GPU Acceleration +
{{ version.replace(" ", "") }}{{ hwModel.replace(" ", "") }}{{ platform.replace(" ", "") }}{{ gpuAccel.replace(" ", "") }} + {{ version.replace(" ", "") }} + + {{ hwModel.replace(" ", "") }} + + {{ platform.replace(" ", "") }} + + {{ gpuAccel.replace(" ", "") }} +
- - - - - + + + + + - - - - - + + + + + - - - - - + + + + +
CPU Usage CPU Temp CPU Memory Usage GPU Memory Usage Disk Usage + CPU Usage + + CPU Temp + + CPU Memory Usage + + GPU Memory Usage + + Disk Usage +
{{ metrics.cpuUtil.replace(" ", "") }}%{{ parseInt(metrics.cpuTemp) }}° C{{ metrics.ramUtil.replace(" ", "") }}MB of {{ metrics.cpuMem }}MB{{ metrics.gpuMemUtil.replace(" ", "") }}MB of {{ metrics.gpuMem }}MB{{ metrics.diskUtilPct.replace(" ", "") }} + {{ metrics.cpuUtil.replace(" ", "") }}% + + {{ parseInt(metrics.cpuTemp) }}° C + + {{ metrics.ramUtil.replace(" ", "") }}MB of {{ metrics.cpuMem }}MB + + {{ metrics.gpuMemUtil.replace(" ", "") }}MB of {{ metrics.gpuMem }}MB + + {{ metrics.diskUtilPct.replace(" ", "") }} +
--------------- + --- + + --- + + --- + + --- + + --- +
- diff --git a/photon-server/build.gradle b/photon-server/build.gradle index 9a9e5acf2..166e35139 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -19,6 +19,9 @@ sourceCompatibility = 11 repositories { jcenter() mavenCentral() + maven { + url = "https://maven.photonvision.org/repository/internal/" + } maven { url = 'https://frcmaven.wpi.edu:443/artifactory/development' } @@ -26,6 +29,7 @@ repositories { ext { wpilibVersion = '2020.3.2-99-g9f4de91' + joglVersion = '2.4.0-rc-20200307' openCVVersion = '3.4.7-2' } @@ -46,7 +50,23 @@ dependencies { implementation "org.apache.commons:commons-collections4:4.4" implementation "org.apache.commons:commons-exec:1.3" - // wpilib stuff + // JOGL stuff (currently we only distribute for aarch64, which is Pi 4) + implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion" + implementation "org.jogamp.jogl:jogl-all:$joglVersion" + +// implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion:natives-linux-amd64" +// implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion:natives-linux-armv6hf" + implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion:natives-linux-aarch64" +// implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion:natives-macosx-universal" +// implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion:natives-windows-amd64" + +// implementation "org.jogamp.jogl:jogl-all:$joglVersion:natives-linux-amd64" +// implementation "org.jogamp.jogl:jogl-all:$joglVersion:natives-linux-armv6hf" + implementation "org.jogamp.jogl:jogl-all:$joglVersion:natives-linux-aarch64" +// implementation "org.jogamp.jogl:jogl-all:$joglVersion:natives-macosx-universal" +// implementation "org.jogamp.jogl:jogl-all:$joglVersion:natives-windows-amd64" + + // WPILib stuff implementation "edu.wpi.first.wpiutil:wpiutil-java:$wpilibVersion" implementation "edu.wpi.first.cameraserver:cameraserver-java:$wpilibVersion" @@ -141,6 +161,18 @@ spotless { } } +run { + if (project.hasProperty("profile")) { + jvmArgs=[ + "-Dcom.sun.management.jmxremote=true", + "-Dcom.sun.management.jmxremote.ssl=false", + "-Dcom.sun.management.jmxremote.authenticate=false", + "-Dcom.sun.management.jmxremote.port=5000", + "-Djava.rmi.server.hostname=0.0.0.0", + ] + } +} + jacocoTestReport { dependsOn test // Tests are required to run before generating the report diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index a235dfc7f..b3cd914fd 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/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) { diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java b/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java index 759efc476..98789b47c 100644 --- a/photon-server/src/main/java/org/photonvision/common/configuration/HardwareConfig.java +++ b/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; diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index 2cdf66096..6d64be5d0 100644 --- a/photon-server/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/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(); 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); diff --git a/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java index cd06340d1..1ffaaa936 100644 --- a/photon-server/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java +++ b/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>(); var dataMap = new HashMap(); + dataMap.put("fps", result.fps); dataMap.put("latency", result.getLatencyMillis()); var targets = result.targets; diff --git a/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java index 63b595699..3e3b7e0fd 100644 --- a/photon-server/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/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; diff --git a/photon-server/src/main/java/org/photonvision/raspi/PicamJNI.java b/photon-server/src/main/java/org/photonvision/raspi/PicamJNI.java new file mode 100644 index 000000000..c7a1fbd91 --- /dev/null +++ b/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 . + */ + +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); +} diff --git a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java b/photon-server/src/main/java/org/photonvision/server/SocketHandler.java index 61b099c5f..4f5507571 100644 --- a/photon-server/src/main/java/org/photonvision/server/SocketHandler.java +++ b/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"}) diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/CameraType.java b/photon-server/src/main/java/org/photonvision/vision/camera/CameraType.java index f90d1a02f..edfb0a4a4 100644 --- a/photon-server/src/main/java/org/photonvision/vision/camera/CameraType.java +++ b/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 } diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index 1594702c9..31787135d 100644 --- a/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-server/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -26,10 +26,17 @@ public class QuirkyCamera { private static final List 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; diff --git a/photon-server/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java b/photon-server/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java new file mode 100644 index 000000000..e60294cd6 --- /dev/null +++ b/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 . + */ + +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 getAllVideoModes() { + return videoModes; + } + } + + @Override + public boolean isVendorCamera() { + return ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV(); + } +} diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java b/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java index 29899c5a8..6763f14bf 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/Frame.java +++ b/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 diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProvider.java b/photon-server/src/main/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProvider.java new file mode 100644 index 000000000..0394f64ac --- /dev/null +++ b/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 . + */ + +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()); + } +} diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index b9e50b0d5..1118b9f18 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/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 diff --git a/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index a77bec076..069852513 100644 --- a/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-server/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -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 diff --git a/photon-server/src/main/java/org/photonvision/vision/opencv/CVMat.java b/photon-server/src/main/java/org/photonvision/vision/opencv/CVMat.java index f625c70c4..bc06be610 100644 --- a/photon-server/src/main/java/org/photonvision/vision/opencv/CVMat.java +++ b/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 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); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java index a6496f350..7ab74330e 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Draw2dTargetsPipe.java @@ -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, Integer>, Draw2dTargetsPipe.Draw2dTargetsParams> { + extends MutatingPipe>, Draw2dTargetsPipe.Draw2dTargetsParams> { private List m_drawnContours = new ArrayList<>(); @Override - protected Void process(Triple, Integer> in) { - // Always draw FPS + protected Void process(Pair> 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; diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindContoursPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindContoursPipe.java index b243f46fa..55820f060 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindContoursPipe.java +++ b/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()); } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/GPUAcceleratedHSVPipe.java new file mode 100644 index 000000000..a3ac0de55 --- /dev/null +++ b/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 . + */ + +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 { + + 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; + } +} diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java index d406ad64f..136a5bf92 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/HSVPipe.java +++ b/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 { - - 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 { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java index 4617be1d6..4e46aa20d 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/AdvancedPipelineSettings.java +++ b/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); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java index 88fea9be4..b0fe3ff9a 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java +++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java @@ -39,19 +39,19 @@ public abstract class CVPipeline(); 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() { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index dce29cbc8..5e619d605 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/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)); diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java index 19e309340..3d9ce13e3 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java +++ b/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); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index 01b96d2e9..f07f018c8 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/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)); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java index 1c14c6103..ede5c3338 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java +++ b/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; } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java new file mode 100644 index 000000000..738981713 --- /dev/null +++ b/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 . + */ + +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 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)); + } +} diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java index ab5df3eee..3f39a9ce4 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/PipelineProfiler.java +++ b/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; diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 53c99a970..1894db0a0 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/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 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 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> findContoursResult = findContoursPipe.run(hsvPipeResult.output); - sumPipeNanosElapsed += pipeProfileNanos[3] = findContoursResult.nanosElapsed; + sumPipeNanosElapsed += pipeProfileNanos[2] = findContoursResult.nanosElapsed; CVPipeResult> speckleRejectResult = speckleRejectPipe.run(findContoursResult.output); - sumPipeNanosElapsed += pipeProfileNanos[4] = speckleRejectResult.nanosElapsed; + sumPipeNanosElapsed += pipeProfileNanos[3] = speckleRejectResult.nanosElapsed; CVPipeResult> filterContoursResult = filterContoursPipe.run(speckleRejectResult.output); - sumPipeNanosElapsed += pipeProfileNanos[5] = filterContoursResult.nanosElapsed; + sumPipeNanosElapsed += pipeProfileNanos[4] = filterContoursResult.nanosElapsed; CVPipeResult> groupContoursResult = groupContoursPipe.run(filterContoursResult.output); - sumPipeNanosElapsed += pipeProfileNanos[6] = groupContoursResult.nanosElapsed; + sumPipeNanosElapsed += pipeProfileNanos[5] = groupContoursResult.nanosElapsed; CVPipeResult> sortContoursResult = sortContoursPipe.run(groupContoursResult.output); - sumPipeNanosElapsed += pipeProfileNanos[7] = sortContoursResult.nanosElapsed; + sumPipeNanosElapsed += pipeProfileNanos[6] = sortContoursResult.nanosElapsed; CVPipeResult> collect2dTargetsResult = collect2dTargetsPipe.run(sortContoursResult.output); - sumPipeNanosElapsed += pipeProfileNanos[8] = collect2dTargetsResult.nanosElapsed; + sumPipeNanosElapsed += pipeProfileNanos[7] = collect2dTargetsResult.nanosElapsed; List 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)); diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java index b314e857d..0dabade01 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java +++ b/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); - } } diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 2ec0997ac..83b204512 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/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 targets; public final Frame outputFrame; public final Frame inputFrame; public CVPipelineResult( - double processingMillis, List targets, Frame outputFrame, Frame inputFrame) { + double processingMillis, + double fps, + List 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 targets, Frame outputFrame) { - this(processingMillis, targets, outputFrame, null); + public CVPipelineResult( + double processingMillis, double fps, List targets, Frame outputFrame) { + this(processingMillis, fps, targets, outputFrame, null); } public boolean hasTargets() { diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java index 6db6a5343..59596ec19 100644 --- a/photon-server/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java +++ b/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); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java index eb0c2b4cc..d4091e702 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/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 resultConsumers = new LinkedList<>(); private final LinkedList 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 targets = new ArrayList<>(); + + private boolean shouldRun = false; + + public StreamRunnable(OutputStreamPipeline outputStreamPipeline) { + this.outputStreamPipeline = outputStreamPipeline; + } + + public void updateData( + Frame inputFrame, + Frame outputFrame, + AdvancedPipelineSettings settings, + List 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 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); } diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 6a081f931..6501cbfa5 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/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(); 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 loadVisionSourcesFromCamConfigs(List camConfigs) { - List usbCameraSources = new ArrayList<>(); - camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration))); - return usbCameraSources; - } - private List filterAllowedDevices(List allDevices) { List filteredDevices = new ArrayList<>(); for (var device : allDevices) { @@ -294,6 +290,21 @@ public class VisionSourceManager { return baseName.replaceAll(" ", "_"); } + private static List loadVisionSourcesFromCamConfigs( + List camConfigs) { + List 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. * diff --git a/photon-server/src/main/resources/nativelibraries/libpicam.so b/photon-server/src/main/resources/nativelibraries/libpicam.so new file mode 100755 index 000000000..a3c37994b Binary files /dev/null and b/photon-server/src/main/resources/nativelibraries/libpicam.so differ diff --git a/photon-server/src/test/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProviderTest.java b/photon-server/src/test/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProviderTest.java new file mode 100644 index 000000000..e3633f1be --- /dev/null +++ b/photon-server/src/test/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProviderTest.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 . + */ + +package org.photonvision.vision.frame.provider; + +import java.io.IOException; +import org.junit.jupiter.api.Test; +import org.opencv.imgcodecs.Imgcodecs; +import org.photonvision.common.configuration.CameraConfiguration; +import org.photonvision.common.util.TestUtils; +import org.photonvision.raspi.PicamJNI; +import org.photonvision.vision.camera.ZeroCopyPicamSource; + +public class AcceleratedPicamFrameProviderTest { + @Test + public void testGrabFrame() throws IOException { + PicamJNI.forceLoad(); + if (!PicamJNI.isSupported()) return; + + TestUtils.loadLibraries(); + + var frameProvider = + new AcceleratedPicamFrameProvider( + new ZeroCopyPicamSource.PicamSettables(new CameraConfiguration("f", "f", "f", "f"))); + + long lastTime = System.currentTimeMillis(); + for (int i = 0; i < 10; i++) { + var frame = frameProvider.get(); + System.out.println(frame.image.getMat().get(0, 0)[0]); + + long time = System.currentTimeMillis(); + System.out.println("dt (ms): " + (time - lastTime)); + lastTime = time; + } + var mat = frameProvider.get().image.getMat(); + Imgcodecs.imwrite("out.png", mat); + } +} diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java index 1fbe2bade..5c4054d3e 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/CirclePNPTest.java @@ -111,7 +111,9 @@ public class CirclePNPTest { var frameProvider = new FileFrameProvider( TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6, false), - TestUtils.WPI2020Image.FOV); + TestUtils.WPI2020Image.FOV, + new Rotation2d(), + TestUtils.get2020LifeCamCoeffs(true)); CVPipelineResult pipelineResult = pipeline.run(frameProvider.get()); printTestResults(pipelineResult); diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/PipelineProfilerTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/PipelineProfilerTest.java index ddcdffec6..8e6f2bfcf 100644 --- a/photon-server/src/test/java/org/photonvision/vision/pipeline/PipelineProfilerTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/PipelineProfilerTest.java @@ -24,7 +24,7 @@ public class PipelineProfilerTest { @Test public void reflectiveProfile() { - long[] invalidNanos = new long[10]; + long[] invalidNanos = new long[20]; long[] validNanos = new long[PipelineProfiler.ReflectivePipeCount]; for (int i = 0; i < validNanos.length; i++) { @@ -34,8 +34,9 @@ public class PipelineProfilerTest { var invalidResult = PipelineProfiler.getReflectiveProfileString(invalidNanos); var validResult = PipelineProfiler.getReflectiveProfileString(validNanos); - Assertions.assertEquals("Invalid data", invalidResult); - Assertions.assertTrue(validResult.contains("Total: 153.0ms")); System.out.println(validResult); + + Assertions.assertEquals("Invalid data", invalidResult); + Assertions.assertTrue(validResult.contains("Total: 45.0ms")); } } diff --git a/photon-server/src/test/resources/hardware/HardwareConfig.json b/photon-server/src/test/resources/hardware/HardwareConfig.json index acfc790fd..f6e92332e 100644 --- a/photon-server/src/test/resources/hardware/HardwareConfig.json +++ b/photon-server/src/test/resources/hardware/HardwareConfig.json @@ -6,9 +6,6 @@ "statusRGBPins" : [-1, -1, -1], "ledSetCommand" : "", "ledsCanDim" : true, - "ledPWMRange" : [0, 100], - "ledPWMSetRange" : "", - "ledPWMFrequency" : 800, "ledDimCommand" : "echo 10", "ledBlinkCommand" : "echo 10", "cpuTempCommand" : "echo 10", diff --git a/photon-server/versioningHelper.gradle b/photon-server/versioningHelper.gradle index 017239f9a..5c37dbc37 100644 --- a/photon-server/versioningHelper.gradle +++ b/photon-server/versioningHelper.gradle @@ -28,7 +28,6 @@ task writeCurrentVersionJava { File versionFile = new File(Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java") .toAbsolutePath().toString()) versionFile.delete() - boolean success = versionFile.createNewFile() versionFile << "package org.photonvision;\n" + "\n" + "/*\n" + @@ -44,4 +43,4 @@ task writeCurrentVersionJava { "}" } -build.dependsOn writeCurrentVersionJava \ No newline at end of file +build.dependsOn writeCurrentVersionJava