mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
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.
This commit is contained in:
@@ -18,6 +18,7 @@ modifiableFileExclude {
|
||||
\.dll$
|
||||
\.webp$
|
||||
\.ico$
|
||||
\.rknn$
|
||||
gradlew
|
||||
}
|
||||
|
||||
|
||||
@@ -30,9 +30,11 @@ ext {
|
||||
joglVersion = "2.4.0-rc-20200307"
|
||||
javalinVersion = "5.6.2"
|
||||
photonGlDriverLibVersion = "dev-v2023.1.0-9-g75fc678"
|
||||
rknnVersion = "dev-v2024.0.0-30-g001b5ec"
|
||||
frcYear = "2024"
|
||||
mrcalVersion = "dev-v2024.0.0-7-gc976aaa";
|
||||
|
||||
|
||||
pubVersion = versionString
|
||||
isDev = pubVersion.startsWith("dev")
|
||||
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
<!DOCTYPE html>
|
||||
<!doctype html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8" />
|
||||
|
||||
8
photon-client/package-lock.json
generated
8
photon-client/package-lock.json
generated
@@ -31,7 +31,7 @@
|
||||
"eslint": "^8.56.0",
|
||||
"eslint-plugin-vue": "^9.19.2",
|
||||
"npm-run-all": "^4.1.5",
|
||||
"prettier": "^3.1.1",
|
||||
"prettier": "3.2.2",
|
||||
"sass": "~1.32",
|
||||
"sass-loader": "^13.3.2",
|
||||
"terser": "^5.14.2",
|
||||
@@ -3917,9 +3917,9 @@
|
||||
}
|
||||
},
|
||||
"node_modules/prettier": {
|
||||
"version": "3.1.1",
|
||||
"resolved": "https://registry.npmjs.org/prettier/-/prettier-3.1.1.tgz",
|
||||
"integrity": "sha512-22UbSzg8luF4UuZtzgiUOfcGM8s4tjBv6dJRT7j275NXsy2jb4aJa4NNveul5x4eqlF1wuhuR2RElK71RvmVaw==",
|
||||
"version": "3.2.2",
|
||||
"resolved": "https://registry.npmjs.org/prettier/-/prettier-3.2.2.tgz",
|
||||
"integrity": "sha512-HTByuKZzw7utPiDO523Tt2pLtEyK7OibUD9suEJQrPUCYQqrHr74GGX6VidMrovbf/I50mPqr8j/II6oBAuc5A==",
|
||||
"dev": true,
|
||||
"bin": {
|
||||
"prettier": "bin/prettier.cjs"
|
||||
|
||||
@@ -27,17 +27,17 @@
|
||||
},
|
||||
"devDependencies": {
|
||||
"@rushstack/eslint-patch": "^1.3.2",
|
||||
"@vue/eslint-config-prettier": "^9.0.0",
|
||||
"@vue/eslint-config-typescript": "^12.0.0",
|
||||
"prettier": "^3.1.1",
|
||||
"@types/node": "^16.11.45",
|
||||
"@types/three": "^0.160.0",
|
||||
"@vitejs/plugin-vue2": "^2.3.1",
|
||||
"@vue/eslint-config-prettier": "^9.0.0",
|
||||
"@vue/eslint-config-typescript": "^12.0.0",
|
||||
"@vue/tsconfig": "^0.5.1",
|
||||
"deepmerge": "^4.3.1",
|
||||
"eslint": "^8.56.0",
|
||||
"eslint-plugin-vue": "^9.19.2",
|
||||
"npm-run-all": "^4.1.5",
|
||||
"prettier": "3.2.2",
|
||||
"sass": "~1.32",
|
||||
"sass-loader": "^13.3.2",
|
||||
"terser": "^5.14.2",
|
||||
|
||||
@@ -7,6 +7,7 @@ import { computed, ref } from "vue";
|
||||
import PvIcon from "@/components/common/pv-icon.vue";
|
||||
import PvInput from "@/components/common/pv-input.vue";
|
||||
import { PipelineType } from "@/types/PipelineTypes";
|
||||
import { useSettingsStore } from "@/stores/settings/GeneralSettingsStore";
|
||||
|
||||
const changeCurrentCameraIndex = (index: number) => {
|
||||
useCameraSettingsStore().setCurrentCameraIndex(index, true);
|
||||
@@ -24,6 +25,8 @@ const changeCurrentCameraIndex = (index: number) => {
|
||||
case PipelineType.Aruco:
|
||||
pipelineType.value = WebsocketPipelineType.Aruco;
|
||||
break;
|
||||
case PipelineType.ObjectDetection:
|
||||
pipelineType.value = WebsocketPipelineType.ObjectDetection;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -154,6 +157,9 @@ const pipelineTypesWrapper = computed<{ name: string; value: number }[]>(() => {
|
||||
{ name: "AprilTag", value: WebsocketPipelineType.AprilTag },
|
||||
{ name: "Aruco", value: WebsocketPipelineType.Aruco }
|
||||
];
|
||||
if (useSettingsStore().general.rknnSupported) {
|
||||
pipelineTypes.push({ name: "Object Detection", value: WebsocketPipelineType.ObjectDetection });
|
||||
}
|
||||
|
||||
if (useCameraSettingsStore().isDriverMode) {
|
||||
pipelineTypes.push({ name: "Driver Mode", value: WebsocketPipelineType.DriverMode });
|
||||
@@ -208,6 +214,9 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
|
||||
case PipelineType.Aruco:
|
||||
pipelineType.value = WebsocketPipelineType.Aruco;
|
||||
break;
|
||||
case PipelineType.ObjectDetection:
|
||||
pipelineType.value = WebsocketPipelineType.ObjectDetection;
|
||||
break;
|
||||
}
|
||||
});
|
||||
</script>
|
||||
@@ -354,7 +363,8 @@ useCameraSettingsStore().$subscribe((mutation, state) => {
|
||||
{ name: 'Reflective', value: WebsocketPipelineType.Reflective },
|
||||
{ name: 'Colored Shape', value: WebsocketPipelineType.ColoredShape },
|
||||
{ name: 'AprilTag', value: WebsocketPipelineType.AprilTag },
|
||||
{ name: 'Aruco', value: WebsocketPipelineType.Aruco }
|
||||
{ name: 'Aruco', value: WebsocketPipelineType.Aruco },
|
||||
{ name: 'Object Detection', value: WebsocketPipelineType.ObjectDetection }
|
||||
]"
|
||||
/>
|
||||
</v-card-text>
|
||||
|
||||
@@ -8,6 +8,7 @@ import ThresholdTab from "@/components/dashboard/tabs/ThresholdTab.vue";
|
||||
import ContoursTab from "@/components/dashboard/tabs/ContoursTab.vue";
|
||||
import AprilTagTab from "@/components/dashboard/tabs/AprilTagTab.vue";
|
||||
import ArucoTab from "@/components/dashboard/tabs/ArucoTab.vue";
|
||||
import ObjectDetectionTab from "@/components/dashboard/tabs/ObjectDetectionTab.vue";
|
||||
import OutputTab from "@/components/dashboard/tabs/OutputTab.vue";
|
||||
import TargetsTab from "@/components/dashboard/tabs/TargetsTab.vue";
|
||||
import PnPTab from "@/components/dashboard/tabs/PnPTab.vue";
|
||||
@@ -40,6 +41,10 @@ const allTabs = Object.freeze({
|
||||
tabName: "Aruco",
|
||||
component: ArucoTab
|
||||
},
|
||||
objectDetectionTab: {
|
||||
tabName: "Object Detection",
|
||||
component: ObjectDetectionTab
|
||||
},
|
||||
outputTab: {
|
||||
tabName: "Output",
|
||||
component: OutputTab
|
||||
@@ -75,6 +80,7 @@ const getTabGroups = (): ConfigOption[][] => {
|
||||
allTabs.contoursTab,
|
||||
allTabs.apriltagTab,
|
||||
allTabs.arucoTab,
|
||||
allTabs.objectDetectionTab,
|
||||
allTabs.outputTab
|
||||
],
|
||||
[allTabs.targetsTab, allTabs.pnpTab, allTabs.map3dTab]
|
||||
@@ -82,14 +88,21 @@ const getTabGroups = (): ConfigOption[][] => {
|
||||
} else if (lgAndDown) {
|
||||
return [
|
||||
[allTabs.inputTab],
|
||||
[allTabs.thresholdTab, allTabs.contoursTab, allTabs.apriltagTab, allTabs.arucoTab, allTabs.outputTab],
|
||||
[
|
||||
allTabs.thresholdTab,
|
||||
allTabs.contoursTab,
|
||||
allTabs.apriltagTab,
|
||||
allTabs.arucoTab,
|
||||
allTabs.objectDetectionTab,
|
||||
allTabs.outputTab
|
||||
],
|
||||
[allTabs.targetsTab, allTabs.pnpTab, allTabs.map3dTab]
|
||||
];
|
||||
} else if (xl) {
|
||||
return [
|
||||
[allTabs.inputTab],
|
||||
[allTabs.thresholdTab],
|
||||
[allTabs.contoursTab, allTabs.apriltagTab, allTabs.arucoTab, allTabs.outputTab],
|
||||
[allTabs.contoursTab, allTabs.apriltagTab, allTabs.arucoTab, allTabs.objectDetectionTab, allTabs.outputTab],
|
||||
[allTabs.targetsTab, allTabs.pnpTab, allTabs.map3dTab]
|
||||
];
|
||||
}
|
||||
@@ -103,17 +116,20 @@ const tabGroups = computed<ConfigOption[][]>(() => {
|
||||
const allow3d = useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled;
|
||||
const isAprilTag = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.AprilTag;
|
||||
const isAruco = useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.Aruco;
|
||||
const isObjectDetection =
|
||||
useCameraSettingsStore().currentWebsocketPipelineType === WebsocketPipelineType.ObjectDetection;
|
||||
|
||||
return getTabGroups()
|
||||
.map((tabGroup) =>
|
||||
tabGroup.filter(
|
||||
(tabConfig) =>
|
||||
!(!allow3d && tabConfig.tabName === "3D") && //Filter out 3D tab any time 3D isn't calibrated
|
||||
!((!allow3d || isAprilTag || isAruco) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags
|
||||
!((isAprilTag || isAruco) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags
|
||||
!((isAprilTag || isAruco) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags
|
||||
!((!allow3d || isAprilTag || isAruco || isObjectDetection) && tabConfig.tabName === "PnP") && //Filter out the PnP config tab if 3D isn't available, or we're doing AprilTags
|
||||
!((isAprilTag || isAruco || isObjectDetection) && tabConfig.tabName === "Threshold") && //Filter out threshold tab if we're doing AprilTags
|
||||
!((isAprilTag || isAruco || isObjectDetection) && tabConfig.tabName === "Contours") && //Filter out contours if we're doing AprilTags
|
||||
!(!isAprilTag && tabConfig.tabName === "AprilTag") && //Filter out apriltag unless we actually are doing AprilTags
|
||||
!(!isAruco && tabConfig.tabName === "Aruco") //Filter out aruco unless we actually are doing Aruco
|
||||
!(!isAruco && tabConfig.tabName === "Aruco") &&
|
||||
!(!isObjectDetection && tabConfig.tabName === "Object Detection") //Filter out aruco unless we actually are doing Aruco
|
||||
)
|
||||
)
|
||||
.filter((it) => it.length); // Remove empty tab groups
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
<script setup lang="ts">
|
||||
import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
|
||||
import { PipelineType } from "@/types/PipelineTypes";
|
||||
import PvSlider from "@/components/common/pv-slider.vue";
|
||||
import { computed, getCurrentInstance } from "vue";
|
||||
import { useStateStore } from "@/stores/StateStore";
|
||||
|
||||
// TODO fix pipeline typing in order to fix this, the store settings call should be able to infer that only valid pipeline type settings are exposed based on pre-checks for the entire config section
|
||||
// Defer reference to store access method
|
||||
const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings;
|
||||
|
||||
const interactiveCols = computed(
|
||||
() =>
|
||||
(getCurrentInstance()?.proxy.$vuetify.breakpoint.mdAndDown || false) &&
|
||||
(!useStateStore().sidebarFolded || useCameraSettingsStore().isDriverMode)
|
||||
)
|
||||
? 9
|
||||
: 8;
|
||||
</script>
|
||||
|
||||
<template>
|
||||
<div v-if="currentPipelineSettings.pipelineType === PipelineType.ObjectDetection">
|
||||
<pv-slider
|
||||
v-model="currentPipelineSettings.confidence"
|
||||
class="pt-2"
|
||||
:slider-cols="interactiveCols"
|
||||
label="Confidence"
|
||||
tooltip="The minimum confidence for a detection to be considered valid. Bigger numbers mean fewer but more probable detections are allowed through."
|
||||
:min="0"
|
||||
:max="1"
|
||||
:step="0.01"
|
||||
@input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ confidence: value }, false)"
|
||||
/>
|
||||
</div>
|
||||
</template>
|
||||
@@ -48,6 +48,10 @@ const resetCurrentBuffer = () => {
|
||||
>
|
||||
Fiducial ID
|
||||
</th>
|
||||
<template v-if="currentPipelineSettings.pipelineType === PipelineType.ObjectDetection">
|
||||
<th class="text-center white--text">Class</th>
|
||||
<th class="text-center white--text">Confidence</th>
|
||||
</template>
|
||||
<template v-if="!useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled">
|
||||
<th class="text-center white--text">Pitch θ°</th>
|
||||
<th class="text-center white--text">Yaw θ°</th>
|
||||
@@ -85,6 +89,18 @@ const resetCurrentBuffer = () => {
|
||||
>
|
||||
{{ target.fiducialId }}
|
||||
</td>
|
||||
<td
|
||||
v-if="currentPipelineSettings.pipelineType === PipelineType.ObjectDetection"
|
||||
class="text-center white--text"
|
||||
>
|
||||
{{ useStateStore().currentPipelineResults?.classNames[target.classId] }}
|
||||
</td>
|
||||
<td
|
||||
v-if="currentPipelineSettings.pipelineType === PipelineType.ObjectDetection"
|
||||
class="text-center white--text"
|
||||
>
|
||||
{{ target.confidence.toFixed(2) }}
|
||||
</td>
|
||||
<template v-if="!useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled">
|
||||
<td class="text-center">{{ target.pitch.toFixed(2) }}°</td>
|
||||
<td class="text-center">{{ target.yaw.toFixed(2) }}°</td>
|
||||
|
||||
@@ -27,7 +27,8 @@ export const useSettingsStore = defineStore("settings", {
|
||||
gpuAcceleration: undefined,
|
||||
hardwareModel: undefined,
|
||||
hardwarePlatform: undefined,
|
||||
mrCalWorking: true
|
||||
mrCalWorking: true,
|
||||
rknnSupported: false
|
||||
},
|
||||
network: {
|
||||
ntServerAddress: "",
|
||||
@@ -99,7 +100,8 @@ export const useSettingsStore = defineStore("settings", {
|
||||
hardwareModel: data.general.hardwareModel || undefined,
|
||||
hardwarePlatform: data.general.hardwarePlatform || undefined,
|
||||
gpuAcceleration: data.general.gpuAcceleration || undefined,
|
||||
mrCalWorking: data.general.mrCalWorking
|
||||
mrCalWorking: data.general.mrCalWorking,
|
||||
rknnSupported: data.general.rknnSupported
|
||||
};
|
||||
this.lighting = data.lighting;
|
||||
this.network = data.networkSettings;
|
||||
|
||||
@@ -54,6 +54,8 @@ export interface PhotonTarget {
|
||||
ambiguity: number;
|
||||
// -1 if not set
|
||||
fiducialId: number;
|
||||
confidence: number;
|
||||
classId: number;
|
||||
// undefined if 3d isn't enabled
|
||||
pose?: Transform3d;
|
||||
}
|
||||
@@ -70,4 +72,6 @@ export interface PipelineResult {
|
||||
targets: PhotonTarget[];
|
||||
// undefined if multitag failed or non-tag pipeline
|
||||
multitagResult?: MultitagResult;
|
||||
// Object detection class names -- empty if not doing object detection
|
||||
classNames: string[];
|
||||
}
|
||||
|
||||
@@ -5,7 +5,8 @@ export enum PipelineType {
|
||||
Reflective = 2,
|
||||
ColoredShape = 3,
|
||||
AprilTag = 4,
|
||||
Aruco = 5
|
||||
Aruco = 5,
|
||||
ObjectDetection = 6
|
||||
}
|
||||
|
||||
export enum AprilTagFamily {
|
||||
@@ -281,14 +282,39 @@ export const DefaultArucoPipelineSettings: ArucoPipelineSettings = {
|
||||
doSingleTargetAlways: false
|
||||
};
|
||||
|
||||
export interface ObjectDetectionPipelineSettings extends PipelineSettings {
|
||||
pipelineType: PipelineType.ObjectDetection;
|
||||
confidence: number;
|
||||
nms: number;
|
||||
box_thresh: number;
|
||||
}
|
||||
export type ConfigurableObjectDetectionPipelineSettings = Partial<
|
||||
Omit<ObjectDetectionPipelineSettings, "pipelineType">
|
||||
> &
|
||||
ConfigurablePipelineSettings;
|
||||
export const DefaultObjectDetectionPipelineSettings: ObjectDetectionPipelineSettings = {
|
||||
...DefaultPipelineSettings,
|
||||
pipelineType: PipelineType.ObjectDetection,
|
||||
cameraGain: 20,
|
||||
targetModel: TargetModel.InfiniteRechargeHighGoalOuter,
|
||||
ledMode: true,
|
||||
outputShowMultipleTargets: false,
|
||||
cameraExposure: 6,
|
||||
confidence: 0.9,
|
||||
nms: 0.45,
|
||||
box_thresh: 0.25
|
||||
};
|
||||
|
||||
export type ActivePipelineSettings =
|
||||
| ReflectivePipelineSettings
|
||||
| ColoredShapePipelineSettings
|
||||
| AprilTagPipelineSettings
|
||||
| ArucoPipelineSettings;
|
||||
| ArucoPipelineSettings
|
||||
| ObjectDetectionPipelineSettings;
|
||||
|
||||
export type ActiveConfigurablePipelineSettings =
|
||||
| ConfigurableReflectivePipelineSettings
|
||||
| ConfigurableColoredShapePipelineSettings
|
||||
| ConfigurableAprilTagPipelineSettings
|
||||
| ConfigurableArucoPipelineSettings;
|
||||
| ConfigurableArucoPipelineSettings
|
||||
| ConfigurableObjectDetectionPipelineSettings;
|
||||
|
||||
@@ -7,6 +7,7 @@ export interface GeneralSettings {
|
||||
hardwareModel?: string;
|
||||
hardwarePlatform?: string;
|
||||
mrCalWorking: boolean;
|
||||
rknnSupported: boolean;
|
||||
}
|
||||
|
||||
export interface MetricData {
|
||||
|
||||
@@ -101,5 +101,6 @@ export enum WebsocketPipelineType {
|
||||
Reflective = 0,
|
||||
ColoredShape = 1,
|
||||
AprilTag = 2,
|
||||
Aruco = 3
|
||||
Aruco = 3,
|
||||
ObjectDetection = 4
|
||||
}
|
||||
|
||||
@@ -10,25 +10,22 @@ export default defineConfig({
|
||||
plugins: [
|
||||
Vue2(),
|
||||
Components({
|
||||
resolvers: [
|
||||
VuetifyResolver()
|
||||
],
|
||||
resolvers: [VuetifyResolver()],
|
||||
dts: true,
|
||||
transformer: "vue2",
|
||||
types: [{
|
||||
from: "vue-router",
|
||||
names: ["RouterLink", "RouterView"]
|
||||
}],
|
||||
types: [
|
||||
{
|
||||
from: "vue-router",
|
||||
names: ["RouterLink", "RouterView"]
|
||||
}
|
||||
],
|
||||
version: 2.7
|
||||
})
|
||||
],
|
||||
css: {
|
||||
preprocessorOptions: {
|
||||
sass: {
|
||||
additionalData: [
|
||||
"@import \"@/assets/styles/variables.scss\"",
|
||||
""
|
||||
].join("\n")
|
||||
additionalData: ['@import "@/assets/styles/variables.scss"', ""].join("\n")
|
||||
}
|
||||
}
|
||||
},
|
||||
|
||||
@@ -37,7 +37,9 @@ dependencies {
|
||||
implementation 'org.zeroturnaround:zt-zip:1.14'
|
||||
|
||||
implementation "org.xerial:sqlite-jdbc:3.41.0.0"
|
||||
|
||||
def rknnjniversion = "dev-v2024.0.0-44-g8022c40"
|
||||
implementation "org.photonvision:rknn_jni-jni:$rknnjniversion:linuxarm64"
|
||||
implementation "org.photonvision:rknn_jni-java:$rknnjniversion"
|
||||
implementation "org.photonvision:photon-libcamera-gl-driver-jni:$photonGlDriverLibVersion:linuxarm64"
|
||||
implementation "org.photonvision:photon-libcamera-gl-driver-java:$photonGlDriverLibVersion"
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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");
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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());
|
||||
// }
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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() {}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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() {
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -27,6 +27,7 @@ import java.util.stream.Collectors;
|
||||
import org.apache.commons.cli.*;
|
||||
import org.photonvision.common.configuration.CameraConfiguration;
|
||||
import org.photonvision.common.configuration.ConfigManager;
|
||||
import org.photonvision.common.configuration.NeuralNetworkModelManager;
|
||||
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
|
||||
import org.photonvision.common.hardware.HardwareManager;
|
||||
import org.photonvision.common.hardware.PiVersion;
|
||||
@@ -37,6 +38,7 @@ import org.photonvision.common.logging.Logger;
|
||||
import org.photonvision.common.networking.NetworkManager;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.util.numbers.IntegerCouple;
|
||||
import org.photonvision.jni.RknnDetectorJNI;
|
||||
import org.photonvision.mrcal.MrCalJNILoader;
|
||||
import org.photonvision.raspi.LibCameraJNILoader;
|
||||
import org.photonvision.server.Server;
|
||||
@@ -348,7 +350,15 @@ public class Main {
|
||||
} catch (IOException e) {
|
||||
logger.error("Failed to load libcamera-JNI!", e);
|
||||
}
|
||||
|
||||
try {
|
||||
if (Platform.isRK3588()) {
|
||||
RknnDetectorJNI.forceLoad();
|
||||
} else {
|
||||
logger.error("Platform does not support RKNN based machine learning!");
|
||||
}
|
||||
} catch (IOException e) {
|
||||
logger.error("Failed to load rknn-JNI!", e);
|
||||
}
|
||||
try {
|
||||
MrCalJNILoader.forceLoad();
|
||||
} catch (IOException e) {
|
||||
@@ -364,7 +374,6 @@ public class Main {
|
||||
} catch (ParseException e) {
|
||||
logger.error("Failed to parse command-line options!", e);
|
||||
}
|
||||
|
||||
CVMat.enablePrint(false);
|
||||
PipelineProfiler.enablePrint(false);
|
||||
|
||||
@@ -399,6 +408,10 @@ public class Main {
|
||||
NetworkTablesManager.getInstance()
|
||||
.setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig());
|
||||
|
||||
logger.info("Loading ML models");
|
||||
NeuralNetworkModelManager.getInstance()
|
||||
.initialize(ConfigManager.getInstance().getModelsDirectory());
|
||||
|
||||
if (!isTestMode) {
|
||||
logger.debug("Loading VisionSourceManager...");
|
||||
VisionSourceManager.getInstance()
|
||||
|
||||
1
photon-server/src/main/resources/models/labels.txt
Normal file
1
photon-server/src/main/resources/models/labels.txt
Normal file
@@ -0,0 +1 @@
|
||||
note
|
||||
Binary file not shown.
Reference in New Issue
Block a user