Add RKNN / Object Detection Pipeline (#1144)

Tested on Orange Pi 5 and Cool Pi 4B. Merge with parts of the OpenCV DNN PR. 

Adds support for YOLOv5s models for Rockchip CPUs with a NPU. Right now hard coded to a note model from alex_idk. Very much still incubating and largely untested.

photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java

@@ -296,4 +296,11 @@ public class ConfigManager {
             }
         }
     }
+
+    /** Get (and create if not present) the subfolder where ML models are stored */
+    public File getModelsDirectory() {
+        var ret = new File(configDirectoryFile, "models");
+        if (!ret.exists()) ret.mkdirs();
+        return ret;
+    }
 }

photon-core/src/main/java/org/photonvision/common/configuration/NeuralNetworkModelManager.java

@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.configuration;
+
+import java.io.File;
+import java.io.FileOutputStream;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.List;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class NeuralNetworkModelManager {
+    private static NeuralNetworkModelManager INSTANCE;
+    private static final Logger logger = new Logger(NeuralNetworkModelManager.class, LogGroup.Config);
+
+    private final String MODEL_NAME = "note-640-640-yolov5s.rknn";
+    private File defaultModelFile;
+    private List<String> labels;
+
+    public static NeuralNetworkModelManager getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new NeuralNetworkModelManager();
+        }
+        return INSTANCE;
+    }
+
+    /**
+     * Perform initial setup and extract default model from JAR to the filesystem
+     *
+     * @param modelsFolder Where models live
+     */
+    public void initialize(File modelsFolder) {
+        var modelResourcePath = "/models/" + MODEL_NAME;
+        this.defaultModelFile = new File(modelsFolder, MODEL_NAME);
+        extractResource(modelResourcePath, defaultModelFile);
+
+        File labelsFile = new File(modelsFolder, "labels.txt");
+        var labelResourcePath = "/models/" + labelsFile.getName();
+        extractResource(labelResourcePath, labelsFile);
+
+        try {
+            labels = Files.readAllLines(Paths.get(labelsFile.getPath()));
+        } catch (IOException e) {
+            logger.error("Error reading labels.txt", e);
+        }
+    }
+
+    private void extractResource(String resourcePath, File outputFile) {
+        try (var in = NeuralNetworkModelManager.class.getResourceAsStream(resourcePath)) {
+            if (in == null) {
+                logger.error("Failed to find jar resource at " + resourcePath);
+                return;
+            }
+
+            if (!outputFile.exists()) {
+                try (FileOutputStream fos = new FileOutputStream(outputFile)) {
+                    int read = -1;
+                    byte[] buffer = new byte[1024];
+                    while ((read = in.read(buffer)) != -1) {
+                        fos.write(buffer, 0, read);
+                    }
+                } catch (IOException e) {
+                    logger.error("Error extracting resource to " + outputFile.toPath().toString(), e);
+                }
+            } else {
+                logger.info(
+                        "File " + outputFile.toPath().toString() + " already exists. Skipping extraction.");
+            }
+        } catch (IOException e) {
+            logger.error("Error finding jar resource " + resourcePath, e);
+        }
+    }
+
+    public File getDefaultRknnModel() {
+        return defaultModelFile;
+    }
+
+    public List<String> getLabels() {
+        return labels;
+    }
+}

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

@@ -27,6 +27,7 @@ import org.photonvision.PhotonVersion;
 import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.networking.NetworkUtils;
 import org.photonvision.common.util.SerializationUtils;
+import org.photonvision.jni.RknnDetectorJNI;
 import org.photonvision.mrcal.MrCalJNILoader;
 import org.photonvision.raspi.LibCameraJNILoader;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -142,7 +143,8 @@ public class PhotonConfiguration {
                 LibCameraJNILoader.isSupported()
                         ? "Zerocopy Libcamera Working"
                         : ""); // TODO add support for other types of GPU accel
-        generalSubmap.put("mrCalWorking", MrCalJNILoader.isWorking());
+        generalSubmap.put("mrCalWorking", MrCalJNILoader.getInstance().isLoaded());
+        generalSubmap.put("rknnSupported", RknnDetectorJNI.getInstance().isLoaded());
         generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
         generalSubmap.put("hardwarePlatform", Platform.getPlatformName());
         settingsSubmap.put("general", generalSubmap);

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

@@ -52,6 +52,7 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
             uiTargets.add(t.toHashMap());
         }
         dataMap.put("targets", uiTargets);
+        dataMap.put("classNames", result.objectDetectionClassNames);
 
         // Only send Multitag Results if they are present, similar to 3d pose
         if (result.multiTagResult.estimatedPose.isPresent) {

photon-core/src/main/java/org/photonvision/common/hardware/Platform.java

@@ -43,6 +43,7 @@ public enum Platform {
             true,
             OSType.LINUX,
             true), // Raspberry Pi 3/4 with a 64-bit image
+    LINUX_RK3588_64("Linux AARCH 64-bit with RK3588", "linuxarm64", false, OSType.LINUX, true),
     LINUX_AARCH64(
             "Linux AARCH64", "linuxarm64", false, OSType.LINUX, true), // Jetson Nano, Jetson TX2
 
@@ -94,6 +95,10 @@ public enum Platform {
         return currentPlatform.osType == OSType.LINUX;
     }
 
+    public static boolean isRK3588() {
+        return Platform.isOrangePi() || Platform.isCoolPi4b();
+    }
+
     public static boolean isRaspberryPi() {
         return currentPlatform.isPi;
     }
@@ -186,7 +191,11 @@ public enum Platform {
                 return LINUX_32;
             } else if (RuntimeDetector.isArm64()) {
                 // TODO - os detection needed?
-                return LINUX_AARCH64;
+                if (isOrangePi()) {
+                    return LINUX_RK3588_64;
+                } else {
+                    return LINUX_AARCH64;
+                }
             } else if (RuntimeDetector.isArm32()) {
                 return LINUX_ARM32;
             } else {
@@ -204,6 +213,14 @@ public enum Platform {
         return fileHasText("/proc/cpuinfo", "Raspberry Pi");
     }
 
+    private static boolean isOrangePi() {
+        return fileHasText("/proc/device-tree/model", "Orange Pi 5");
+    }
+
+    private static boolean isCoolPi4b() {
+        return fileHasText("/proc/device-tree/model", "CoolPi 4B");
+    }
+
     private static boolean isJetsonSBC() {
         // https://forums.developer.nvidia.com/t/how-to-recognize-jetson-nano-device/146624
         return fileHasText("/proc/device-tree/model", "NVIDIA Jetson");

photon-core/src/main/java/org/photonvision/jni/PhotonJNICommon.java

@@ -26,12 +26,15 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 
 public abstract class PhotonJNICommon {
-    static boolean libraryLoaded = false;
+    public abstract boolean isLoaded();
+
+    public abstract void setLoaded(boolean state);
+
     protected static Logger logger = null;
 
-    protected static synchronized void forceLoad(Class<?> clazz, List<String> libraries)
-            throws IOException {
-        if (libraryLoaded) return;
+    protected static synchronized void forceLoad(
+            PhotonJNICommon instance, Class<?> clazz, List<String> libraries) throws IOException {
+        if (instance.isLoaded()) return;
         if (logger == null) logger = new Logger(clazz, LogGroup.Camera);
 
         for (var libraryName : libraries) {
@@ -42,7 +45,7 @@ public abstract class PhotonJNICommon {
                 var in = clazz.getResourceAsStream("/nativelibraries/" + arch_name + "/" + nativeLibName);
 
                 if (in == null) {
-                    libraryLoaded = false;
+                    instance.setLoaded(false);
                     return;
                 }
 
@@ -69,15 +72,11 @@ public abstract class PhotonJNICommon {
                 break;
             }
         }
-        libraryLoaded = true;
+        instance.setLoaded(true);
     }
 
-    protected static synchronized void forceLoad(Class<?> clazz, String libraryName)
-            throws IOException {
-        forceLoad(clazz, List.of(libraryName));
-    }
-
-    public static boolean isWorking() {
-        return libraryLoaded;
+    protected static synchronized void forceLoad(
+            PhotonJNICommon instance, Class<?> clazz, String libraryName) throws IOException {
+        forceLoad(instance, clazz, List.of(libraryName));
     }
 }

photon-core/src/main/java/org/photonvision/jni/RknnDetectorJNI.java

@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.jni;
+
+import java.io.IOException;
+import java.util.Arrays;
+import java.util.List;
+import java.util.concurrent.CopyOnWriteArrayList;
+import java.util.stream.Collectors;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.TestUtils;
+import org.photonvision.rknn.RknnJNI;
+import org.photonvision.rknn.RknnJNI.RknnResult;
+import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.pipe.impl.NeuralNetworkPipeResult;
+
+public class RknnDetectorJNI extends PhotonJNICommon {
+    private static final Logger logger = new Logger(RknnDetectorJNI.class, LogGroup.General);
+    private boolean isLoaded;
+    private static RknnDetectorJNI instance = null;
+
+    private RknnDetectorJNI() {
+        isLoaded = false;
+    }
+
+    public static RknnDetectorJNI getInstance() {
+        if (instance == null) instance = new RknnDetectorJNI();
+
+        return instance;
+    }
+
+    public static synchronized void forceLoad() throws IOException {
+        TestUtils.loadLibraries();
+
+        forceLoad(getInstance(), RknnDetectorJNI.class, List.of("rga", "rknnrt", "rknn_jni"));
+    }
+
+    @Override
+    public boolean isLoaded() {
+        return isLoaded;
+    }
+
+    @Override
+    public void setLoaded(boolean state) {
+        isLoaded = state;
+    }
+
+    public static class RknnObjectDetector {
+        long objPointer = -1;
+        private List<String> labels;
+        private final Object lock = new Object();
+
+        private static final CopyOnWriteArrayList<Long> detectors = new CopyOnWriteArrayList<>();
+
+        public RknnObjectDetector(String modelPath, List<String> labels) {
+            synchronized (lock) {
+                objPointer = RknnJNI.create(modelPath, labels.size());
+                detectors.add(objPointer);
+                System.out.println(
+                        "Created " + objPointer + "! Detectors: " + Arrays.toString(detectors.toArray()));
+            }
+            this.labels = labels;
+        }
+
+        public List<String> getClasses() {
+            return labels;
+        }
+
+        /**
+         * Detect forwards using this model
+         *
+         * @param in The image to process
+         * @param nmsThresh Non-maximum supression threshold. Probably should not change
+         * @param boxThresh Minimum confidence for a box to be added. Basically just confidence
+         *     threshold
+         */
+        public List<NeuralNetworkPipeResult> detect(CVMat in, double nmsThresh, double boxThresh) {
+            RknnResult[] ret;
+            synchronized (lock) {
+                // We can technically be asked to detect and the lock might be acquired _after_ release has
+                // been called. This would mean objPointer would be invalid which would call everything to
+                // explode.
+                if (objPointer > 0) {
+                    ret = RknnJNI.detect(objPointer, in.getMat().getNativeObjAddr(), nmsThresh, boxThresh);
+                } else {
+                    logger.warn("Detect called after destroy -- giving up");
+                    return List.of();
+                }
+            }
+            if (ret == null) {
+                return List.of();
+            }
+            return List.of(ret).stream()
+                    .map(it -> new NeuralNetworkPipeResult(it.rect, it.class_id, it.conf))
+                    .collect(Collectors.toList());
+        }
+
+        public void release() {
+            synchronized (lock) {
+                if (objPointer > 0) {
+                    RknnJNI.destroy(objPointer);
+                    detectors.remove(objPointer);
+                    System.out.println(
+                            "Killed " + objPointer + "! Detectors: " + Arrays.toString(detectors.toArray()));
+                    objPointer = -1;
+                } else {
+                    logger.error("RKNN Detector has already been destroyed!");
+                }
+            }
+        }
+    }
+
+    // public static void createRknnDetector() {
+    //     objPointer =
+    //             RknnJNI.create(
+    //                     NeuralNetworkModelManager.getInstance()
+    //                             .getDefaultRknnModel()
+    //                             .getAbsolutePath()
+    //                             .toString(),
+    //                     NeuralNetworkModelManager.getInstance().getLabels().size());
+    // }
+}

photon-core/src/main/java/org/photonvision/mrcal/MrCalJNILoader.java

@@ -24,6 +24,19 @@ import org.photonvision.common.util.TestUtils;
 import org.photonvision.jni.PhotonJNICommon;
 
 public class MrCalJNILoader extends PhotonJNICommon {
+    private boolean isLoaded;
+    private static MrCalJNILoader instance = null;
+
+    private MrCalJNILoader() {
+        isLoaded = false;
+    }
+
+    public static synchronized MrCalJNILoader getInstance() {
+        if (instance == null) instance = new MrCalJNILoader();
+
+        return instance;
+    }
+
     public static synchronized void forceLoad() throws IOException {
         // Force load opencv
         TestUtils.loadLibraries();
@@ -32,6 +45,7 @@ public class MrCalJNILoader extends PhotonJNICommon {
         if (Platform.isWindows()) {
             // Order is correct to match dependencies of libraries
             forceLoad(
+                    MrCalJNILoader.getInstance(),
                     MrCalJNILoader.class,
                     List.of(
                             "libamd",
@@ -47,11 +61,21 @@ public class MrCalJNILoader extends PhotonJNICommon {
                             "mrcal_jni"));
         } else {
             // Nothing else to do on linux
-            forceLoad(MrCalJNILoader.class, List.of("mrcal_jni"));
+            forceLoad(MrCalJNILoader.getInstance(), MrCalJNILoader.class, List.of("mrcal_jni"));
         }
 
-        if (!MrCalJNILoader.isWorking()) {
+        if (!MrCalJNILoader.getInstance().isLoaded()) {
             throw new IOException("Unable to load mrcal JNI!");
         }
     }
+
+    @Override
+    public boolean isLoaded() {
+        return isLoaded;
+    }
+
+    @Override
+    public void setLoaded(boolean state) {
+        isLoaded = state;
+    }
 }

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

@@ -33,6 +33,10 @@ public abstract class CVPipe<I, O, P> {
         this.params = params;
     }
 
+    public P getParams() {
+        return this.params;
+    }
+
     /**
      * Runs the process for the pipe.
      *

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

@@ -44,6 +44,13 @@ public class ArucoDetectionPipe
     @Override
     protected List<ArucoDetectionResult> process(CVMat in) {
         var imgMat = in.getMat();
+
+        // Sanity check -- image should not be empty
+        if (imgMat.empty()) {
+            // give up is best we can do here
+            return List.of();
+        }
+
         var detections = photonDetector.detect(imgMat);
         // manually do corner refinement ourselves
         if (params.useCornerRefinement) {

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

@@ -77,7 +77,7 @@ public class Calibrate3dPipe
 
         CameraCalibrationCoefficients ret;
         var start = System.nanoTime();
-        if (MrCalJNILoader.isWorking() && params.useMrCal) {
+        if (MrCalJNILoader.getInstance().isLoaded() && params.useMrCal) {
             logger.debug("Calibrating with mrcal!");
             ret = calibrateMrcal(in);
         } else {

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

@@ -15,7 +15,7 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-package org.photonvision.vision.pipeline;
+package org.photonvision.vision.pipe.impl;
 
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
@@ -36,10 +36,10 @@ import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.ImageRotationMode;
 import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
-import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
-import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
-import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
 import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
+import org.photonvision.vision.pipeline.CVPipeline;
+import org.photonvision.vision.pipeline.Calibration3dPipelineSettings;
+import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
 

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

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import org.opencv.core.Rect2d;
+
+public class NeuralNetworkPipeResult {
+    public NeuralNetworkPipeResult(Rect2d box2, Integer classIdx, Float confidence) {
+        box = box2;
+        this.classIdx = classIdx;
+        this.confidence = confidence;
+    }
+
+    public final int classIdx;
+    public final Rect2d box;
+    public final double confidence;
+}

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

@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.util.List;
+import org.photonvision.common.configuration.NeuralNetworkModelManager;
+import org.photonvision.jni.RknnDetectorJNI.RknnObjectDetector;
+import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.opencv.Releasable;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class RknnDetectionPipe
+        extends CVPipe<CVMat, List<NeuralNetworkPipeResult>, RknnDetectionPipe.RknnDetectionPipeParams>
+        implements Releasable {
+    private RknnObjectDetector detector;
+
+    public RknnDetectionPipe() {
+        // For now this is hard-coded to defaults. Should be refactored into set pipe params, though.
+        // And ideally a little wrapper helper for only changing native stuff on content change created.
+        this.detector =
+                new RknnObjectDetector(
+                        NeuralNetworkModelManager.getInstance().getDefaultRknnModel().getAbsolutePath(),
+                        NeuralNetworkModelManager.getInstance().getLabels());
+    }
+
+    @Override
+    protected List<NeuralNetworkPipeResult> process(CVMat in) {
+        var frame = in.getMat();
+
+        // Make sure we don't get a weird empty frame
+        if (frame.empty()) {
+            return List.of();
+        }
+
+        return detector.detect(in, params.nms, params.confidence);
+    }
+
+    public static class RknnDetectionPipeParams {
+        public double confidence;
+        public double nms;
+        public int max_detections;
+
+        public RknnDetectionPipeParams() {}
+    }
+
+    public List<String> getClassNames() {
+        return detector.getClasses();
+    }
+
+    @Override
+    public void release() {
+        detector.release();
+    }
+}

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

@@ -21,9 +21,11 @@ import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.FrameThresholdType;
+import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 
-public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
+public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings>
+        implements Releasable {
     protected S settings;
     protected FrameStaticProperties frameStaticProperties;
     protected QuirkyCamera cameraQuirks;
@@ -75,4 +77,11 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
 
         return result;
     }
+
+    /**
+     * Release any native memory associated with this pipeline. Called by pipelinemanager at pipeline
+     * switch. Stubbed out, but override if needed.
+     */
+    @Override
+    public void release() {}
 }

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

@@ -32,7 +32,8 @@ import org.photonvision.vision.opencv.ImageRotationMode;
     @JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
     @JsonSubTypes.Type(value = DriverModePipelineSettings.class),
     @JsonSubTypes.Type(value = AprilTagPipelineSettings.class),
-    @JsonSubTypes.Type(value = ArucoPipelineSettings.class)
+    @JsonSubTypes.Type(value = ArucoPipelineSettings.class),
+    @JsonSubTypes.Type(value = ObjectDetectionPipelineSettings.class)
 })
 public class CVPipelineSettings implements Cloneable {
     public int pipelineIndex = 0;

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

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameThresholdType;
+import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
+import org.photonvision.vision.pipe.impl.*;
+import org.photonvision.vision.pipe.impl.RknnDetectionPipe.RknnDetectionPipeParams;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TrackedTarget;
+import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
+
+public class ObjectDetectionPipeline
+        extends CVPipeline<CVPipelineResult, ObjectDetectionPipelineSettings> {
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+    private final RknnDetectionPipe rknnPipe = new RknnDetectionPipe();
+
+    private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
+
+    public ObjectDetectionPipeline() {
+        super(PROCESSING_TYPE);
+        settings = new ObjectDetectionPipelineSettings();
+    }
+
+    public ObjectDetectionPipeline(ObjectDetectionPipelineSettings settings) {
+        super(PROCESSING_TYPE);
+        this.settings = settings;
+    }
+
+    @Override
+    protected void setPipeParamsImpl() {
+        // this needs to be based off of the current backend selected!!
+        var params = new RknnDetectionPipeParams();
+        params.confidence = settings.confidence;
+        params.nms = settings.nms;
+        rknnPipe.setParams(params);
+    }
+
+    @Override
+    protected CVPipelineResult process(Frame input_frame, ObjectDetectionPipelineSettings settings) {
+        long sumPipeNanosElapsed = 0;
+
+        // ***************** change based on backend ***********************
+
+        CVPipeResult<List<NeuralNetworkPipeResult>> ret = rknnPipe.run(input_frame.colorImage);
+        sumPipeNanosElapsed += ret.nanosElapsed;
+        List<NeuralNetworkPipeResult> targetList;
+
+        targetList = ret.output;
+        var names = rknnPipe.getClassNames();
+
+        input_frame.colorImage.getMat().copyTo(input_frame.processedImage.getMat());
+
+        // ***************** change based on backend ***********************
+
+        List<TrackedTarget> targets = new ArrayList<>();
+
+        for (var t : targetList) {
+            targets.add(
+                    new TrackedTarget(
+                            t,
+                            new TargetCalculationParameters(
+                                    false, null, null, null, null, frameStaticProperties)));
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targets, input_frame, names);
+    }
+
+    @Override
+    public void release() {
+        rknnPipe.release();
+    }
+}

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

@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+public class ObjectDetectionPipelineSettings extends AdvancedPipelineSettings {
+    public double confidence;
+    public double nms; // non maximal suppression
+
+    public ObjectDetectionPipelineSettings() {
+        super();
+        this.pipelineType = PipelineType.ObjectDetection; // TODO: FIX this
+        this.outputShowMultipleTargets = true;
+        cameraExposure = 20;
+        cameraAutoExposure = false;
+        ledMode = false;
+        confidence = .9;
+        nms = .45;
+    }
+}

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

@@ -17,6 +17,8 @@
 
 package org.photonvision.vision.pipeline;
 
+import org.photonvision.vision.pipe.impl.Calibrate3dPipeline;
+
 @SuppressWarnings("rawtypes")
 public enum PipelineType {
     Calib3d(-2, Calibrate3dPipeline.class),
@@ -24,7 +26,8 @@ public enum PipelineType {
     Reflective(0, ReflectivePipeline.class),
     ColoredShape(1, ColoredShapePipeline.class),
     AprilTag(2, AprilTagPipeline.class),
-    Aruco(3, ArucoPipeline.class);
+    Aruco(3, ArucoPipeline.class),
+    ObjectDetection(4, ObjectDetectionPipeline.class);
 
     public final int baseIndex;
     public final Class clazz;

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

@@ -32,10 +32,20 @@ public class CVPipelineResult implements Releasable {
     public final List<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
     public MultiTargetPNPResult multiTagResult;
+    public final List<String> objectDetectionClassNames;
 
     public CVPipelineResult(
             double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
-        this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame);
+        this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame, List.of());
+    }
+
+    public CVPipelineResult(
+            double processingNanos,
+            double fps,
+            List<TrackedTarget> targets,
+            Frame inputFrame,
+            List<String> classNames) {
+        this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame, classNames);
     }
 
     public CVPipelineResult(
@@ -44,10 +54,21 @@ public class CVPipelineResult implements Releasable {
             List<TrackedTarget> targets,
             MultiTargetPNPResult multiTagResult,
             Frame inputFrame) {
+        this(processingNanos, fps, targets, multiTagResult, inputFrame, List.of());
+    }
+
+    public CVPipelineResult(
+            double processingNanos,
+            double fps,
+            List<TrackedTarget> targets,
+            MultiTargetPNPResult multiTagResult,
+            Frame inputFrame,
+            List<String> classNames) {
         this.processingNanos = processingNanos;
         this.fps = fps;
         this.targets = targets != null ? targets : Collections.emptyList();
         this.multiTagResult = multiTagResult;
+        this.objectDetectionClassNames = classNames;
 
         this.inputAndOutputFrame = inputFrame;
     }
@@ -57,7 +78,7 @@ public class CVPipelineResult implements Releasable {
             double fps,
             List<TrackedTarget> targets,
             MultiTargetPNPResult multiTagResult) {
-        this(processingNanos, fps, targets, multiTagResult, null);
+        this(processingNanos, fps, targets, multiTagResult, null, List.of());
     }
 
     public boolean hasTargets() {

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

@@ -27,6 +27,7 @@ import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.pipe.impl.Calibrate3dPipeline;
 import org.photonvision.vision.pipeline.*;
 
 @SuppressWarnings({"rawtypes", "unused"})
@@ -41,7 +42,7 @@ public class PipelineManager {
     protected final DriverModePipeline driverModePipeline = new DriverModePipeline();
 
     /** Index of the currently active pipeline. Defaults to 0. */
-    private int currentPipelineIndex = 0;
+    private int currentPipelineIndex = DRIVERMODE_INDEX;
 
     /** The currently active pipeline. */
     private CVPipeline currentUserPipeline = driverModePipeline;
@@ -188,6 +189,11 @@ public class PipelineManager {
             return;
         }
 
+        // Cleanup potential old native resources before swapping over
+        if (currentUserPipeline != null) {
+            currentUserPipeline.release();
+        }
+
         currentPipelineIndex = newIndex;
         if (newIndex >= 0) {
             var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
@@ -212,6 +218,11 @@ public class PipelineManager {
                     logger.debug("Creating Aruco Pipeline");
                     currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings);
                     break;
+                case ObjectDetection:
+                    logger.debug("Creating ObjectDetection Pipeline");
+                    currentUserPipeline =
+                            new ObjectDetectionPipeline(
+                                    (ObjectDetectionPipelineSettings) desiredPipelineSettings);
                 default:
                     // Can be calib3d or drivermode, both of which are special cases
                     break;
@@ -313,6 +324,12 @@ public class PipelineManager {
                     added.pipelineNickname = nickname;
                     return added;
                 }
+            case ObjectDetection:
+                {
+                    var added = new ObjectDetectionPipelineSettings();
+                    added.pipelineNickname = nickname;
+                    return added;
+                }
             default:
                 {
                     logger.error("Got invalid pipeline type: " + type);

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -27,6 +27,7 @@ import org.opencv.core.Mat;
 import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
+import org.opencv.core.Rect2d;
 import org.opencv.core.RotatedRect;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.math.MathUtils;
@@ -38,6 +39,7 @@ import org.photonvision.vision.opencv.CVShape;
 import org.photonvision.vision.opencv.Contour;
 import org.photonvision.vision.opencv.DualOffsetValues;
 import org.photonvision.vision.opencv.Releasable;
+import org.photonvision.vision.pipe.impl.NeuralNetworkPipeResult;
 
 public class TrackedTarget implements Releasable {
     public final Contour m_mainContour;
@@ -65,6 +67,9 @@ public class TrackedTarget implements Releasable {
 
     private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
 
+    private int m_classId = -1;
+    private double m_confidence = -1;
+
     public TrackedTarget(
             PotentialTarget origTarget, TargetCalculationParameters params, CVShape shape) {
         this.m_mainContour = origTarget.m_mainContour;
@@ -154,6 +159,61 @@ public class TrackedTarget implements Releasable {
         m_robotOffsetPoint = new Point();
     }
 
+    public TrackedTarget(
+            Rect2d box, int class_id, double confidence, TargetCalculationParameters params) {
+        m_targetOffsetPoint = new Point(box.x + box.width / 2.0, box.y + box.height / 2.0);
+        m_robotOffsetPoint = new Point();
+
+        var yawPitch =
+                TargetCalculations.calculateYawPitch(
+                        params.cameraCenterPoint.x,
+                        box.x + box.width / 2.0,
+                        params.horizontalFocalLength,
+                        params.cameraCenterPoint.y,
+                        box.y + box.height / 2.0,
+                        params.verticalFocalLength);
+        m_yaw = yawPitch.getFirst();
+        m_pitch = yawPitch.getSecond();
+
+        Point[] cornerPoints =
+                new Point[] {
+                    // Box.x/y is the top-left corner, not the center
+                    new Point(box.x, box.y), // tl
+                    new Point(box.x + box.width, box.y), // tr
+                    new Point(box.x + box.width, box.y + box.height), // br
+                    new Point(box.x, box.y + box.height), // bl
+                };
+
+        m_targetCorners = List.of(cornerPoints);
+        MatOfPoint contourMat = new MatOfPoint(cornerPoints);
+        m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
+
+        m_mainContour = new Contour(contourMat);
+        m_area = m_mainContour.getArea() / params.imageArea * 100;
+
+        m_classId = class_id;
+        m_confidence = confidence;
+    }
+
+    public TrackedTarget(
+            NeuralNetworkPipeResult t, TargetCalculationParameters targetCalculationParameters) {
+        this(t.box, t.classIdx, t.confidence, targetCalculationParameters);
+    }
+
+    /**
+     * @return Returns the confidence of the detection ranging from 0 - 1.
+     */
+    public double getConfidence() {
+        return m_confidence;
+    }
+
+    /**
+     * @return O-indexed class index for the detected object.
+     */
+    public double getClassID() {
+        return m_classId;
+    }
+
     public TrackedTarget(
             ArucoDetectionResult result,
             AprilTagPoseEstimate tagPose,
@@ -388,6 +448,8 @@ public class TrackedTarget implements Releasable {
         ret.put("skew", getSkew());
         ret.put("area", getArea());
         ret.put("ambiguity", getPoseAmbiguity());
+        ret.put("confidence", m_confidence);
+        ret.put("classId", m_classId);
 
         var bestCameraToTarget3d = getBestCameraToTarget3d();
         if (bestCameraToTarget3d != null) {

photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java

@@ -44,6 +44,7 @@ import org.photonvision.vision.frame.FrameDivisor;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.CVMat;
+import org.photonvision.vision.pipe.impl.Calibrate3dPipeline;
 
 public class Calibrate3dPipeTest {
     @BeforeAll